static carmen_pose_3D_t
tf_transform_to_carmen_pose_3D(tf::Transform transform)
{
carmen_pose_3D_t pose;
pose.position = tf_vector3_to_carmen_vector3(transform.getOrigin());
pose.orientation = get_carmen_orientation_from_tf_transform(transform);
return pose;
}
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;
}
