本文共 8803 字,大约阅读时间需要 29 分钟。
转自:http://blog.csdn.net/heyijia0327/article/details/41831529
在这一篇文章中,将主要介绍如何将DSP上采集到的速度转化为Odom,即左右轮速度转化为机器人离起点的x,y坐标和机器人的朝向角yaw,让move_base可以订阅到这个信息并做出相应的路径规划。在wiki论坛上有一个很详细的例程是,希望大家先仔细阅读。在这个程序里,它把转化好的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
-
-
- 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
程序中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:
-
-
- 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
- return x
-
- def talker(self):
- self.rec_data = COM_ctr.SerialData( datalen = 2)
- 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)
-
- pub = rospy.Publisher('car_speed', carOdom)
- pub_wheel = rospy.Publisher('wheel_speed', Num)
-
- r = rospy.Rate(500)
- 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
-
- 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
- v_x = (Rspeed + Lspeed)/10.0/2.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)
-
-
- 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指令来监控这个主题发布的频率:
看主题发布的频率是否和期待的一致。
现在已经将左右轮速度转化为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>
-
- 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();
-
- pub_ = n_.advertise<nav_msgs::Odometry>("odom", 1);
-
-
- sub_ = n_.subscribe("car_speed", 1, &SubscribeAndPublish::callback, this);
- }
-
- void callback(const beginner_tutorials::carOdom::ConstPtr& input)
- {
-
-
- current_time_ = ros::Time::now();
-
- vx_ = input->x;
- vy_ = input->y;
- vth_ = input->vth;
-
-
-
- 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;
-
-
- geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th_);
-
-
- 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;
-
-
- odom_broadcaster_.sendTransform(odom_trans);
-
-
- nav_msgs::Odometry odom;
- odom.header.stamp = current_time_;
- odom.header.frame_id = "odom";
-
-
- 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;
-
-
- odom.child_frame_id = "base_link";
- odom.twist.twist.linear.x = vx_;
- odom.twist.twist.linear.y = vy_;
- odom.twist.twist.angular.z = vth_;
-
-
- 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_ ;
-
- };
-
- int main(int argc, char **argv)
- {
-
- ros::init(argc, argv, "odometry_publisher");
-
-
- 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这个文件,打开它就可看到相应的转换程序,楼主和他们的其实相差无几。