从零搭建一辆ROS小车（二）发布里程计数据在rviz中显示

在上一篇文章中介绍了ros和单片机的通讯，这一篇将介绍如何发布里程计信息，并在rviz中显示。

本文全部代码：网盘

---

1、创建功能包

创建odom_tf_package功能包

cd ~/catkin_ws/src
catkin_create_pkg odom_tf_package std_msgs rospy roscpp

 

创建odom_tf_pub节点

　　在src文件夹下新建odom_tf_pub.cpp，代码如下 ：

（注意第四行的头文件，是上一篇文章所提到的自定义消息编译生成的，所以在编译这个功能包的时候一定先编译上一篇文章的）

#include "ros/ros.h"
 #include 
 #include 
 #include 


    double x = 0.0;
    double y = 0.0;
    double th = 0.0;
//默认机器人的起始位置是odom参考系下的0点
    double vx = 0;
    double vy = 0;
    double vth = 0;
    double dt ;

void messageCallback(const serial_port::header::ConstPtr& msg) //获取上篇文章串口功能包发来的数据
{
    //ROS_INFO("I heard: [%d] [%d] [%d]", msg->num1, msg->num2, msg->num3);
    vx = double(msg->num1)/1000;
    vy = double(msg->num2)/100;
    vth = double(msg->num3)/1000;
    ROS_INFO("Vx = [%f] Vth = [%f]", vx,  vth);
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "odom_pub");
    ros::NodeHandle n;
    ros::Subscriber sub = n.subscribe("serial_data_odom", 1, messageCallback);//订阅上篇文章串口功能包的话题
    ros::Publisher odom_pub = n.advertise("odom", 1);
    tf::TransformBroadcaster odom_broadcaster;

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


    ros::Rate r(20);//以20Hz的速率发布里程信息，
    while(n.ok())
    {
        ros::spinOnce();            
        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)) * dt;
        double delta_y = (vx * sin(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);
        //我们通常会尝试在我们的系统中使用所有消息的3D版本，以允许2D和3D组件在适当的情况下协同工作，
        //并将消息数量保持在最低限度。因此，有必要将我们的yaw(偏航值)变为四元数。 
        //tf提供的功能允许从yaw(偏航值)容易地创建四元数，并且可以方便地获取四元数的偏航值。
        
        //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";
        /*在这里，我们将创建一个TransformStamped消息，通过tf发送。
        我们要发布在current_time时刻的从"odom"坐标系到“base_link”坐标系的变换。
        因此，我们将相应地设置消息头和child_frame_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;

        //send the transform
        odom_broadcaster.sendTransform(odom_trans);
//将我们的里程数据中填入变换消息中，然后使用TransformBroadcaster发送变换。

        //next, we'll publish the odometry message over ROS
        nav_msgs::Odometry odom;
        odom.header.stamp = current_time;
        odom.header.frame_id = "odom";
//还需要发布nav_msgs/Odometry消息，以便导航包可以从中获取速度信息。
//将消息的header设置为current_time和“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;
//这将使用里程数据填充消息，并发送出去。
//我们将消息的child_frame_id设置为“base_link”坐标系，
//因为我们要发送速度信息到这个坐标系。

        //publish the message
        odom_pub.publish(odom);
        last_time = current_time;
        
        r.sleep();
        }
 // return 0;
}

修改package.xml和CMakeLists.txt

　　参考我分享的功能包里的 链接

　　修改完成后就可以编译了。

 

2、运行并在rviz显示

roscore
rosrun serial_port keybord.py 
rosrun serial_port port_SubAndPub.py 
rosrun odom_tf_package om_tf_pub
rviz

　　这里需要运行上一篇的功能包，如果没有arduino的话就请自行修改代码，直接订阅键盘/cmd_vel，后续会更新在gazebo中仿真阿克曼车型的机器人。

 

打开rviz后 点击右下角add 添加tf

　　这时可以看到odom和base_link重合。

 

 

 

把右边第一个Global Options的Frame改成odom

　　修改后在键盘控制界面使用 U I O 控制 就可以看到移动了。

 

到此完成了在rviz中的显示。