arduino上采集的数据转化为odom

在这一篇文章中，将主要介绍如何将arduino上采集到的速度转化为Odom，即左右轮速度转化为机器人离起点的x,y坐标和机器人的朝向角yaw，让move_base可以订阅到这个信息并做出相应的路径规划。在wiki论坛上有一个很详细的例程是关于如何发布Odometry信息的，希望大家先仔细阅读。在这个程序里，它把转化好的Odom信息发布到了两个地方，第一个是广播了tf关系，即每次机器人移动以后，/odom坐标系和/base_linke的关系，(关于为什么要发布这tf关系，见第三篇博文)；第二个是将消息发布到odom topic上。这两个东西都将是move_base需要的。

但是它的那段演示程序里，将机器人x轴方向的速度，y轴方向速度，以及旋转速度设置为常数了，实际中肯定是变化的。因此我们只需要将两轮的速度转化为x轴的速度(即前进方向的速度)和绕z轴旋转的速度，再套用到那个程序里去，就能发布机器人的位姿给move_base了。

      下面这段程序就是我的转换方法：

[python]  view plain  copy

 

 

1. def speed_to_odom(self, Lspeed = 0, Rspeed = 0):  
2.     delta_speed = Rspeed - Lspeed  
3.     if delta_speed < 0:  
4.         theta_to_speed = 0.0077 #右转系数  
5.     else:  
6.         theta_to_speed = 0.0076  #左转系数  
7.           
8.     #*比例系数是将单位时间内的左右轮位移差（速度差）转化旋转的角度增量，再除以20ms，得到旋转角速度  
9.     v_th = delta_speed  * theta_to_speed / 0.02     
10.     v_x = (Rspeed + Lspeed)/2.0      
11.     v_y = 0.0  
12.     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：

[python]  view plain  copy

 

 

1. #!/usr/bin/env python  
2. # -*- coding: utf-8 -*-  
3. import roslib;roslib.load_manifest('beginner_tutorials')  
4. import rospy  
5. from  beginner_tutorials.msg import Num, carOdom #自定义的消息  
6. from geometry_msgs.msg import Twist  
7.   
8. import serial_lisenning as COM_ctr #自己写的串口监听模块  
9. import glob  
10. from math import sqrt, atan2, pow  
11.   
12.   
13. class bluetooth_cmd():  
14.     def __init__(self):  
15.         rospy.init_node('robot_driver', anonymous=True)  
16.       
17.     def callback(self,msg ):  
18.   
19.         cmd_twist_rotation =  msg.angular.z #  
20.         cmd_twist_x  = msg.linear.x * 10.0  
21.         cmd_twist_y =  msg.linear.y * 10.0  
22.           
23.         wheelspeed = self.odom_to_speed(cmd_twist_x, cmd_twist_y,cmd_twist_rotation)  
24.         print 'msg:', msg  
25.         print wheelspeed  
26.         self.blue_tooth_send([wheelspeed[0], self.speed_kp, self.speed_ki,  wheelspeed[1]])  
27.       
28.     def odom_to_speed(self, cmd_twist_x =0, cmd_twist_y=0,cmd_twist_rotation=0):  
29.           
30.         cent_speed = sqrt(pow(cmd_twist_x, 2) + pow(cmd_twist_y, 2))  
31.         yawrate2 = self.yawrate_to_speed(cmd_twist_rotation)  
32.           
33.         Lwheelspeed = cent_speed - yawrate2/2  
34.         Rwheelspeed = cent_speed + yawrate2/2  
35.           
36.         return Lwheelspeed, Rwheelspeed  
37.           
38.     def yawrate_to_speed(self, yawrate):  
39.         if yawrate > 0:  
40.             theta_to_speed = 0.0077 #右转系数  
41.         else:  
42.             theta_to_speed = 0.0076  #左转系数  
43.               
44.         x = (yawrate * 0.02) / theta_to_speed #yawrate ：rad/s  *0.02表示 20ms内应该转多少弧度，/0.0076是把 要转的弧度转化为左右轮速度差  
45.         return   x  
46.           
47.     def talker(self):  
48.         self.rec_data = COM_ctr.SerialData( datalen = 2)  #启动监听COM 线程  
49.         allport = glob.glob('/dev/ttyU*')  
50.         port = allport[0]  
51.         baud = 115200  
52.         openflag = self.rec_data.open_com(port, baud)  
53.           
54.         rospy.Subscriber("/cmd_vel", Twist, self.callback)#订阅move_base发出的控制指令  
55.           
56.         pub = rospy.Publisher('car_speed', carOdom)  
57.         pub_wheel = rospy.Publisher('wheel_speed', Num) #左右轮速度  
58.           
59.         r = rospy.Rate(500) # 100hz  
60.         Lwheelpwm= 0  
61.           
62.         sumL = 0  
63.         sumR = 0  
64.           
65.         while not rospy.is_shutdown():  
66.             all_data = []  
67.             if self.rec_data.com_isopen():  
68.                 all_data = self.rec_data.next()  #接收的数据组  
69.          
70.             if all_data != []:  #如果没收到数据，不执行下面的  
71.                 wheelspeed = Num()  #自己的消息  
72.                 car_speed = carOdom()  
73.                 leftspeed = all_data[0][0]  
74.                 rightspeed = all_data[1][0]  
75.                 wheelspeed.leftspeed = leftspeed  
76.                 wheelspeed.rightspeed = rightspeed  
77.                 #左右轮速度转化为机器人x轴前进速度和绕Z轴旋转的速度  
78.                 resluts = self.speed_to_odom(leftspeed, rightspeed)  
79.                 car_speed.x = resluts[0]  
80.                 car_speed.y = resluts[1]  
81.                 car_speed.vth = resluts[2]  
82.   
83.                 pub.publish(car_speed)  
84.                 pub_wheel.publish(wheelspeed)  
85.                   
86.             r.sleep()  
87.            
88.         if openflag:  
89.             self.rec_data.close_lisen_com()    
90.       
91.     def speed_to_odom(self, Lspeed = 0, Rspeed = 0):  
92.         delta_speed = Rspeed - Lspeed  
93.         if delta_speed < 0:  
94.             theta_to_speed = 0.0077 #右转系数  
95.         else:  
96.             theta_to_speed = 0.0076  #左转系数  
97.               
98.         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  
99.         v_x = (Rspeed + Lspeed)/10.0/2.0    # Lspeed : dm/s   -- > m/s  so need to /10.0  
100.         v_y = 0.0  
101.         return v_x, v_y, v_th  
102.           
103.     def blue_tooth_send(self, data = [], head = 'HY'):  
104.         if data !=[] and self.rec_data.com_isopen():  
105.             self.rec_data.send_data(data, head)   #绕中心轴旋转 设定为0  
106. #        print data  
107.               
108. if __name__ == '__main__':  
109.     try:  
110.         car_cmd = bluetooth_cmd()  
111.         car_cmd.talker()  
112.     except rospy.ROSInterruptException: pass  

       注意这段程序里用了自己定义的msg：Num 和 carOdom。这两个msg文件存放于beginner_tutorials/msg文件夹下。如果不知道怎么创建msg，可以看官网的教程或者我的另一篇博文。

这里贴出我定义的消息的内容：

Num.msg:

[python]  view plain  copy

 

 

1. float32 leftspeed  
2. float32 rightspeed  

carOdom .msg:

[python]  view plain  copy

 

 

1. float32 x  
2. float32 y  
3. float32 vth  

      上面程序发布的/car_speed topic就包含了车子的linear.x和angular.z，运行这个节点以后，我们可以使用rostopic指令来监控这个主题发布的频率：

[python]  view plain  copy

 

 

1. rostopic hz /car_speed  

看主题发布的频率是否和期待的一致。

      现在已经将左右轮速度转化为x轴速度和旋转速度了，下面贴出我改进的官网教程代码，教大家如何发布Odom信息和odom与base_link之间的tf转换关系。官网教程里的vx,vy,vth为常数，我们这里先订阅自己上段程序发布的car_speed主题，也就是订阅机器人实时的前进速度x和旋转速度。把官网程序由常数改为机器人实际速度就行了。下面程序为C++写的，在beginner_tutorials/src文件夹下创建空白文档，命名为your_filename.cpp，把下列代码复制进去：

[cpp]  view plain  copy

 

 

1. #include <ros/ros.h>  
2. #include <tf/transform_broadcaster.h>  
3. #include <nav_msgs/Odometry.h>  
4. #include <beginner_tutorials/carOdom.h>  
5. //goal:subscribe the car_speed, then send them  
6. class SubscribeAndPublish  
7. {  
8. public:  
9.   SubscribeAndPublish()  
10.   {  
11.      x_ = 0.0;  
12.      y_ = 0.0;  
13.      th_ = 0.0;  
14.   
15.      vx_ = 0.0;  
16.      vy_ = 0.0;  
17.      vth_ = 0.0;  
18.      current_time_ = ros::Time::now();  
19.      last_time_ = ros::Time::now();  
20.     //Topic you want to publish  
21.     pub_ = n_.advertise<nav_msgs::Odometry>("odom", 1);  
22.   
23.     //Topic you want to subscribe  
24.     sub_ = n_.subscribe("car_speed", 1, &SubscribeAndPublish::callback, this);  
25.   }  
26.   
27.   void callback(const beginner_tutorials::carOdom::ConstPtr& input)  
28.   {  
29.     //nav_msgs::Odometry output;  
30.     //.... do something with the input and generate the output...  
31.     current_time_ = ros::Time::now();  
32.       
33.     vx_ = input->x;  
34.     vy_ = input->y;  
35.     vth_ = input->vth;  
36.       
37.     //compute odometry in a typical way given the velocities of the robot  
38.     //double dt = (current_time - last_time).toSec();  
39.     double dt = 0.02;  
40.     double delta_x = (vx_ * cos(th_) - vy_ * sin(th_)) * dt;  
41.     double delta_y = (vx_ * sin(th_) + vy_ * cos(th_)) * dt;  
42.     double delta_th = vth_ * dt;  
43.   
44.     x_ += delta_x;  
45.     y_ += delta_y;  
46.     th_ += delta_th;  
47.   
48.     //since all odometry is 6DOF we'll need a quaternion created from yaw  
49.     geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th_);  
50.   
51.     //first, we'll publish the transform over tf  
52.     geometry_msgs::TransformStamped odom_trans;  
53.     odom_trans.header.stamp = current_time_;  
54.     odom_trans.header.frame_id = "odom";  
55.     odom_trans.child_frame_id = "base_link";  
56.   
57.     odom_trans.transform.translation.x = x_;  
58.     odom_trans.transform.translation.y = y_;  
59.     odom_trans.transform.translation.z = 0.0;  
60.     odom_trans.transform.rotation = odom_quat;  
61.   
62.     //send the transform  
63.     odom_broadcaster_.sendTransform(odom_trans);  
64.   
65.     //next, we'll publish the odometry message over ROS  
66.     nav_msgs::Odometry odom;  
67.     odom.header.stamp = current_time_;  
68.     odom.header.frame_id = "odom";  
69.   
70.     //set the position  
71.     odom.pose.pose.position.x = x_;  
72.     odom.pose.pose.position.y = y_;  
73.     odom.pose.pose.position.z = 0.0;  
74.     odom.pose.pose.orientation = odom_quat;  
75.   
76.     //set the velocity  
77.     odom.child_frame_id = "base_link";  
78.     odom.twist.twist.linear.x = vx_;  
79.     odom.twist.twist.linear.y = vy_;  
80.     odom.twist.twist.angular.z = vth_;  
81.   
82.     //publish the message  
83.     pub_.publish(odom);  
84.   
85.     last_time_ = current_time_;  
86.   
87.   }  
88.   
89. private:  
90. //   
91.   ros::NodeHandle n_;   
92.   ros::Publisher pub_;  
93.   ros::Subscriber sub_;  
94.   ros::Time current_time_, last_time_;  
95.   tf::TransformBroadcaster odom_broadcaster_;  
96.   double x_ ;  
97.   double y_ ;  
98.   double th_ ;  
99.   
100.   double vx_;  
101.   double vy_ ;  
102.   double vth_ ;  
103.   
104. };//End of class SubscribeAndPublish  
105.   
106. int main(int argc, char **argv)  
107. {  
108.   //Initiate ROS  
109.   ros::init(argc, argv, "odometry_publisher");  
110.   
111.   //Create an object of class SubscribeAndPublish that will take care of everything  
112.   SubscribeAndPublish SAPObject;  
113.   
114.   ros::spin();  
115.   
116.   return 0;  
117. }  

      这段程序首先订阅car_speed这个主题，一旦收到机器人的x轴速度和转向速度，就调用callback去发布消息，让move_base可以订阅到。

      注意：这段程序要能运行，必须在你的beginner_tutorials这个包里添加对tf,nav_msgs的依赖。

[python]  view plain  copy

 

 

1. <depend package="tf"/>  
2. <depend package="nav_msgs"/>  

要添加对这两个包的依赖，需要修改 在package.xml和CMakeLists.txt进行修改:
在package.xml中添加：

[python]  view plain  copy

 

 

1. <build_depend>tf</build_depend>  
2. <build_depend>nav_msgs</build_depend>  

和

[python]  view plain  copy

 

 

1. <run_depend>tf</run_depend>  
2. <run_depend>nav_msgs</run_depend>  

然后在CMakeLists.txt中 find_package（...）里添加 tf 和 nav_msgs就行了，最终得到：

[python]  view plain  copy

 

 

1. find_package(catkin REQUIRED COMPONENTS  
2.   roscpp  
3.   rospy  
4.   std_msgs  
5.   message_generation  
6.   tf  
7.   nav_msgs  
8. )  

最后还要为了使得你的C++程序能够运行，在CMakeLists.txt中最后或者相应位置，还要添加上如下指令：

[cpp]  view plain  copy

 

 

1. add_executable(publish_odom src/publish_odom.cpp)  
2. target_link_libraries(publish_odom ${catkin_LIBRARIES})  
3. add_dependencies(publish_odom beginner_tutorials_generate_messages_cpp)   

完成这些以后，编译一下你的catkin_ws工作空间，在新终端中输入如下指令：

[python]  view plain  copy

 

 

1. cd ~/catkin_ws  
2. catkin_make  

          现在，有了这两个节点程序，dsp到move_base和move_base到dsp这条路通了，只要建立地图，发布坐标转换就可以用了。在下一篇文章中，我们将介绍几个与导航有关的 tf 坐标转换，再在一个空白地图上使用move_base进行导航。

       最后，关于这种左右轮速度转化为Odom的程序，ros论坛里有,如这个链接。ros自己写好的教程里也有如arbotix_driver 这个节点程序里有一句，你可以在你的文件系统里搜索arbotix_driver：

[python]  view plain  copy

 

 

1. from arbotix_python.diff_controller import DiffController  

你在文件系统里搜索diff_controller这个文件，打开它就可看到相应的转换程序。