Ejemplos de carmen_fused_odometry_initialize en C++ (Cpp)

Ejemplo n.º 1

Archivo: xsens_xyz_handler.cpp Proyecto: LCAD-UFES/carmen_lcad

static void 
xsens_mti_message_handler(carmen_xsens_global_quat_message *xsens_mti)
{
	if (!xsens_handler.initial_state_initialized)
	{
		initialize_states(xsens_mti);
		return;
	}

	// Must check if timestamp has changed due to log file jump on log playback

	if (reinitialized_gps)//if (check_time_difference(xsens_mti->timestamp))
	{
		carmen_fused_odometry_initialize(fused_odometry_parameters);
		initialize_states(xsens_mti);
		reinitialized_gps = 0;

		return;
	}

	xsens_sensor_vector_index = (xsens_sensor_vector_index + 1) % XSENS_SENSOR_VECTOR_SIZE;
	if (xsens_sensor_vector[xsens_sensor_vector_index] != NULL)
		destroy_sensor_vector_xsens_xyz(xsens_sensor_vector[xsens_sensor_vector_index]);

	sensor_vector_xsens_xyz *sensor_vector = create_sensor_vector_xsens_mti(xsens_mti, &xsens_handler.gps_xyz);
	xsens_sensor_vector[xsens_sensor_vector_index] = sensor_vector;
	
	set_best_yaw_estimate(xsens_sensor_vector, xsens_sensor_vector_index);

	xsens_handler.orientation = sensor_vector->orientation; 	// Used by create_car_odometry_control()
	xsens_handler.ang_velocity = sensor_vector->ang_velocity; 	// Used by create_car_odometry_control()

	if (!kalman_filter)
	{
		if (xsens_handler.gps_performance_changed)
		{
			xsens_handler.gps_performance_degradation = 50.0;
			xsens_handler.gps_performance_changed = 0;
		}
		if (xsens_handler.gps_orientation_performance_changed)
		{
			xsens_handler.gps_orientation_performance_degradation = 40.0;
			xsens_handler.gps_orientation_performance_changed = 0;
		}
		prediction(sensor_vector->timestamp, fused_odometry_parameters);
		correction(measure_weight_orientation, sensor_vector, fused_odometry_parameters);
		publish_fused_odometry();
		xsens_handler.gps_performance_degradation *= 0.98; // fator de decaimento
		xsens_handler.gps_orientation_performance_degradation *= 0.98; // fator de decaimento
	}
	xsens_handler.last_xsens_message_timestamp = xsens_mti->timestamp;
}

Ejemplo n.º 2

Archivo: fused_odometry_main.cpp Proyecto: ulyssesrr/carmen_lcad

int 
main(int argc, char** argv)
{ 	
	argc_g = argc;
	argv_g = argv;
	carmen_fused_odometry_parameters fused_odometry_parameters;

	carmen_ipc_initialize(argc, argv);
	carmen_param_check_version(argv[0]);

	initialize_carmen_parameters(argc, argv, &fused_odometry_parameters);

	register_ipc_messages();

	carmen_fused_odometry_initialize(&fused_odometry_parameters);

	carmen_ipc_dispatch();

	return 0;
}

Ejemplo n.º 3

Archivo: fused_odometry_main.cpp Proyecto: LCAD-UFES/carmen_lcad

int 
main(int argc, char** argv)
{ 	
	argc_g = argc;
	argv_g = argv;
	carmen_fused_odometry_parameters fused_odometry_parameters;

	carmen_ipc_initialize(argc, argv);
	carmen_param_check_version(argv[0]);

	initialize_carmen_parameters(argc, argv, &fused_odometry_parameters);

	register_ipc_messages();

	carmen_fused_odometry_initialize(&fused_odometry_parameters);

	// carmen_ipc_addPeriodicTimer(1.0 / fused_odometry_frequency, (TIMER_HANDLER_TYPE) correct_fused_odometry_timming_and_publish, NULL);

	carmen_ipc_dispatch();
	// fused_odometry_main_loop();

	return 0;
}