DSP

ROS 教程之 navigation ： 用 move_base 控制自己的机器人(2)

2019-07-13 19:56发布

站内文章 / DSP
11159 0 1270
转自：http://blog.csdn.net/heyijia0327/article/details/41831529 在这一篇文章中，将主要介绍如何将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了。       下面这段程序就是我的转换方法： [python] view plain copy  在CODE上查看代码片派生到我的代码片
  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  在CODE上查看代码片派生到我的代码片
  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()  

热门文章