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