static void
initialize_transformations(void)
{
tf::Time::init();
tf::Transform velodyne_position_on_board;
velodyne_position_on_board.setOrigin(carmen_vector3_to_tf_vector3(velodyne_pose.position));
velodyne_position_on_board.setRotation(carmen_rotation_to_tf_quaternion(velodyne_pose.orientation));
tf::StampedTransform board_to_velodyne_transform(velodyne_position_on_board, tf::Time(0), board_tf_name, velodyne_tf_name);
transformer.setTransform(board_to_velodyne_transform, "board_to_velodyne_transform");
tf::Transform board_position_on_car;
board_position_on_car.setOrigin(carmen_vector3_to_tf_vector3(sensor_board_pose.position));
board_position_on_car.setRotation(carmen_rotation_to_tf_quaternion(sensor_board_pose.orientation));
tf::StampedTransform car_to_board_transform(board_position_on_car, tf::Time(0), car_tf_name, board_tf_name);
transformer.setTransform(car_to_board_transform, "car_to_board_transform");
}
static void
initialize_transformations(xsens_xyz_handler *xsens_handler)
{
tf::Time::init();
tf::Transform xsens_position_on_board;
xsens_position_on_board.setOrigin(carmen_vector3_to_tf_vector3(xsens_handler->xsens_pose.position));
xsens_position_on_board.setRotation(carmen_rotation_to_tf_quaternion(xsens_handler->xsens_pose.orientation));
tf::StampedTransform board_to_xsens_transform(xsens_position_on_board, tf::Time(0), board_tf_name, xsens_tf_name);
transformer.setTransform(board_to_xsens_transform, "board_to_xsens_transform");
tf::Transform gps_position_on_board;
gps_position_on_board.setOrigin(carmen_vector3_to_tf_vector3(xsens_handler->gps_pose_in_the_car.position));
gps_position_on_board.setRotation(carmen_rotation_to_tf_quaternion(xsens_handler->gps_pose_in_the_car.orientation));
tf::StampedTransform board_to_gps_transform(gps_position_on_board, tf::Time(0), board_tf_name, gps_tf_name);
transformer.setTransform(board_to_gps_transform, "board_to_gps_transform");
tf::Transform board_position_on_car;
board_position_on_car.setOrigin(carmen_vector3_to_tf_vector3(xsens_handler->sensor_board_pose.position));
board_position_on_car.setRotation(carmen_rotation_to_tf_quaternion(xsens_handler->sensor_board_pose.orientation));
tf::StampedTransform car_to_board_transform(board_position_on_car, tf::Time(0), car_tf_name, board_tf_name);
transformer.setTransform(car_to_board_transform, "car_to_board_transform");
}
static carmen_orientation_3D_t
get_car_ang_velocity_from_message(carmen_xsens_global_quat_message *xsens_mti_message)
{
carmen_orientation_3D_t gyr_orientation;
gyr_orientation.roll = xsens_mti_message->m_gyr.x;
gyr_orientation.pitch = xsens_mti_message->m_gyr.y;
gyr_orientation.yaw = xsens_mti_message->m_gyr.z;
tf::Quaternion tf_quat_xsens = carmen_rotation_to_tf_quaternion(gyr_orientation);
carmen_orientation_3D_t ang_velocity = get_orientation_car_reference_from_message(tf_quat_xsens);
return ang_velocity;
}