ros发布节点信息python_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'''
importrospyfrom geometry_msgs.msg importTwistfrom math importpiclassForwardAndBack():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 = 50r=rospy.Rate(rate)
linear_speed= 0.2goal_distance= 5linear_duration= goal_distance/linear_speed
angular_speed= 1.0goal_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 inrange(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 inrange(ticks):
self.cmd_vel.publish(move_cmd)
r.sleep()
self.cmd_vel.publish(Twist())
rospy.sleep(1)defshutdown(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'''
importrospyfrom geometry_msgs.msg importTwist, Point, Quaternionimporttffrom rbx1_nav.tranform_utils importquat_to_angle, normalize_anglefrom math importpi, radians, copysign, sqrt, powclassOdometry_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= 20r=rospy.Rate(rate)
linear_speed= 0.2goal_distance=1.0angular_speed= 1.0goal_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 notrospy.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=0while abs(angle_turn+angular_tolerance)
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())defget_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))defshutdown(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))