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

#include <odometry_parser.h>

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

Public 成员函数

 OdometryParser ()
 
 ~OdometryParser ()
 
void imuCallback (std::shared_ptr< RosImuMsg > msg)
 
void gpsCallback (std::shared_ptr< RosNavMsg > msg)
 
void publishOdometry ()
 
virtual bool ConvertMsg (InputTypes< RosWrapMsgPtr > &, OutputTypes< OutputWrapMsgPtr > &)
 convert the message between ros and apollo
 
- Public 成员函数 继承自 apollo::cyber::RosApolloMessageConverter< InputTypes< RosWrapMsgPtr >, OutputTypes< OutputWrapMsgPtr > >
 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< RosWrapMsgPtr >, OutputTypes< OutputWrapMsgPtr > >
virtual bool ConvertMsg (const std::shared_ptr< InputTypes< RosWrapMsgPtr > > &ros_msg0, const std::shared_ptr< OutputTypes< OutputWrapMsgPtr > > &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_
 

详细描述

在文件 odometry_parser.h 第 76 行定义.

构造及析构函数说明

◆ OdometryParser()

apollo::cyber::OdometryParser::OdometryParser ( )
inline

在文件 odometry_parser.h 第 80 行定义.

80 {
81#ifdef ENABLE_ROS_MSG
82 imu_sub_ = ros_node_->create_subscription<RosImuMsg>(
83 converter_conf_.ros_topic_name_0(), 10,
84 std::bind(&OdometryParser::imuCallback, this, std::placeholders::_1));
85 gps_sub_ = ros_node_->create_subscription<RosNavMsg>(
86 converter_conf_.ros_topic_name_1(), 10,
87 std::bind(&OdometryParser::gpsCallback, this, std::placeholders::_1));
88 odom_pub_ = ros_node_->create_publisher<RosOdomMsg>(
89 converter_conf_.ros_topic_name_2(), 10);
90 timer_ = ros_node_->create_wall_timer(
91 std::chrono::milliseconds(100),
92 std::bind(&OdometryParser::publishOdometry, this));
93#endif
94
95 wgs84pj_source_ = pj_init_plus("+proj=latlong +ellps=WGS84");
96 X_ = Eigen::VectorXd::Zero(9);
97 P_ = Eigen::MatrixXd::Identity(9, 9);
98 F_ = Eigen::MatrixXd::Identity(9, 9);
99 H_ = Eigen::MatrixXd::Identity(9, 9);
100 R_ = Eigen::MatrixXd::Identity(9, 9) * 0.1;
101 Q_ = Eigen::MatrixXd::Identity(9, 9) * 0.01;
102 last_time_ = apollo::cyber::Time::Now().ToSecond();
103 }
apollo::cyber::OdometryParser::gpsCallback
void gpsCallback(std::shared_ptr< RosNavMsg > msg)
Definition odometry_parser.cc:59
apollo::cyber::OdometryParser::imuCallback
void imuCallback(std::shared_ptr< RosImuMsg > msg)
Definition odometry_parser.cc:23
apollo::cyber::Time::Now
static Time Now()
get the current time.
Definition time.cc:57
apollo::cyber::Time::ToSecond
double ToSecond() const
convert time to second.
Definition time.cc:77

◆ ~OdometryParser()

apollo::cyber::OdometryParser::~OdometryParser ( )
inline

在文件 odometry_parser.h 第 104 行定义.

104{}

成员函数说明

◆ ConvertMsg()

bool apollo::cyber::OdometryParser::ConvertMsg ( InputTypes< RosWrapMsgPtr > &  in,
OutputTypes< OutputWrapMsgPtr > &  out 
)
virtual

convert the message between ros and apollo

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

在文件 odometry_parser.cc 第 132 行定义.

133 {
134 // do nothing
135 return true;
136}

◆ gpsCallback()

void apollo::cyber::OdometryParser::gpsCallback ( std::shared_ptr< RosNavMsg >  msg)

在文件 odometry_parser.cc 第 59 行定义.

59 {
60#ifdef ENABLE_ROS_MSG
61 gps_received_.store(true);
62 constexpr double DEG_TO_RAD_LOCAL = M_PI / 180.0;
63 Eigen::VectorXd Z(9);
64
65 double lon = msg->longitude;
66 double lat = msg->latitude;
67 double px = lon * DEG_TO_RAD_LOCAL;
68 double py = lat * DEG_TO_RAD_LOCAL;
69
70 if (utm_target_ == NULL) {
71 std::string proj4_text;
72 int zone = static_cast<int>(std::ceil((lon + 180.0) / 6.0));
73 proj4_text = "+proj=utm +zone=" + std::to_string(zone) +
74 " +ellps=WGS84 +towgs84=0,0,0,0,0,0,0 +units=m +no_defs";
75 utm_target_ = pj_init_plus(proj4_text.c_str());
76 }
77 pj_transform(wgs84pj_source_, utm_target_, 1, 1, &px, &py, NULL);
78
79 Z << px, py, msg->altitude, 0, 0, 0, 0, 0, 0;
80
81 Eigen::MatrixXd y = Z - H_ * X_;
82 Eigen::MatrixXd S = H_ * P_ * H_.transpose() + R_;
83 Eigen::MatrixXd K = P_ * H_.transpose() * S.inverse();
84 X_ = X_ + K * y;
85 P_ = (Eigen::MatrixXd::Identity(9, 9) - K * H_) * P_;
86#endif
87}

◆ imuCallback()

void apollo::cyber::OdometryParser::imuCallback ( std::shared_ptr< RosImuMsg >  msg)

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

23 {
24#ifdef ENABLE_ROS_MSG
25 imu_received_.store(true);
26 auto current_time = msg->header.stamp.sec + msg->header.stamp.nanosec / 1e9;
27 double dt = current_time - last_time_;
28 last_time_ = current_time;
29
30 F_(0, 3) = dt;
31 F_(1, 4) = dt;
32 F_(2, 5) = dt;
33
34 X_ = F_ * X_;
35 P_ = F_ * P_ * F_.transpose() + Q_;
36
37 X_(3) += msg->linear_acceleration.x * dt;
38 X_(4) += msg->linear_acceleration.y * dt;
39 X_(5) += msg->linear_acceleration.z * dt;
40
41 double euler_angles[3];
42
43 quaternion_[0] = msg->orientation.x;
44 quaternion_[1] = msg->orientation.y;
45 quaternion_[2] = msg->orientation.z;
46 quaternion_[3] = msg->orientation.w;
47
48 QuaternionToEuler(quaternion_, euler_angles);
49 auto roll = euler_angles[0];
50 auto pitch = euler_angles[1];
51 auto yaw = euler_angles[2];
52
53 X_(6) = roll;
54 X_(7) = pitch;
55 X_(8) = yaw;
56#endif
57}
apollo::cyber::QuaternionToEuler
void QuaternionToEuler(const double quaternion[4], double att[3])
Definition quaternion_math.h:22

◆ publishOdometry()

void apollo::cyber::OdometryParser::publishOdometry ( )

在文件 odometry_parser.cc 第 89 行定义.

89 {
90#ifdef ENABLE_ROS_MSG
91 if (!gps_received_.load() || !imu_received_.load()) {
92 return;
93 }
94 RosOdomMsg odom_msg;
95 odom_msg.header.stamp = ros_node_->get_clock()->now();
96
97 odom_msg.pose.pose.position.x = X_(0);
98 odom_msg.pose.pose.position.y = X_(1);
99 odom_msg.pose.pose.position.z = X_(2);
100
101 odom_msg.pose.pose.orientation.x = quaternion_[0];
102 odom_msg.pose.pose.orientation.y = quaternion_[1];
103 odom_msg.pose.pose.orientation.z = quaternion_[2];
104 odom_msg.pose.pose.orientation.w = quaternion_[3];
105
106 odom_msg.twist.twist.linear.x = X_(3);
107 odom_msg.twist.twist.linear.y = X_(4);
108 odom_msg.twist.twist.linear.z = X_(5);
109
110 for (int i = 0; i < 6; ++i) {
111 for (int j = 0; j < 6; ++j) {
112 if (i < 3 && j < 3) {
113 odom_msg.pose.covariance[i * 6 + j] = P_(i, j);
114 } else if (i >= 3 && j >= 3) {
115 odom_msg.pose.covariance[i * 6 + j] = P_(i + 3, j + 3);
116 }
117 }
118 }
119
120 for (int i = 0; i < 3; ++i) {
121 for (int j = 0; j < 3; ++j) {
122 odom_msg.twist.covariance[i * 6 + j] = P_(i + 3, j + 3);
123 }
124 }
125
126 odom_pub_->publish(odom_msg);
127 imu_received_.store(false);
128 imu_received_.store(false);
129#endif
130}
该类的文档由以下文件生成: