ROS中用Twist消息控制機器人
INSTALLING THE ROS-BY-EXAMPLE CODE是書中第五章的內容，如果我們按照上一篇教程執行過了，就可以直接進入第五章，安裝一個叫rbx1的包。這個包裏面包括了本書中用到的所有例子的源碼，包括路徑規劃，視覺，語音識別等功能。這本書基本就圍繞這個包來學習ROS的使用方法。
- 基本包的功能介紹
- 從終端發佈Twist消息控制機器人
- 從節點發布Twist消息控制機器人
- 下一步
1.基本包的功能介紹
ROS Base Controller是ROS中的基本控制節點，其中包含基本PID算法來控制電機運動。它主要任務是監聽終端中或其他節點發出的以/cmd_vel爲topic的控制指令(Topic是ROS中的一個基本概念，不同的節點之間通過發佈和訂閱相同Topic的消息來通信)，然後發佈/odom topic的odometry 消息(里程，指直接獲得的機器人走過的距離，通常由編碼器數據計算得到或結合慣導經EKF得到),同時發佈/odom座標系和機體座標系之間的轉換(如果odom數據由EKF得到的話，這個工作由 robot_pose_ekf 節點完成).
move_base pacakge 可以讓機器人移動至我們指定的目標地點，其中包含避障，路徑規劃等功能。
gmapping package :SLAM,用激光雷達或Kinect構建實時地圖
amcl package : 機器人利用SLAM或odometry數據實時定位
下面是整個流程圖：
2.從終端發佈Twist消息控制機器人
其實這個Twist消息我們上一次已經用過了，它的Topic是/cmd_vel，base controler訂閱Twist消息來控制電機。
在終端中執行以下指令查看Twist消息的具體內容：
rosmsg show geometry_msgs/Twist一切正常的話，終端會輸出以下信息。
其中有linear和angular兩個子消息，可以唯一確定機器人的運動狀態。
首先roscore，然後啓動機器人和仿真器：
roslaunch rbx1_bringup fake_turtlebot.launchrosrun rviz rviz -d `rospack find rbx1_nav`/sim.rviz打開新的終端，執行以下語句：
rostopic pub -r 10 /cmd_vel geometry_msgs/Twist '{linear: {x: 0.1, y: 0, z: 0}, angular: {x: 0, y: 0, z: -0.5}}'現在詳細解讀一下這條指令，
“－r 10” 表示以10HZ的頻率發佈這條消息，
“/cmd_vel”爲消息的Topic，”geometry_msgs/Twist” 爲消息類型，
‘{linear: {x: 0.1, y: 0, z: 0}, angular: {x: 0, y: 0, z: -0.5}}’表示指定的線速度和角速度。
運行效果如下：
然後我們讓讓機器人停下來，先Ctrl＋C終止進程，再輸入以下指令：
rostopic pub -1 /cmd_vel geometry_msgs/Twist '{}'點擊Rviz左下角的Reset，清除方向箭頭。再執行以下指令：
rostopic pub -1 /cmd_vel geometry_msgs/Twist '{linear: {x: 0.2, y: 0, z: 0}, angular: {x: 0, y: 0, z: 0}}'; rostopic pub -r 10 /cmd_vel geometry_msgs/Twist '{linear: {x: 0.2, y: 0, z: 0}, angular: {x: 0, y: 0, z: 0.5}}'運行效果如下：
pub -1表示僅發佈一次消息，機器人現已0.2m/s直線運行，大概3s之後，開始做圓周運動。
3.從節點發布Twist消息控制機器人
以上演示瞭如何從終端發佈指令控制機器人，但是大多數情況下是在程序中控制的。現在我們來看一下怎麼通過節點發布Twist消息控制機器人，這裏的demo都是用Python寫的，簡潔易懂。
還是先啓動機器人和仿真器：
roslaunch rbx1_bringup fake_turtlebot.launchrosrun rviz rviz -d `rospack find rbx1_nav`/sim.rviz然後啓動控制節點：
rosrun rbx1_nav timed_out_and_back.py這個 timed_out_and_back.py 程序在 rbx1/rbx1_nav/nodes 目錄下，功能是先讓機器人沿直線前進特定的距離，再旋轉180度，再前進相同的距離返回出發點。
運行效果如下：
現在我們看一下源代碼分析一下具體怎麼實現：
#!/usr/bin/env python
import rospy
#導入最主要的Python for ROS庫
from geometry_msgs.msg import Twist
from math import pi
#導入geometry_msgs包中的Twist消息類型
class OutAndBack():
def __init__(self):
# 節點名稱
rospy.init_node('out_and_back', anonymous=False)
# 當終端按下Ctrl＋C之後可以終止節點
rospy.on_shutdown(self.shutdown)
# 定義在/cmd_vel Topic中發佈Twist消息，控制機器人速度
self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
rate = 50
# 設置更新頻率爲50HZ
r = rospy.Rate(rate)
# 線速度
linear_speed = 0.2
# 目標距離
goal_distance = 1.0
# 到達目標的時間
linear_duration = goal_distance / linear_speed
# 角速度 1.0rad/s
angular_speed = 1.0
# 轉角爲Pi(180 degrees)
goal_angle = pi
# How long should it take to rotate?
angular_duration = goal_angle / angular_speed
# Loop through the two legs of the trip
for i in range(2):
# Initialize the movement command
move_cmd = Twist()
# Set the forward speed
move_cmd.linear.x = linear_speed
# 機器人向前運動，延時一定時間
ticks = int(linear_duration * rate)
for t in range(ticks):
self.cmd_vel.publish(move_cmd)
r.sleep()
# 發送一個空的Twist消息是機器人停止
move_cmd = Twist()
self.cmd_vel.publish(move_cmd)
rospy.sleep(1)
move_cmd.angular.z = angular_speed
# 機器人開始旋轉，延時一定時間使機器人轉180度
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)
# 循環兩次之後停止
self.cmd_vel.publish(Twist())
# 定義 shutdown(self)可以手動停止機器人
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.")