static void print_transform(tf::Transform transform) { carmen_orientation_3D_t orientation = get_carmen_orientation_from_tf_transform(transform); printf("y:% lf p:% lf r:% lf\n", orientation.yaw, orientation.pitch, orientation.roll); printf("x:% lf y:% lf z:% lf\n", transform.getOrigin().x(), transform.getOrigin().y(), transform.getOrigin().z()); printf("\n"); }
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_orientation_3D_t get_orientation_car_reference_from_message(tf::Quaternion xsens_reading) { tf::StampedTransform xsens_to_car; transformer.lookupTransform(xsens_tf_name, car_tf_name, tf::Time(0), xsens_to_car); tf::Transform xsens_matrix; xsens_matrix.setOrigin(tf::Vector3(0.0, 0.0, 0.0)); xsens_matrix.setRotation(xsens_reading); tf::Transform car_pose = xsens_matrix * xsens_to_car; carmen_orientation_3D_t orientation = get_carmen_orientation_from_tf_transform(car_pose); return orientation; }