在上一篇文章中介紹了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中的顯示。