Apollo 10.0
自动驾驶开放平台
heading_msg_fusion.cc
浏览该文件的文档.
1/******************************************************************************
2 * Copyright 2023 The Apollo Authors. All Rights Reserved.
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 *****************************************************************************/
16
19
20namespace apollo {
21namespace cyber {
22
26#ifdef ENABLE_ROS_MSG
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);
31
32 auto heading_msg = std::get<0>(out.values);
33
34 auto unix_msg_time =
35 ros_nav.header.stamp.sec + ros_nav.header.stamp.nanosec / 1e9;
36
37 double quaternion[4];
38 double euler_angles[3];
39
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;
44
45 QuaternionToEuler(quaternion, euler_angles);
46 auto pitch = euler_angles[1];
47 auto heading = QuaternionToHeading(euler_angles[2]);
48
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);
52
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);
65 }
66 heading_msg->set_heading(heading);
67 heading_msg->set_pitch(pitch);
68
69 /*
70 pose covariance:
71 [
72 xx, xy, xz, xroll, xpitch, xyaw,
73 yx, yy, yz, yroll, ypitch, yyaw,
74 zx, zy, zz, zroll, zpitch, zzaw,
75 rollx, rolly, rollz, rollroll, rollpitch, rollyaw,
76 pitchx, pitchy, pitchz, pitchroll, pitchpitch, pitchyaw,
77 yawx, yawy, yawz, yawroll, yawpitch, yawyaw
78 ]
79 thus pitch_stddev = sqrt(covariance[28]), yaw_stddev = sqrt(covariance[35])
80 */
81
82 auto covariance = ros_odometry.pose.covariance;
83 auto pitch_variance = covariance[28];
84 auto yaw_variance = covariance[35];
85
86 double pitch_stddev = sqrt(pitch_variance);
87 double yaw_stddev = sqrt(yaw_variance);
88
89 heading_msg->set_heading_std_dev(yaw_stddev);
90 heading_msg->set_pitch_std_dev(pitch_stddev);
91
92#endif
93 return true;
94}
95
96} // namespace cyber
97} // namespace apollo
virtual bool ConvertMsg(InputTypes< RosNavMsgPtr, RosOdometryMsgPtr > &, OutputTypes< OutputMsgPtr > &)
convert the message between ros and apollo
double QuaternionToHeading(double yaw)
void QuaternionToEuler(const double quaternion[4], double att[3])
class register implement
Definition arena_queue.h:37
std::tuple< Types... > values
std::tuple< Types... > values