void
localize_ackerman_handler(carmen_localize_ackerman_globalpos_message *localize_ackerman_message)
{

	car_fused_pose = localize_ackerman_message->pose;
	car_fused_pose.position = sub_vectors(car_fused_pose.position, get_position_offset());
	car_fused_pose.orientation.yaw = localize_ackerman_message->globalpos.theta;

	car_fused_velocity = localize_ackerman_message->velocity;
	car_fused_time = localize_ackerman_message->timestamp;
	car_phi = localize_ackerman_message->phi;

	car_fused_pose.position.z        = 0.0;
	car_fused_pose.orientation.pitch = 0.0;
	car_fused_pose.orientation.roll  = 0.0;

	if (first_offline_map_message_flag)
	{
		car_global_pose = car_fused_pose;
		first_offline_map_message_flag = -1;
	}

	localize_initialized = 1;
}
Esempio n. 2
0
void hexbright::absolute_vector(int* out_vector, int* in_vector) {
  sub_vectors(out_vector, in_vector, down_vector);
}