码迷,mamicode.com
首页 > 其他好文 > 详细

move_base 控制机器人(2)

时间:2017-11-17 14:58:27      阅读:190      评论:0      收藏:0      [点我收藏+]

标签:0.00   something   工作空间   文件   串口   node   中间   call   void   

在这一篇文章中,将主要介绍如何将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 well need a quaternion created from yaw  
    geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th_);  
  
    //first, well 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, well 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这个文件,打开它就可看到相应的转换程序,楼主和他们的其实相差无几。

move_base 控制机器人(2)

标签:0.00   something   工作空间   文件   串口   node   中间   call   void   

原文地址:http://www.cnblogs.com/arbain/p/7851146.html

(0)
(0)
   
举报
评论 一句话评论(0
登录后才能评论!
© 2014 mamicode.com 版权所有  联系我们:gaon5@hotmail.com
迷上了代码!