ROS Twist和Odometry消息类型使用（Python）
消息类型：
1. Twist - 线速度角速度
通常被用于发送到/cmd_vel话题，被base controller节点监听，控制机器人运动
geometry_msgs/Twist geometry_msgs/Vector3 linear float64 x float64 y float64 z geometry_msgs/Vector3 angular float64 x float64 y float64 z
linear.x指向机器人前方，linear.y指向左方，linear.z垂直向上满足右手系，平面移动机器人常常linear.y和linear.z均为0
angular.z代表平面机器人的角速度，因为此时z轴为旋转轴
示例：
#! /usr/bin/env python ''' Author: xushangnjlh at gmail dot com Date: 2017/05/22 @package forward_and_back ''' import rospy from geometry_msgs.msg import Twist from math import pi class ForwardAndBack(): def __init__(self): rospy.init_node('forward_and_back', anonymous=False) rospy.on_shutdown(self.shutdown) # this "forward_and_back" node will publish Twist type msgs to /cmd_vel # topic, where this node act like a Publisher self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5) # parameters rate = 50 r = rospy.Rate(rate) linear_speed = 0.2 goal_distance = 5 linear_duration = goal_distance/linear_speed angular_speed = 1.0 goal_angular = pi angular_duration = goal_angular/angular_speed # forward->rotate->back->rotate for i in range(2): # this is the msgs variant, has Twist type, no data now move_cmd = Twist() move_cmd.linear.x = linear_speed # # should keep publishing ticks = int(linear_duration*rate) for t in range(ticks): # one node can publish msgs to different topics, here only publish # to /cmd_vel self.cmd_vel.publish(move_cmd) r.sleep # sleep according to the rate # stop 1 ms before ratate move_cmd = Twist() self.cmd_vel.publish(move_cmd) rospy.sleep(1) move_cmd.angular_speed.z = angular_speed ticks = int(angular_duration*rate) for t in range(ticks): self.cmd_vel.publish(move_cmd) r.sleep() self.cmd_vel.publish(Twist()) rospy.sleep(1) def shutdown(self): rospy.loginfo("Stopping the robot") self.cmd_vel.publish(Twist()) rospy.sleep(1) if __name__=='__main__': try: ForwardAndBack() except: rospy.loginfo("forward_and_back node terminated by exception")
2. nav_msgs/Odometry - 里程计（位姿+线速度角速度+各自协方差）
通常，发布到/cmd_vel topic然后被机器人（例如/base_controller节点）监听到的Twist消息是不可能准确地转换为实际的机器人运动路径的，误差来源于机器人内部传感器误差（编码器），标定精度，以及环境影响（地面是否打滑平整）；因此我们可以用更准确的Odometry消息类型类获取机器人位姿（/odom到/base_link的tf变换）。在标定准确时，该消息类型与真实的机器人位姿误差大致在1%量级（即使在rviz中显示的依然误差较大）。
还包括
- 参考系信息，Odometry使用/odom作为parent frame id，/base_link作为child frame id；也就是说世界参考系为/odom（通常设定为起始位姿，固定不动），移动参考系为/base_link（这里还有点不理解，后面来填坑）
- 时间戳，因此不仅知道运动轨迹，还可以知道对应时间点
Header header string child_frame_id geometry_msgs/PoseWithCovariance pose geometry_msgs/TwistWithCovariance twist
# 展开 Header header uint32 seq time stamp string frame_id string child_frame_id geometry_msgs/PoseWithCovariance pose geometry_msgs/Pose pose geometry_msgs/Point position float64 x float64 y float64 z geometry_msgs/Quaternion orientation float64 x float64 y float64 z float64 w float64[36] covariance geometry_msgs/TwistWithCovariance twist geometry_msgs/Twist twist geometry_msgs/Vector3 linear float64 x float64 y float64 z geometry_msgs/Vector3 angular float64 x float64 y float64 z float64[36] covariance
示例：
运动路径和位姿通过内部的Odometry获取，该Odemetry的位姿通过监听tf坐标系变换获取（/odom和/base_link）
#! /usr/bin/env python ''' Author: xushangnjlh at gmail dot com Date: 2017/05/23 @package odometry_forward_and_back ''' import rospy from geometry_msgs.msg import Twist, Point, Quaternion import tf from rbx1_nav.tranform_utils import quat_to_angle, normalize_angle from math import pi, radians, copysign, sqrt, pow class Odometry_forward_and_back: def __init__(self): rospy.ini_node('odometry_forward_and_back', anonymous=False) rospy.on_shutdown(self.shutdown) self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5) rate = 20 r = rospy.Rate(rate) linear_speed = 0.2 goal_distance =1.0 angular_speed = 1.0 goal_angle = pi angular_tolerance = radians(2.5) # Initialize tf listener, and give some time to fill its buffer self.tf_listener = tf.TransformListener() rospy.sleep(2) # Set odom_frame and base_frame self.odom_frame = '/odom' try: self.tf_listener.waitForTransform(self.odom_frame, '/base_footprint', rospy.Time(), rospy.Duration(1.0)) self.base_frame = '/base_footprint' except(tf.Exception, tf.ConnectivityException, tf.LookupException): try: self.tf_listener.waitForTransform(self.odom_frame, '/base_link', rospy.Time(), rospy.Duration(1.0)) self.base_frame = '/base_link' except(tf.Exception, tf.ConnectivityException, tf.LookupException): rospy.loginfo("Cannot find base_frame transformed from /odom") rospy.signal_shutdown("tf Exception") position = Point() for i in range(2): move_cmd = Twist() move_cmd.linear.x = linear_speed # Initial pose, obtained from internal odometry (position, rotation) = self.get_odom() x_start = position.x y_start = position.y distance = 0 # Keep publishing Twist msgs, until the internal odometry reach the goal while distance < goal_distance and not rospy.is_shutdown(): self.cmd_vel.publish(move_cmd) r.sleep() (position, rotation) = self.get_odom() distance = sqrt( pow( (position.x-x_start), 2 ) + \ pow( (position.y-y_start), 2 ) ) # Stop 1 ms before rotate move_cmd = Twist() self.cmd_vel.publish(move_cmd) rospy.sleep(1) move_cmd.angular.z = angular_speed # should be the current ration from odom angle_last = rotation angle_turn = 0 while abs(angle_turn+angular_tolerance) < abs(goal_angle) \ and not rospy.is_shutdown(): self.cmd_vel.publish(move_cmd) r.sleep() (position, rotation) = self.get_odom delta_angle = normalize_angle(rotation - angle_last) angle_turn += delta_angle angle_last = rotation move_cmd = Twist() self.cmd_vel.publish(move_cmd) rospy.sleep(1) self.cmd_vel.publish(Twist()) def get_dom(self): try: (trans, rot) = self.tf_listener.lookupTransfrom(self.odom_frame, self.base_frame, rospy.Time(0)) except(tf.Exception, tf.ConnectivityException, tf.LookupException): rospy.loginfo("TF exception, cannot get_dom()!") return # angle is only yaw : RPY()[2] return (Point(*trans), quat_to_angle(*rot)) def shutdown(self): rospy.loginfo("Stopping the robot...") self.cmd_vel.publish(Twist(0)) rospy.sleep(1) if __name__=='__main__': try: Odometry_forward_and_back() except: rospy.loginfo("Odometry_forward_and_back node terminated!")
注意这里存在tf操作：
self.tf_listener = tf.TransformListener()
rospy.sleep(2)
创建TransformListener对象监听坐标系变换，这里需要sleep 2ms用于tf缓冲。
可以通过以下API获取tf变换，保存在TransformListener对象中，通过lookupTransform获取：
# TransformListener.waitForTransform('ref_coordinate', 'moving_coordinate', rospy.Time(), rospy.Duration(1.0)) self.tf_listener.waitForTransform(self.odom_frame, '/base_footprint', rospy.Time(), rospy.Duration(1.0))
(trans, rot) = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
