在這一篇文章中，將主要介紹如何將DSP上采集到的速度轉化為Odom，即左右輪速度轉化為機器人離起點的x,y坐標和機器人的朝向角yaw，讓move_base可以訂閱到這個信息並做出相應的路徑規划。在wiki論壇上有一個很詳細的例程是關於如何發布Odometry信息的，希望大家先仔細閱讀。在這個程序里，它把轉化好的Odom信息發布到了兩個地方，第一個是廣播了tf關系，即每次機器人移動以后，/odom坐標系和/base_linke的關系，(關於為什么要發布這tf關系，見第三篇博文)；第二個是將消息發布到odom topic上。這兩個東西都將是move_base需要的。
但是它的那段演示程序里，將機器人x軸方向的速度，y軸方向速度，以及旋轉速度設置為常數了，實際中肯定是變化的。因此我們只需要將兩輪的速度轉化為x軸的速度(即前進方向的速度)和繞z軸旋轉的速度，再套用到那個程序里去，就能發布機器人的位姿給move_base了。
下面這段程序就是我的轉換方法：
def speed_to_odom(self, Lspeed = 0, Rspeed = 0): delta_speed = Rspeed - Lspeed if delta_speed < 0: theta_to_speed = 0.0077 #右轉系數 else: theta_to_speed = 0.0076 #左轉系數 #*比例系數是將單位時間內的左右輪位移差（速度差）轉化旋轉的角度增量，再除以20ms，得到旋轉角速度 v_th = delta_speed * theta_to_speed / 0.02 v_x = (Rspeed + Lspeed)/2.0 v_y = 0.0 return v_x, v_y, v_th #返回x,y軸速度，以及旋轉速度th
程序中20ms為速度采樣的周期。在這個轉換關系,我是把y軸速度設為0，左右輪速度的平均就是前進速度（即x軸速度），左右輪速度的差轉化為旋轉速度。請注意：將y軸速度設為0這種轉換時可行，也就是假定20ms內，機器人沒有在垂直於輪子的方向上發生位移。
將左右輪速度轉化完以后，就可以用官網的例程發布Odom消息了。
下面總結下思路，再貼出這段的完整源程序。在我的程序中，也就是前面所說的中間通信層程序，首先用pyserial監聽串口，一旦收到左右輪的速度信息，馬上將左右輪的速度信息轉化為x軸方向的前進速度，和繞z軸的旋轉速度，然后將這個信息發布到一個主題上（我程序中為car_speed主題）。對官網那段程序進行改進，訂閱這個car_speed消息，一旦收到各軸速度，由其速度轉化機器人的坐標以及航向角yaw,這些信息作為Odom topic發布。
首先看如何將左右輪速度值轉變為前進速度linear.x和轉向速度angular.z的程序，有了linear.x和angular.z以后再來考慮發布odom：
#!/usr/bin/env python # -*- coding: utf-8 -*- import roslib;roslib.load_manifest('beginner_tutorials') import rospy from beginner_tutorials.msg import Num, carOdom #自定義的消息 from geometry_msgs.msg import Twist import serial_lisenning as COM_ctr #自己寫的串口監聽模塊 import glob from math import sqrt, atan2, pow class bluetooth_cmd(): def __init__(self): rospy.init_node('robot_driver', anonymous=True) def callback(self,msg ): cmd_twist_rotation = msg.angular.z # cmd_twist_x = msg.linear.x * 10.0 cmd_twist_y = msg.linear.y * 10.0 wheelspeed = self.odom_to_speed(cmd_twist_x, cmd_twist_y,cmd_twist_rotation) print 'msg:', msg print wheelspeed self.blue_tooth_send([wheelspeed[0], self.speed_kp, self.speed_ki, wheelspeed[1]]) def odom_to_speed(self, cmd_twist_x =0, cmd_twist_y=0,cmd_twist_rotation=0): cent_speed = sqrt(pow(cmd_twist_x, 2) + pow(cmd_twist_y, 2)) yawrate2 = self.yawrate_to_speed(cmd_twist_rotation) Lwheelspeed = cent_speed - yawrate2/2 Rwheelspeed = cent_speed + yawrate2/2 return Lwheelspeed, Rwheelspeed def yawrate_to_speed(self, yawrate): if yawrate > 0: theta_to_speed = 0.0077 #右轉系數 else: theta_to_speed = 0.0076 #左轉系數 x = (yawrate * 0.02) / theta_to_speed #yawrate ：rad/s *0.02表示 20ms內應該轉多少弧度，/0.0076是把 要轉的弧度轉化為左右輪速度差 return x def talker(self): self.rec_data = COM_ctr.SerialData( datalen = 2) #啟動監聽COM 線程 allport = glob.glob('/dev/ttyU*') port = allport[0] baud = 115200 openflag = self.rec_data.open_com(port, baud) rospy.Subscriber("/cmd_vel", Twist, self.callback)#訂閱move_base發出的控制指令 pub = rospy.Publisher('car_speed', carOdom) pub_wheel = rospy.Publisher('wheel_speed', Num) #左右輪速度 r = rospy.Rate(500) # 100hz Lwheelpwm= 0 sumL = 0 sumR = 0 while not rospy.is_shutdown(): all_data = [] if self.rec_data.com_isopen(): all_data = self.rec_data.next() #接收的數據組 if all_data != []: #如果沒收到數據，不執行下面的 wheelspeed = Num() #自己的消息 car_speed = carOdom() leftspeed = all_data[0][0] rightspeed = all_data[1][0] wheelspeed.leftspeed = leftspeed wheelspeed.rightspeed = rightspeed #左右輪速度轉化為機器人x軸前進速度和繞Z軸旋轉的速度 resluts = self.speed_to_odom(leftspeed, rightspeed) car_speed.x = resluts[0] car_speed.y = resluts[1] car_speed.vth = resluts[2] pub.publish(car_speed) pub_wheel.publish(wheelspeed) r.sleep() if openflag: self.rec_data.close_lisen_com() def speed_to_odom(self, Lspeed = 0, Rspeed = 0): delta_speed = Rspeed - Lspeed if delta_speed < 0: theta_to_speed = 0.0077 #右轉系數 else: theta_to_speed = 0.0076 #左轉系數 v_th = delta_speed * theta_to_speed / 0.02 # first : transform delta_speed to delta_theta . second: dived by delta_t (20ms), get the yawrate v_x = (Rspeed + Lspeed)/10.0/2.0 # Lspeed : dm/s -- > m/s so need to /10.0 v_y = 0.0 return v_x, v_y, v_th def blue_tooth_send(self, data = [], head = 'HY'): if data !=[] and self.rec_data.com_isopen(): self.rec_data.send_data(data, head) #繞中心軸旋轉 設定為0 # print data if __name__ == '__main__': try: car_cmd = bluetooth_cmd() car_cmd.talker() except rospy.ROSInterruptException: pass
注意這段程序里用了自己定義的msg：Num 和 carOdom。這兩個msg文件存放於beginner_tutorials/msg文件夾下。如果不知道怎么創建msg，可以看官網的教程或者我的另一篇博文。
這里貼出我定義的消息的內容：
Num.msg:
float32 leftspeed
float32 rightspeed
carOdom.msg:
float32 x
float32 y
float32 vth
上面程序發布的/car_speed topic就包含了車子的linear.x和angular.z，運行這個節點以后，我們可以使用rostopic指令來監控這個主題發布的頻率：
rostopic hz /car_speed
看主題發布的頻率是否和期待的一致。
現在已經將左右輪速度轉化為x軸速度和旋轉速度了，下面貼出我改進的官網教程代碼，教大家如何發布Odom信息和odom與base_link之間的tf轉換關系。官網教程里的vx,vy,vth為常數，我們這里先訂閱自己上段程序發布的car_speed主題，也就是訂閱機器人實時的前進速度x和旋轉速度。把官網程序由常數改為機器人實際速度就行了。下面程序為C++寫的，在beginner_tutorials/src文件夾下創建空白文檔，命名為your_filename.cpp，把下列代碼復制進去：
#include <ros/ros.h> #include <tf/transform_broadcaster.h> #include <nav_msgs/Odometry.h> #include <beginner_tutorials/carOdom.h> //goal:subscribe the car_speed, then send them class SubscribeAndPublish { public: SubscribeAndPublish() { x_ = 0.0; y_ = 0.0; th_ = 0.0; vx_ = 0.0; vy_ = 0.0; vth_ = 0.0; current_time_ = ros::Time::now(); last_time_ = ros::Time::now(); //Topic you want to publish pub_ = n_.advertise<nav_msgs::Odometry>("odom", 1); //Topic you want to subscribe sub_ = n_.subscribe("car_speed", 1, &SubscribeAndPublish::callback, this); } void callback(const beginner_tutorials::carOdom::ConstPtr& input) { //nav_msgs::Odometry output; //.... do something with the input and generate the output... current_time_ = ros::Time::now(); vx_ = input->x; vy_ = input->y; vth_ = input->vth; //compute odometry in a typical way given the velocities of the robot //double dt = (current_time - last_time).toSec(); double dt = 0.02; 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 pub_.publish(odom); last_time_ = current_time_; } private: // ros::NodeHandle n_; ros::Publisher pub_; ros::Subscriber sub_; ros::Time current_time_, last_time_; tf::TransformBroadcaster odom_broadcaster_; double x_ ; double y_ ; double th_ ; double vx_; double vy_ ; double vth_ ; };//End of class SubscribeAndPublish int main(int argc, char **argv) { //Initiate ROS ros::init(argc, argv, "odometry_publisher"); //Create an object of class SubscribeAndPublish that will take care of everything SubscribeAndPublish SAPObject; ros::spin(); return 0; }
這段程序首先訂閱car_speed這個主題，一旦收到機器人的x軸速度和轉向速度，就調用callback去發布消息，讓move_base可以訂閱到。
注意：這段程序要能運行，必須在你的beginner_tutorials這個包里添加對tf,nav_msgs的依賴。
<depend package="tf"/> <depend package="nav_msgs"/>
要添加對這兩個包的依賴，需要修改在package.xml和CMakeLists.txt進行修改:
在package.xml中添加：
<build_depend>tf</build_depend> <build_depend>nav_msgs</build_depend>
和
<run_depend>tf</run_depend> <run_depend>nav_msgs</run_depend>
然后在CMakeLists.txt中 find_package（...）里添加 tf 和 nav_msgs就行了，最終得到：
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
message_generation
tf
nav_msgs
)
最后還要為了使得你的C++程序能夠運行，在CMakeLists.txt中最后或者相應位置，還要添加上如下指令：
add_executable(publish_odom src/publish_odom.cpp)
target_link_libraries(publish_odom ${catkin_LIBRARIES})
add_dependencies(publish_odom beginner_tutorials_generate_messages_cpp)
完成這些以后，編譯一下你的catkin_ws工作空間，在新終端中輸入如下指令：
cd ~/catkin_ws
catkin_make
現在，有了這兩個節點程序，dsp到move_base和move_base到dsp這條路通了，只要建立地圖，發布坐標轉換就可以用了。在下一篇文章中，我們將介紹幾個與導航有關的 tf 坐標轉換，再在一個空白地圖上使用move_base進行導航。
最后，關於這種左右輪速度轉化為Odom的程序，ros論壇里有,如這個鏈接。ros自己寫好的教程里也有如arbotix_driver 這個節點程序里有一句，你可以在你的文件系統里搜索arbotix_driver：
from arbotix_python.diff_controller import DiffController
你在文件系統里搜索diff_controller這個文件，打開它就可看到相應的轉換程序，樓主和他們的其實相差無幾。