24 {
25#ifdef ENABLE_ROS_MSG
26 auto ros_odometry_ptr = std::get<0>(in.values);
27 auto& ros_odometry = (*ros_odometry_ptr);
28 auto localization_estimate = std::get<0>(out.values);
29 auto tf = std::get<1>(out.values);
30
31 auto unix_msg_time =
32 ros_odometry.header.stamp.sec + ros_odometry.header.stamp.nanosec / 1e9;
33 localization_estimate->mutable_header()->set_timestamp_sec(unix_msg_time);
34 localization_estimate->mutable_header()->set_module_name("localization");
35 localization_estimate->set_measurement_time(unix_msg_time);
36 localization_estimate->mutable_pose()->mutable_position()->set_x(
37 ros_odometry.pose.pose.position.x);
38 localization_estimate->mutable_pose()->mutable_position()->set_y(
39 ros_odometry.pose.pose.position.y);
40 localization_estimate->mutable_pose()->mutable_position()->set_z(
41 ros_odometry.pose.pose.position.z);
42 localization_estimate->mutable_pose()->mutable_linear_velocity()->set_x(
43 ros_odometry.twist.twist.linear.x);
44 localization_estimate->mutable_pose()->mutable_linear_velocity()->set_y(
45 ros_odometry.twist.twist.linear.y);
46 localization_estimate->mutable_pose()->mutable_linear_velocity()->set_z(
47 ros_odometry.twist.twist.linear.z);
48 localization_estimate->mutable_pose()->mutable_angular_velocity()->set_x(
49 ros_odometry.twist.twist.angular.x);
50 localization_estimate->mutable_pose()->mutable_angular_velocity()->set_y(
51 ros_odometry.twist.twist.angular.y);
52 localization_estimate->mutable_pose()->mutable_angular_velocity()->set_z(
53 ros_odometry.twist.twist.angular.z);
54 localization_estimate->mutable_pose()->mutable_orientation()->set_qx(
55 ros_odometry.pose.pose.orientation.x);
56 localization_estimate->mutable_pose()->mutable_orientation()->set_qy(
57 ros_odometry.pose.pose.orientation.y);
58 localization_estimate->mutable_pose()->mutable_orientation()->set_qz(
59 ros_odometry.pose.pose.orientation.z);
60 localization_estimate->mutable_pose()->mutable_orientation()->set_qw(
61 ros_odometry.pose.pose.orientation.w);
62
63 auto single_tf = tf->add_transforms();
64 single_tf->mutable_header()->set_timestamp_sec(unix_msg_time);
65 single_tf->mutable_header()->set_frame_id("world");
66 single_tf->set_child_frame_id("localization");
67 single_tf->mutable_transform()->mutable_translation()->set_x(
68 ros_odometry.pose.pose.position.x);
69 single_tf->mutable_transform()->mutable_translation()->set_y(
70 ros_odometry.pose.pose.position.y);
71 single_tf->mutable_transform()->mutable_translation()->set_z(
72 ros_odometry.pose.pose.position.z);
73 single_tf->mutable_transform()->mutable_rotation()->set_qx(
74 ros_odometry.pose.pose.orientation.x);
75 single_tf->mutable_transform()->mutable_rotation()->set_qy(
76 ros_odometry.pose.pose.orientation.y);
77 single_tf->mutable_transform()->mutable_rotation()->set_qz(
78 ros_odometry.pose.pose.orientation.z);
79 single_tf->mutable_transform()->mutable_rotation()->set_qw(
80 ros_odometry.pose.pose.orientation.w);
81#endif
82 return true;
83}