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_vector_3D_t
get_car_position_from_message(carmen_xsens_xyz_message *xsens_xyz_message)
{
tf::StampedTransform xsens_to_car;
transformer.lookupTransform(xsens_tf_name, car_tf_name, tf::Time(0), xsens_to_car);
tf::Transform global_to_xsens;
global_to_xsens.setOrigin(carmen_vector3_to_tf_vector3(xsens_xyz_message->position));
global_to_xsens.setRotation(carmen_quaternion_to_tf_quaternion(xsens_xyz_message->quat));
tf::Transform global_to_car = global_to_xsens * xsens_to_car;
carmen_vector_3D_t car_position = tf_vector3_to_carmen_vector3(global_to_car.getOrigin());
return car_position;
}
static carmen_vector_3D_t
get_car_position_from_message(carmen_gps_xyz_message *gps_xyz_message)
{
tf::StampedTransform gps_to_car;
transformer.lookupTransform(gps_tf_name, car_tf_name, tf::Time(0), gps_to_car);
carmen_vector_3D_t gps_position;
gps_position.x = gps_xyz_message->x;
gps_position.y = gps_xyz_message->y;
gps_position.z = gps_xyz_message->z;
tf::Transform global_to_gps;
global_to_gps.setOrigin(carmen_vector3_to_tf_vector3(gps_position));
tf::Quaternion zero_quat(0.0, 0.0, 0.0);
global_to_gps.setRotation(zero_quat);
tf::Transform global_to_car = global_to_gps * gps_to_car;
carmen_vector_3D_t car_position = tf_vector3_to_carmen_vector3(global_to_car.getOrigin());
return car_position;
}
