EGOReplanFSM::odometryCallback() 函数代码解析 - ROS 里程计数据处理

以下是 void EGOReplanFSM::odometryCallback(const nav_msgs::OdometryConstPtr &msg) 函数中每一行代码的详细解释：

1. odom_pos_(0) = msg->pose.pose.position.x;: 将接收到的里程计消息中的 x 坐标赋值给 odom_pos_ 变量的第一个元素。

2. odom_pos_(1) = msg->pose.pose.position.y;: 将接收到的里程计消息中的 y 坐标赋值给 odom_pos_ 变量的第二个元素。

3. odom_pos_(2) = msg->pose.pose.position.z;: 将接收到的里程计消息中的 z 坐标赋值给 odom_pos_ 变量的第三个元素。

4. odom_vel_(0) = msg->twist.twist.linear.x;: 将接收到的里程计消息中的线性速度 x 分量赋值给 odom_vel_ 变量的第一个元素。

5. odom_vel_(1) = msg->twist.twist.linear.y;: 将接收到的里程计消息中的线性速度 y 分量赋值给 odom_vel_ 变量的第二个元素。

6. odom_vel_(2) = msg->twist.twist.linear.z;: 将接收到的里程计消息中的线性速度 z 分量赋值给 odom_vel_ 变量的第三个元素。

7. odom_orient_.w() = msg->pose.pose.orientation.w;: 将接收到的里程计消息中的四元数 w 分量赋值给 odom_orient_ 变量的 w 分量。

8. odom_orient_.x() = msg->pose.pose.orientation.x;: 将接收到的里程计消息中的四元数 x 分量赋值给 odom_orient_ 变量的 x 分量。

9. odom_orient_.y() = msg->pose.pose.orientation.y;: 将接收到的里程计消息中的四元数 y 分量赋值给 odom_orient_ 变量的 y 分量。

10. odom_orient_.z() = msg->pose.pose.orientation.z;: 将接收到的里程计消息中的四元数 z 分量赋值给 odom_orient_ 变量的 z 分量。

11. have_odom_ = true;: 将 have_odom_ 标志设置为 true，表示已经接收到里程计数据。

该函数的主要作用是处理接收到的里程计消息，并更新类的状态，包括位置、速度和姿态等信息。