在上一篇文章中介绍了ros和单片机的通讯，这一篇将介绍如何发布里程计信息，并在rviz中显示。
本文全部代码：网盘
创建功能包
创建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 <tf/transform_broadcaster.h> #include <nav_msgs/Odometry.h> #include </home/max/catkin_ws/devel/include/serial_port/header.h> 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<nav_msgs::Odometry>("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
参考我分享的功能包里的 链接
修改完成后就可以编译了。
运行并在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中的显示。