27 auto ros_nav_ptr = std::get<0>(in.
values);
28 auto ros_odometry_ptr = std::get<1>(in.
values);
29 auto& ros_nav = (*ros_nav_ptr);
30 auto& ros_odometry = (*ros_odometry_ptr);
32 auto heading_msg = std::get<0>(out.
values);
35 ros_nav.header.stamp.sec + ros_nav.header.stamp.nanosec / 1e9;
38 double euler_angles[3];
40 quaternion[0] = ros_odometry.pose.pose.orientation.x;
41 quaternion[1] = ros_odometry.pose.pose.orientation.y;
42 quaternion[2] = ros_odometry.pose.pose.orientation.z;
43 quaternion[3] = ros_odometry.pose.pose.orientation.w;
46 auto pitch = euler_angles[1];
49 heading_msg->mutable_header()->set_timestamp_sec(unix_msg_time);
50 heading_msg->mutable_header()->set_module_name(
"gnss");
51 heading_msg->set_measurement_time(unix_msg_time);
53 if (ros_nav.status.status == -1) {
54 heading_msg->set_solution_status(19);
55 heading_msg->set_position_type(0);
56 }
else if (ros_nav.status.status == 0) {
57 heading_msg->set_solution_status(0);
58 heading_msg->set_position_type(16);
59 }
else if (ros_nav.status.status == 1) {
60 heading_msg->set_solution_status(0);
61 heading_msg->set_position_type(18);
62 }
else if (ros_nav.status.status == 2) {
63 heading_msg->set_solution_status(0);
64 heading_msg->set_position_type(50);
66 heading_msg->set_heading(heading);
67 heading_msg->set_pitch(pitch);
82 auto covariance = ros_odometry.pose.covariance;
83 auto pitch_variance = covariance[28];
84 auto yaw_variance = covariance[35];
86 double pitch_stddev = sqrt(pitch_variance);
87 double yaw_stddev = sqrt(yaw_variance);
89 heading_msg->set_heading_std_dev(yaw_stddev);
90 heading_msg->set_pitch_std_dev(pitch_stddev);