⑥【从0制作自己的ros导航小车：上位机篇】01、里程计与坐标变换发布

从0制作自己的ros导航小车

前言

前面已经实现了上下位机的串口通信，现在可以在接收串口数据的程序中发布里程计数据和坐标变换了！

一、里程计数据计算与发布

里程计数据的消息类型是nav_msgs::Odometry，包含位置、姿态（四元数表示）、线速度和角速度、协方差矩阵等信息：

Header header
  uint32 seq
  time stamp
  string frame_id      //父坐标系：一般设置为odom
string child_frame_id     //子坐标系：小车的基坐标系，一般设置为base_footprint
geometry_msgs/PoseWithCovariance pose
  geometry_msgs/Pose pose
    geometry_msgs/Point position
      float64 x
      float64 y
      float64 z
    geometry_msgs/Quaternion orientation
      float64 x
      float64 y
      float64 z
      float64 w
  float64[36] covariance
geometry_msgs/TwistWithCovariance twist
  geometry_msgs/Twist twist
    geometry_msgs/Vector3 linear
      float64 x
      float64 y
      float64 z
    geometry_msgs/Vector3 angular
      float64 x
      float64 y
      float64 z
  float64[36] covariance

①位置
现在我们知道的只有速度和角度，机器人的位置信息可以由速度积分出来，本项目使用的是两轮差速机器人模型，由数学推导可知当前位置与上一次位置的关系是：
在这里插入图片描述
其中(x,y) 是机器人的上一时刻位置，s是小车中心点的位移{s = （Dl +Dr）/ 2，其中Dl是左轮位移，Dr是右轮位移，位移=速度*时间}，θ是两次偏航角的夹角（这里需要先将角度值转换为弧度值，因为三角函数通常接收弧度作为参数，如果不使用弧度的话后续可视化可以看出来是有问题的，这里的转换如下②）。

②位姿
里程计消息格式中的位姿数据是四元数的形式，可以使用tf2::Quaternion对象中的setRPY方法来设置四元数，这个函数要求输入数据是欧拉角，并且单位需要是弧度，所以也需要先对角度进行如下转换：

double delta_th_rad = angle * (M_PI / 180.0);

后续需要将经过setRPY填充的tf2::Quaternion对象转换为 ROS 消息类型 geometry_msgs::Quaternion，再填充至里程计数据里才能正确发布。

③线速度
对于两轮差速模型而言，线速度计算公式可以如下：

linear = (left_speed_now + right_speed_now) / 2 /100;       //（左轮速度+右轮速度） /  2 /100。由于下位机上传的速度信息单位是cm/s，所以这里/100转换成m/s。

④角速度
角速度计算公式可以如下，也可以只使用轮速，不使用imu的偏航角：

delta_th = (delta_th_rad - angle_last) / t;         //（本次角度-上次角度）/ 时间差
这里使用的也是弧度，因为使用弧度可以使角速度的计算与线性速度的计算保持一致性，因为线性速度（如米/秒）也是基于SI单位制。

⑤协方差矩阵
从上面的消息类型中可以看到pose和 twist最后都有一个float64[36] covariance，这就是协方差矩阵，
我在网上参考其它博主的设置，如下所示，将这个定义放到代码中即可：

odom_pose_covariance_({
{1e-9, 0, 0, 0, 0, 0, 
0, 1e-3,1e-9, 0, 0, 0, 
0, 0, 1e6, 0, 0, 0,
0, 0, 0, 1e6, 0, 0, 
0, 0, 0, 0, 1e6, 0, 
0, 0, 0, 0, 0, 1e-9}}),

odom_twist_covariance({
{1e-9, 0, 0, 0, 0, 0, 
0, 1e-3,1e-9, 0, 0, 0, 
0, 0, 1e6, 0, 0, 0, 
0, 0, 0, 1e6, 0, 0, 
0, 0, 0, 0, 1e6, 0, 
0, 0, 0, 0, 0, 1e-9}})

经过以上总结，里程计数据发布代码如下：

   void MyNode::controlLoop() {
        static double position_x = 0.0;
        static double position_y = 0.0;
        static double position_x_last = 0.0;
        static double position_y_last = 0.0;
        static double center_d = 0.0;
        static double delta_th = 0.0;
        static double duration = 0.0;
        
        double left_speed_now = 0.0;
        double right_speed_now = 0.0;
        double angle = 0.0;
        double angle_last = 0.0;
        unsigned char testRece4 = 0x00;
        double linear = 0.0;   
        static ros::Time time_last = ros::Time::now();
        
        ros::Time time_now = ros::Time::now();

        readSpeed(left_speed_now, right_speed_now, angle, testRece4);
        
        double delta_th_rad = angle * (M_PI / 180.0);    //角度转弧度
        duration = (time_now - time_last).toSec();   //时间间隔：s
        center_d =  (left_speed_now*duration + right_speed_now*duration) / 200;    //中心点位移
        position_x = position_x_last + center_d *cos(delta_th_rad - angle_last) ;    //位置
        position_y = position_y_last + center_d *sin(delta_th_rad - angle_last) ;
        linear = (left_speed_now + right_speed_now) / 2;    //线速度
        delta_th = (delta_th_rad - angle_last)*duration;    //角速度
        
        //保存当前值作为下一次的值
        angle_last = delta_th_rad;
        position_y_last = position_y;
        position_x_last = position_x;

        //下面填充里程计数据并发布
        tf2::Quaternion odom_quat_tf2;
        odom_quat_tf2.setRPY(0, 0, delta_th_rad);
        
        nav_msgs::Odometry msg;
        msg.header.stamp = ros::Time::now();
        msg.header.frame_id = "odom";

        msg.pose.pose.position.x = position_x;
        msg.pose.pose.position.y = position_y;
        msg.pose.pose.position.z = 0.0;

        msg.pose.pose.orientation = tf2::toMsg(odom_quat_tf2);
        
        for (int i = 0; i < 36; ++i) {
            msg.pose.covariance[i] = odom_pose_covariance_[i];
        }

        msg.child_frame_id = "base_footprint";
  
        msg.twist.twist.linear.x = linear;
        msg.twist.twist.linear.y = 0.0;
        msg.twist.twist.angular.z = delta_th;
        
        for (int i = 0; i < 36; ++i) {
            msg.twist.covariance[i] = odom_twist_covariance[i];
        }
        
        odom_publisher.publish(msg);
    }
       
  这里面所有计算过程都在上面的介绍中，仔细看，对照着修改以适配自己的小车。

二、坐标变换发布

有小伙伴应该会疑惑，为什么发布了里程计数据之后还要发布坐标变换呢？这是因为里程计数据虽然发布了，但是odom和base_footprint是相对的，比如：此时在rviz中如果选择坐标系为odom，那就看不到base_footprint数据，如果选择base_footprint为坐标系，那就没有参考的坐标系了。所以发布base_footprint到odom的坐标变换，就是为了机器人的里程计数据能够在不同的坐标系之间正确转换。下一节会介绍如何在rviz中可视化机器人的位姿。

动态坐标变换的消息类型是geometry_msgs::TransformStamped，它封装了从一个坐标系到另一个坐标系的变换关系，并且包含了时间戳，这使得它非常适合用于动态环境中的坐标变换。直接给出代码：

        geometry_msgs::TransformStamped odom_trans;
        /*geometry_msgs::msg::TransformStamped odom_trans;*/
        odom_trans.header.stamp = ros::Time::now();
        odom_trans.header.frame_id = "odom";
        odom_trans.child_frame_id = "base_footprint";

        odom_trans.transform.translation.x = position_x;
        odom_trans.transform.translation.y = position_y;
        odom_trans.transform.translation.z = 0.0;
        
        odom_trans.transform.rotation = tf2::toMsg(odom_quat_tf2);
        tf_broadcaster->sendTransform(odom_trans);

这样这部分的内容就结束了，下一节在rviz中看看这节课的代码是否有问题，可视化小车的坐标系。