消息類型：
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 rospyfrom geometry_msgs.msg import Twistfrom math import piclass 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 headerstring 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 comDate: 2017/05/23
@package odometry_forward_and_back'''import rospyfrom geometry_msgs.msg import Twist, Point, Quaternionimport tffrom rbx1_nav.tranform_utils import quat_to_angle, normalize_anglefrom math import pi, radians, copysign, sqrt, powclass 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))