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))