ROS探索总结(二十一)——如何发布里程计消息

  • 内容
  • 评论
  • 相关

       ROS使用tf来决定机器人的位置和静态地图中的传感器数据,但是tf中没有机器人的速度信息,所以导航功能包要求机器人能够通过里程计信息源发布包含速度信息的里程计nav_msgs/Odometry 消息。 本篇将介绍nav_msgs/Odometry消息,并且通过代码实现消息的发布,以及tf树的变换。

1、nav_msgs/Odometry消息格式

       nav_msgs/Odometry消息包含有机器人在自由空间中的位置和速度估算值。

  1. # This represents an estimate of a position and velocity in free space.
  2. # The pose in this message should be specified in the coordinate frame given by header.frame_id.
  3. # The twist in this message should be specified in the coordinate frame given by the child_frame_id
  4. Header header
  5. string child_frame_id
  6. geometry_msgs/PoseWithCovariance pose
  7. geometry_msgs/TwistWithCovariance twist

       pose参数包含机器人在里程计参考系下的位置估算值,同时带有可选的估算协方差。twist参数包含机器人在子参考系(一般是机器人基础参考系)下的速度,同时带有可选的速度估算协方差。

2、使用tf发布里程计变换信息

        参见 Transform Configuration教程。

3、代码例程

        这里使用一个简单的例程,实现 nav_msgs/Odometry消息的发布和tf变换,通过伪造的数据,实现机器人圆周运动。

        首先需要将一些以来添加到manifest.xml中:

  1. <depend package="tf"/>
  2. <depend package="nav_msgs"/>

        实现代码如下:

  1. #include <ros/ros.h>
  2. #include <tf/transform_broadcaster.h>
  3. #include <nav_msgs/Odometry.h>
  4.  
  5. int main(int argc, char** argv){
  6.   ros::init(argc, argv, "odometry_publisher");
  7.  
  8.   ros::NodeHandle n;
  9.   ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);
  10.   tf::TransformBroadcaster odom_broadcaster;
  11.  
  12.   double x = 0.0;
  13.   double y = 0.0;
  14.   double th = 0.0;
  15.  
  16.   double vx = 0.1;
  17.   double vy = -0.1;
  18.   double vth = 0.1;
  19.  
  20.   ros::Time current_time, last_time;
  21.   current_time = ros::Time::now();
  22.   last_time = ros::Time::now();
  23.  
  24.   ros::Rate r(1.0);
  25.   while(n.ok()){
  26.  
  27.     ros::spinOnce();               // check for incoming messages
  28.     current_time = ros::Time::now();
  29.  
  30.     //compute odometry in a typical way given the velocities of the robot
  31.     double dt = (current_time - last_time).toSec();
  32.     double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
  33.     double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
  34.     double delta_th = vth * dt;
  35.  
  36.     x += delta_x;
  37.     y += delta_y;
  38.     th += delta_th;
  39.  
  40.     //since all odometry is 6DOF we'll need a quaternion created from yaw
  41.     geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);
  42.  
  43.     //first, we'll publish the transform over tf
  44.     geometry_msgs::TransformStamped odom_trans;
  45.     odom_trans.header.stamp = current_time;
  46.     odom_trans.header.frame_id = "odom";
  47.     odom_trans.child_frame_id = "base_link";
  48.  
  49.     odom_trans.transform.translation.x = x;
  50.     odom_trans.transform.translation.y = y;
  51.     odom_trans.transform.translation.z = 0.0;
  52.     odom_trans.transform.rotation = odom_quat;
  53.  
  54.     //send the transform
  55.     odom_broadcaster.sendTransform(odom_trans);
  56.  
  57.     //next, we'll publish the odometry message over ROS
  58.     nav_msgs::Odometry odom;
  59.     odom.header.stamp = current_time;
  60.     odom.header.frame_id = "odom";
  61.  
  62.     //set the position
  63.     odom.pose.pose.position.x = x;
  64.     odom.pose.pose.position.y = y;
  65.     odom.pose.pose.position.z = 0.0;
  66.     odom.pose.pose.orientation = odom_quat;
  67.  
  68.     //set the velocity
  69.     odom.child_frame_id = "base_link";
  70.     odom.twist.twist.linear.x = vx;
  71.     odom.twist.twist.linear.y = vy;
  72.     odom.twist.twist.angular.z = vth;
  73.  
  74.     //publish the message
  75.     odom_pub.publish(odom);
  76.  
  77.     last_time = current_time;
  78.     r.sleep();
  79.   }
  80. }

       下面来剖析代码进行分析:

  1. #include <tf/transform_broadcaster.h>
  2. #include <nav_msgs/Odometry.h>

       我们需要实现“odom”参考系到“base_link”参考系的变换,以及nav_msgs/Odometry消息的发布,所以首先需要包含相关的头文件。

  1.   ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);  
  2.   tf::TransformBroadcaster odom_broadcaster;

       定义一个消息发布者来发布“odom”消息,在定义一个tf广播,来发布tf变换信息。

  1.   double x = 0.0;
  2.   double y = 0.0;
  3.   double th = 0.0;

       默认机器人的起始位置是odom参考系下的0点。

  1.   double vx = 0.1;
  2.   double vy = -0.1;
  3.   double vth = 0.1;

       我们设置机器人的默认前进速度,让机器人的base_link参考系在odom参考系下以x轴方向0.1m/s,Y轴速度-0.1m/s,角速度0.1rad/s的状态移动,这种状态下,可以让机器人保持圆周运动。

  1.   ros::Rate r(1.0);

        使用1Hz的频率发布odom消息,当然,在实际系统中,往往需要更快的速度进行发布。

  1.     //compute odometry in a typical way given the velocities of the robot
  2.     double dt = (current_time - last_time).toSec();
  3.     double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
  4.     double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
  5.     double delta_th = vth * dt;
  6.  
  7.     x += delta_x;
  8.     y += delta_y;
  9.     th += delta_th;

        使用我们设置的速度信息,来计算并更新里程计的信息,包括单位时间内机器人在x轴、y轴的坐标变化和角度的变化。在实际系统中,需要更具里程计的实际信息进行更新。

  1.    //since all odometry is 6DOF we'll need a quaternion created from yaw
  2.     geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);

        为了兼容二维和三维的功能包,让消息结构更加通用,里程计的偏航角需要转换成四元数才能发布,辛运的是,ROS为我们提供了偏航角与四元数相互转换的功能。

  1.     //first, we'll publish the transform over tf
  2.     geometry_msgs::TransformStamped odom_trans;
  3.     odom_trans.header.stamp = current_time;
  4.     odom_trans.header.frame_id = "odom";
  5.     odom_trans.child_frame_id = "base_link";

        创建一个tf发布需要使用的TransformStamped类型消息,然后根据消息结构填充当前的时间戳、参考系id、子参考系id,注意两个参考系的id必须要是“odom”和“base_link”。

  1.     odom_trans.transform.translation.x = x;
  2.     odom_trans.transform.translation.y = y;
  3.     odom_trans.transform.translation.z = 0.0;
  4.     odom_trans.transform.rotation = odom_quat;

        填充里程计信息,然后发布tf变换的消息。

  1.     //next, we'll publish the odometry message over ROS
  2.     nav_msgs::Odometry odom;
  3.     odom.header.stamp = current_time;

        别忘了,我们还要发布nav_msgs/Odometry消息,让导航包获取机器人的速度。创建消息变量,然后填充时间戳。

  1.     //set the position
  2.     odom.pose.pose.position.x = x;
  3.     odom.pose.pose.position.y = y;
  4.     odom.pose.pose.position.z = 0.0;
  5.     odom.pose.pose.orientation = odom_quat;
  6.  
  7.     //set the velocity
  8.     odom.child_frame_id = "base_link";
  9.     odom.twist.twist.linear.x = vx;
  10.     odom.twist.twist.linear.y = vy;
  11.     odom.twist.twist.angular.z = vth;

        填充机器人的位置、速度,然后发布消息。注意,我们发布的是机器人本体的信息,所以参考系需要填"base_link"。

 

        参考链接:http://wiki.ros.org/navigation/Tutorials/RobotSetup/Odom


原创文章,转载请注明: 转载自古月居

本文链接地址: ROS探索总结(二十一)——如何发布里程计消息

微信 OR 支付宝 扫描二维码
为本文作者 打个赏
pay_weixinpay_weixin

评论

26条评论
  1. Gravatar 头像

    Ziping 回复

    你好,我想问一下就是我现在可以用编码器读取单位时间内电机转过的圈数,然后要怎么转换成速度(我问的是一个单位问题,就是怎么转换成odom用的数据,因为我后续要用robot_pose_ekf对编码器和imu进行卡尔曼融合),所以这个单位怎么去统一? 🙂

    • 古月

      古月 回复

      @Ziping ROS中的线速度单位是m/s,角速度单位是rad/s

      • Gravatar 头像

        Ziping 回复

        @古月 好的,那假如我现在要slam建图,是不是odom需要和base_link同步,上文中程序base_link替换为map?因为我的odom是相对map移动的,需要这样修改吗?

        • 古月

          古月 回复

          @Ziping 不用的,坐标系之间的变换是slam功能包完成的,自己只要发布odom和base_link之间的变换就可以了

  2. Gravatar 头像

    Ziping 回复

    你好,我想问一下就是我现在可以用编码器读取单位时间内电机转过的圈数,然后要怎么转换成速度(我问的是一个单位问题,就是怎么转换成odom用的数据,因为我后续要用robot_pose_ekf对编码器和imu进行卡尔曼融合),所以这个单位怎么去统一?

  3. Gravatar 头像

    Billy 回复

    你好,古月大师。请问一下,如果自己搭建的小车无法输出运动速度和方向,也就是没有里程计信息,想通过激光雷达(Hokuyo)的扫描数据在地图(地图用hector_mapping得到)中定位,是否可行?顺便问一句,使用里程计信息进行导航的纠偏原理是什么?

    • 古月

      古月 回复

      @Billy 可以的, 使用hector包就可以实现激光定位。使用里程计会有累积误差,可以用卡尔曼滤波减少误差,同时配合其他传感器纠正定位。

      • Gravatar 头像

        Billy 回复

        @古月 多谢指教!还有个疑问,hector实现绘制地图与定位,那如何在绘制的地图中进行导航与纠偏呢?还能使用move_base包吗?我看move_base包订阅了/odom主题,但我没有里程计信息,怎么实现呢?

        • 古月

          古月 回复

          @Billy hector和gmapping建立的地图都是一样的,都可以用来导航,导航必须要有里程计话题的,里程计有很多种,如果你都没有的话,找下ROS里有没有基于激光雷达实现里程计话题的功能包

  4. Gravatar 头像

    曾杰 回复

    你好 古月 最近在用gazebo做强化学习,看的东西有些糊涂了。现在我在我的程序中提取小车的实时坐标,我想就用里程计的数据来表示坐标,可是看到了tf,似乎需要将里程计信息转换成map坐标?可是gazebo里面没有map啊,有些迷茫,希望能给点建议

    • 古月

      古月 回复

      @曾杰 里程计可以输出坐标的,是odom那个话题

  5. Gravatar 头像

    Aaron 回复

    您好,想问下这个如何得到真实的odom,是要时时更新vx vy vth么?我用gmapping,需要odom信息,才能发布速度,那我odom又要订阅速度。。。。有点懵。。。。

    • 古月

      古月 回复

      @Aaron 你好,速度是积分算出来的,也就是单位时间内的编码器旋转圈数,然后填入odom

      • Gravatar 头像

        Aaron 回复

        @古月 我原先这么想的,但是我不太知道是不是需要再写一个节点去读编码器的值运算后发布出速度信息,之后odom再去订阅获得信息

        • 古月

          古月 回复

          @Aaron 可以不用,在发布odom的节点里做就行了

          • Gravatar 头像

            Aaron 回复

            @古月 因为我是用串口通信。那我就先用串口读车的编码器的数据,进行积分,代替vx等,算出速度串口再发给地盘。这样理解对么?我用编码器算出的速度和我最后用slam算出的速度的差异在哪里。

            • 古月

              古月 回复

              @Aaron 可以的,很多机器人都是这样做的,通过串口直接读取到速度。这个地方你可能理解的不是很清楚,SLAM并不计算速度,相反的,SLAM需要机器人提供速度信息,也就是odom消息。这篇博客是在odom中积分计算速度的,和你在底盘控制器上计算速度原理一样,只是位置不同而已。

  6. Gravatar 头像

    Liu 回复

    不知道为何重复送出了.. 造成版面混乱实在不好意思

    • 古月

      古月 回复

      @Liu 没关系,网站问题,我已经删除了多余的留言

  7. Gravatar 头像

    Liu 回复

    古月大 您好!
    我最近刚接触ROS,使用的是Turtlebot机器人
    他的/odom child_frame_id是base_footprint
    我是否能透过这种发布方式 改变/odom Pose的位置呢?

    我有写个Script如下想透过这种方式改变机器人的Pose 好像不行

    odom = Odometry()
    odom.pose.pose.position.x = 0
    odom.pose.pose.position.y = 0
    odom.pose.pose.position.z = 0

    publish(odom)

    只有Script在执行的时候 可以在 rostopic echo /odom看的见值得变化
    可是停止执行后 rostopic echo /odom 显示的值又是原本的 毫无改变

    想要写个code 可以直接 改变Pose 值 已经困扰很久但是一直找不到方法
    希望古月大能够指点一番

    • 古月

      古月 回复

      @Liu 您好,这种方法是可以修改机器人的位置,但是除了发布odom消息外,还需要修改odom的tf变换,你在代码中的实现流程应该是这样的:
      //first, we'll publish the transform over tf
      geometry_msgs::TransformStamped odom_trans;
      odom_trans.header.stamp = current_time;
      odom_trans.header.frame_id = "odom";
      odom_trans.child_frame_id = "base_link";

      odom_trans.transform.translation.x = x;
      odom_trans.transform.translation.y = y;
      odom_trans.transform.translation.z = 0.0;
      odom_trans.transform.rotation = odom_quat;

      //send the transform
      odom_broadcaster.sendTransform(odom_trans);

      //next, we'll publish the odometry message over ROS
      nav_msgs::Odometry odom;
      odom.header.stamp = current_time;
      odom.header.frame_id = "odom";

      //set the position
      odom.pose.pose.position.x = x;
      odom.pose.pose.position.y = y;
      odom.pose.pose.position.z = 0.0;
      odom.pose.pose.orientation = odom_quat;

      //set the velocity
      odom.child_frame_id = "base_link";
      odom.twist.twist.linear.x = vx;
      odom.twist.twist.linear.y = vy;
      odom.twist.twist.angular.z = vth;

      //publish the message
      odom_pub.publish(odom);
      另外,可以参考《ROS by example》这本书中的Turtlebot控制,有配套的代码可以参考

  8. Gravatar 头像

    acyloin 回复

    您好,我是最近接触tf与导航系统的。我不太清楚这里的协方差是用来表示什么的?是谁与谁的协方差?希望能回答,谢谢。

    • 古月

      古月 回复

      @acyloin 你好,这里协方差的概念目前我也不是很理解,涉及到位置估计方面的算法,我还没有研究过这个方面。

      • Gravatar 头像

        蒋卓 回复

        @古月 您好,我想问下tf的根坐标系是怎么建立的,也就是第一个坐标系是怎么确定的?

    • Gravatar 头像

      alert(/1/) 回复

      @acyloin alert(/1/) :razz:好的

发表评论

电子邮件地址不会被公开。 必填项已用*标注