• 欢迎访问开心洋葱网站,在线教程,推荐使用最新版火狐浏览器和Chrome浏览器访问本网站,欢迎加入开心洋葱 QQ群
  • 为方便开心洋葱网用户,开心洋葱官网已经开启复制功能!
  • 欢迎访问开心洋葱网站,手机也能访问哦~欢迎加入开心洋葱多维思维学习平台 QQ群
  • 如果您觉得本站非常有看点,那么赶紧使用Ctrl+D 收藏开心洋葱吧~~~~~~~~~~~~~!
  • 由于近期流量激增,小站的ECS没能经的起亲们的访问,本站依然没有盈利,如果各位看如果觉着文字不错,还请看官给小站打个赏~~~~~~~~~~~~~!

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

人工智能 古月 1941次浏览 0个评论

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

1、nav_msgs/Odometry消息格式

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

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

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

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

        参见 Transform Configuration教程。

3、代码例程

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

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

<depend package="tf"/>
<depend package="nav_msgs"/>

        实现代码如下:

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>

int main(int argc, char** argv){
  ros::init(argc, argv, "odometry_publisher");

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

  double x = 0.0;
  double y = 0.0;
  double th = 0.0;

  double vx = 0.1;
  double vy = -0.1;
  double vth = 0.1;

  ros::Time current_time, last_time;
  current_time = ros::Time::now();
  last_time = ros::Time::now();

  ros::Rate r(1.0);
  while(n.ok()){

    ros::spinOnce();               // check for incoming messages
    current_time = ros::Time::now();

    //compute odometry in a typical way given the velocities of the robot
    double dt = (current_time - last_time).toSec();
    double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
    double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
    double delta_th = vth * dt;

    x += delta_x;
    y += delta_y;
    th += delta_th;

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

    //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);

    last_time = current_time;
    r.sleep();
  }
}

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

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

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

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

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

  double x = 0.0;
  double y = 0.0;
  double th = 0.0;

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

  double vx = 0.1;
  double vy = -0.1;
  double vth = 0.1;

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

  ros::Rate r(1.0);

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

    //compute odometry in a typical way given the velocities of the robot
    double dt = (current_time - last_time).toSec();
    double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
    double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
    double delta_th = vth * dt;

    x += delta_x;
    y += delta_y;
    th += delta_th;

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

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

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

    //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";

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

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

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

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

    //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;

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

 

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


开心洋葱 , 版权所有丨如未注明 , 均为原创丨未经授权请勿修改 , 转载请注明ROS探索总结(二十一)——如何发布里程计消息
喜欢 (0)

您必须 登录 才能发表评论!

加载中……