Apollo 10.0
自动驾驶开放平台
Public 成员函数 | 所有成员列表
apollo::cyber::HeadingMsgFusion类 参考

#include <heading_msg_fusion.h>

类 apollo::cyber::HeadingMsgFusion 继承关系图:
apollo::cyber::HeadingMsgFusion 的协作图:

Public 成员函数

 HeadingMsgFusion ()
 
 ~HeadingMsgFusion ()
 
virtual bool ConvertMsg (InputTypes< RosNavMsgPtr, RosOdometryMsgPtr > &, OutputTypes< OutputMsgPtr > &)
 convert the message between ros and apollo
 
- Public 成员函数 继承自 apollo::cyber::RosApolloMessageConverter< InputTypes< RosNavMsgPtr, RosOdometryMsgPtr >, OutputTypes< OutputMsgPtr > >
 RosApolloMessageConverter ()
 
 RosApolloMessageConverter ()
 
 ~RosApolloMessageConverter () override
 
 ~RosApolloMessageConverter () override
 
bool Init () override
 
bool Init () override
 
- Public 成员函数 继承自 apollo::cyber::MessageConverter
 MessageConverter ()
 
virtual ~MessageConverter ()
 
bool IsInit () const
 

额外继承的成员函数

- Protected 成员函数 继承自 apollo::cyber::RosApolloMessageConverter< InputTypes< RosNavMsgPtr, RosOdometryMsgPtr >, OutputTypes< OutputMsgPtr > >
virtual bool ConvertMsg (const std::shared_ptr< InputTypes< RosNavMsgPtr, RosOdometryMsgPtr > > &ros_msg0, const std::shared_ptr< OutputTypes< OutputMsgPtr > > &ros_msg1, const std::shared_ptr< NullType > &ros_msg2, const std::shared_ptr< NullType > &ros_msg3, std::shared_ptr< NullType > &apollo_msg0, std::shared_ptr< NullType > &apollo_msg1, std::shared_ptr< NullType > &apollo_msg2, std::shared_ptr< NullType > &apollo_msg3)=0
 
virtual bool ConvertMsg (InputTypes &input, OutputTypes &output)=0
 
- Protected 成员函数 继承自 apollo::cyber::MessageConverter
bool LoadConfig (ConverterConf *config)
 
- Protected 属性 继承自 apollo::cyber::MessageConverter
std::atomic< bool > init_
 
std::unique_ptr< apollo::cyber::Node > cyber_node_
 
std::vector< std::shared_ptr< apollo::cyber::proto::RoleAttributes > > apollo_attrs_
 
std::vector< std::shared_ptr< apollo::cyber::ReaderBase > > apollo_readers_
 
std::vector< std::shared_ptr< apollo::cyber::WriterBase > > apollo_writers_
 
const std::string node_name_ = "converter_base"
 
ConverterConf converter_conf_
 

详细描述

在文件 heading_msg_fusion.h 第 64 行定义.

构造及析构函数说明

◆ HeadingMsgFusion()

apollo::cyber::HeadingMsgFusion::HeadingMsgFusion ( )
inline

在文件 heading_msg_fusion.h 第 68 行定义.

68{}

◆ ~HeadingMsgFusion()

apollo::cyber::HeadingMsgFusion::~HeadingMsgFusion ( )
inline

在文件 heading_msg_fusion.h 第 69 行定义.

69{}

成员函数说明

◆ ConvertMsg()

bool apollo::cyber::HeadingMsgFusion::ConvertMsg ( InputTypes< RosNavMsgPtr, RosOdometryMsgPtr > &  in,
OutputTypes< OutputMsgPtr > &  out 
)
virtual

convert the message between ros and apollo

参数
InputTypesinput message container
OutputTypesoutput message container
返回
result, true for success

在文件 heading_msg_fusion.cc 第 23 行定义.

25 {
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}
apollo::cyber::QuaternionToHeading
double QuaternionToHeading(double yaw)
Definition quaternion_math.h:41
apollo::cyber::QuaternionToEuler
void QuaternionToEuler(const double quaternion[4], double att[3])
Definition quaternion_math.h:22
该类的文档由以下文件生成: