一、控制底座

效果：讓機器人花一段時間向前移動1m，再旋轉180度，最後返回原點。

1、基於定時的timed_out_and_back.py

# -*- coding: utf-8 -*-    有中文註釋
#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist
from math import pi

class OutAndBack():
    def __init__(self):
        #1、初始化：給出節點名字，設定回撥函式
        rospy.init_node('out_and_back', anonymous=False)
        rospy.on_shutdown(self.shutdown)
        
        #2、釋出話題，機器人運動速度
        self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
        rate = 50
        r = rospy.Rate(rate)
        #3、設定前進速度、距離；轉動速度、距離
        linear_speed = 0.2
        goal_distance = 1.0
        linear_duration = goal_distance / linear_speed
        angular_speed = 1.0
        goal_angle = pi
        angular_duration = goal_angle / angular_speed
     
        #往返兩次行程 
        for i in range(2):
            #4、前進1m
            move_cmd = Twist()
            move_cmd.linear.x = linear_speed
            ticks = int(linear_duration * rate)
            
            for t in range(ticks):
                self.cmd_vel.publish(move_cmd)
                r.sleep()
            
            # 旋轉之前停下來
            move_cmd = Twist()
            self.cmd_vel.publish(move_cmd)
            rospy.sleep(1)
            
            # 5、轉動180度
            move_cmd.angular.z = angular_speed
            ticks = int(goal_angle * rate)
            
            for t in range(ticks):           
                self.cmd_vel.publish(move_cmd)
                r.sleep()
                
            # 下一步之前停下來
            move_cmd = Twist()
            self.cmd_vel.publish(move_cmd)
            rospy.sleep(1)    
            
        # 釋出空的Twist訊息讓機器人停下來
        self.cmd_vel.publish(Twist())
        
    def shutdown(self):
        # 釋出空的Twist訊息讓機器人停下來
        rospy.loginfo("Stopping the robot...")
        self.cmd_vel.publish(Twist())
        rospy.sleep(1)
 
if __name__ == '__main__':
    try:
        OutAndBack()
    except:
        rospy.loginfo("Out-and-Back node terminated.")

 

OutAndBack類主要分為5部分：1、初始化：給出節點名字，設定回撥函式；2、釋出話題，機器人運動速度；3、設定前進速度、距離；轉動速度、距離；4、迴圈部分，前進1m，旋轉180度；5、主函式

接下來將詳細介紹5部分內容

1、def __init__(self) 標準的類初始化，self類似於C++中this指標。

2、self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5) 釋出Twist命令給/cmd_vel話題，且每秒50次更新機器人運動

4、前進1m

            # 初始化運動命令       
            move_cmd = Twist()
            # 設定前進速度
            move_cmd.linear.x = linear_speed
            # 在前進1米的這段時間控制命令更新的次數
            ticks = int(linear_duration * rate)
            for t in range(ticks):
                self.cmd_vel.publish(move_cmd)
                r.sleep()
 

 r.sleep（）是rospy.sleep(1/rate)的簡單寫法。

r=rospy.Rate(rate) 保持一定的速率來進行迴圈。

我們需要每1/rate秒釋出一次move_cmd訊息。一共釋出linear_duration*rate次訊息。

5、標準程式塊，例項化OutAndBack類執行指令碼。

if __name__ == '__main__':
    try:
        OutAndBack()
    except:
        rospy.loginfo("Out-and-Back node terminated.")

2、基於測量的odomz-out_and_back.py

#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist, Point, Quaternion
import tf
from rbx1_nav.transform_utils import quat_to_angle, normalize_angle
from math import radians, copysign, sqrt, pow, pi

class OutAndBack():
    def __init__(self):
        # 1、初始化：給出節點名稱，設定回撥函式
        rospy.init_node('out_and_back', anonymous=False)      
        rospy.on_shutdown(self.shutdown)
        # 2、釋出話題、前進速度和旋轉速度
        self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5)
        rate = 20
        r = rospy.Rate(rate)
        linear_speed = 0.15
        goal_distance = 1.0
        angular_speed = 0.5
        angular_tolerance = radians(1.0)
        goal_angle = pi
        # 3、新增的一系列功能：tf、odom、base_footprint、Point
        # 初始化tf監聽器，填補緩衝區
        self.tf_listener = tf.TransformListener()
        rospy.sleep(2)
        # 設定odom座標系
        self.odom_frame = '/odom'
        # 詢問機器人使用/base_link座標系還是/base_footprint座標系
        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 transform between /odom and /base_link or /base_footprint")
                rospy.signal_shutdown("tf Exception")         
        # 初始化Point型別的變數
        position = Point()
        # 4、迴圈，前進1m，轉動180度
        for i in range(2):
            # 前進1m
            move_cmd = Twist()
            move_cmd.linear.x = linear_speed
            # 得到開始的位姿資訊
            (position, rotation) = self.get_odom()            
            x_start = position.x
            y_start = position.y  
            # 追蹤行駛距離
            distance = 0  
            # 迴圈
            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))
            # 轉動前停止
            move_cmd = Twist()
            self.cmd_vel.publish(move_cmd)
            rospy.sleep(1)
            
            # 給旋轉配置運動命令
            move_cmd.angular.z = angular_speed
            # 跟蹤最後角度
            last_angle = rotation
            # 跟蹤轉動的角度
            turn_angle = 0  
            while abs(turn_angle + 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 - last_angle)
                # 新增到合計中
                turn_angle += delta_angle
                last_angle = rotation          
            # 轉動前停止
            move_cmd = Twist()
            self.cmd_vel.publish(move_cmd)
            rospy.sleep(1)   
        # 停止機器人
        self.cmd_vel.publish(Twist())
        
    def get_odom(self):
        # 得到現測量值與基框架間轉換
        try:
            (trans, rot)  = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
        except (tf.Exception, tf.ConnectivityException, tf.LookupException):
            rospy.loginfo("TF Exception")
            return

        return (Point(*trans), quat_to_angle(Quaternion(*rot)))
        
    def shutdown(self):
        # Always stop the robot when shutting down the node.
        rospy.loginfo("Stopping the robot...")
        self.cmd_vel.publish(Twist())
        rospy.sleep(1)
 
if __name__ == '__main__':
    try:
        OutAndBack()
    except:
        rospy.loginfo("Out-and-Back node terminated.")

在上一節基礎上增加了

3、新增的一系列功能：tf、odom、base_footprint、Point；4、對於前進直線和旋轉的記錄。

1、self.tf_listener = tf.TransformListener() 

      rospy.sleep(2)

我們建立一個TransformListener物件來監視座標系轉換（frame transforms）。 注意 tf需要一些時間去填補監聽者(listener)的緩衝區，所以我們需要呼叫 rospy.sleep(2)。

2、get_odom()

(position, rotation) = self.get_odom()
    def get_odom(self):
        # Get the current transform between the odom and base frames
        try:
            (trans, rot)  = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
        except (tf.Exception, tf.ConnectivityException, tf.LookupException):
            rospy.loginfo("TF Exception")
            return

        return (Point(*trans), quat_to_angle(Quaternion(*rot)))

首先使用tf_listener物件查詢當前 odom 座標系和 base（基礎）座標系之間的變換。正常的話返回位置和旋轉（四元數表示）

其中：return (Point(*trans), quat_to_angle(Quaternion(*rot))) 這段語句。在 python 中使用 “ * ” 修飾的trans 和 rot 變數，表示傳給一個函式的形參是一個列表。我們這裡的 trans 是 x、y、z 軸座標值；rot是 x、y、z、w 四元數值。

總結：

上一篇文章核心思想是：通過前進1m這段時間控制命令更新的次數。

ticks = int(linear_duration * rate)
for t in range(ticks):
      self.cmd_vel.publish(move_cmd)
        r.sleep()

本文使用TransformListener 來訪問 odom資訊，使用 tf 去監視 /odom 座標系與 /base_link 座標系之間的轉換要比依賴 /odom 話題要安全的多。