8 ROS move_base（路徑規划）

博客轉載自：https://blog.csdn.net/hcx25909/article/details/9470297

在上一篇的博客中，我們一起學習了ROS定位於導航的總體框架，這一篇我們主要研究其中最重要的move_base包。

 

 

在總體框架圖中可以看到，move_base提供了ROS導航的配置、運行、交互接口，它主要包括兩個部分：

1. 全局路徑規划（global planner）：根據給定的目標位置進行總體路徑的規划；
2. 本地實時規划（local planner）：根據附近的障礙物進行躲避路線規划。

一、數據結構

ROS中定義了MoveBaseActionGoal數據結構來存儲導航的目標位置數據，其中最重要的就是位置坐標（position）和方向（orientation）

rosmsg show MoveBaseActionGoal

[move_base_msgs/MoveBaseActionGoal]:
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
actionlib_msgs/GoalID goal_id
  time stamp
  string id
move_base_msgs/MoveBaseGoal goal
  geometry_msgs/PoseStamped target_pose
    std_msgs/Header header
      uint32 seq
      time stamp
      string frame_id
    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

二、配置文件

 move_base使用前需要配置一些參數：運行成本、機器人半徑、到達目標位置的距離，機器人移動的速度，這些參數都在rbx1_nav包的以下幾個配置文件中：

 　　• base_local_planner_params.yaml
        • costmap_common_params.yaml
        • global_costmap_params.yaml
        • local_costmap_params.yaml

三、全局路徑規划（global planner）

在ROS的導航中，首先會通過全局路徑規划，計算出機器人到目標位置的全局路線。這一功能是navfn這個包實現的。navfn通過Dijkstra最優路徑的算法，計算costmap上的最小花費路徑，作為機器人的全局路線。將來在算法上應該還會加入A*算法（針對博主的Fuerte版本).  具體見：nav_fn

四、本地實時規划（local planner）

本地的實時規划是利用base_local_planner包實現的。該包使用Trajectory Rollout 和Dynamic Window approaches算法計算機器人每個周期內應該行駛的速度和角度（dx，dy，dtheta velocities）。

base_local_planner這個包通過地圖數據，通過算法搜索到達目標的多條路經，利用一些評價標准（是否會撞擊障礙物，所需要的時間等等）選取最優的路徑，並且計算所需要的實時速度和角度。其中，Trajectory Rollout 和Dynamic Window approaches算法的主要思路如下：

1. 采樣機器人當前的狀態（dx,dy,dtheta）；
2. 針對每個采樣的速度，計算機器人以該速度行駛一段時間后的狀態，得出一條行駛的路線。
3. 利用一些評價標准為多條路線打分。
4. 根據打分，選擇最優路徑。
5. 重復上面過程。

具體參見: local_planner

五、ArbotiX仿真——手動設定目標

在這一步，我們暫時使用空白地圖（blank_map.pgm），就在空地上進行無障礙仿真。首先運行ArbotiX節點，並且加載機器人的URDF文件。

roslaunch rbx1_bringup fake_turtlebot.launch

然后運行move_base和加載空白地圖的launch文件（fake_move_base_blank_map.launch）：

roslaunch rbx1_nav fake_move_base_blank_map.launch

 該文件的具體內容如下：

<launch>
  <!-- Run the map server with a blank map -->
  <node name="map_server" pkg="map_server" type="map_server" args="$(find rbx1_nav)/maps/blank_map.yaml"/>
    
  <include file="$(find rbx1_nav)/launch/fake_move_base.launch" />

  <!-- Run a static transform between /odom and /map -->
  <node pkg="tf" type="static_transform_publisher" name="odom_map_broadcaster" args="0 0 0 0 0 0 /map /odom 100" />

</launch>

 其中調用了fake_move_base.launch文件，是運行move_base節點並進行參數配置,  然后調用rviz就可以看到機器人了。

rosrun rviz rviz -d `rospack find rbx1_nav`/nav_fuerte.vcg

indigo/kinetic版本

rosrun rviz rviz -d `rospack find rbx1_nav`/nav.rviz

 

我們先以1m的速度進行一下測試, 讓機器人前進一米：

rostopic pub /move_base_simple/goal geometry_msgs/PoseStamped \
'{ header: { frame_id: "base_link" }, pose: { position: { x: 1.0, y: 0, z: 0 }, orientation: { x: 0, y: 0, z: 0, w: 1 } } }'

 讓機器人后退一米，回到原來的位置：

rostopic pub /move_base_simple/goal geometry_msgs/PoseStamped \
'{ header: { frame_id: "map" }, pose: { position: { x: 0, y: 0, z: 0 }, orientation: { x: 0, y: 0, z: 0, w: 1 } } }'

在rviz中的軌跡圖如下：

 

在機器人移動過程中，有一條藍色的線（被黃線擋住了）就是機器人的全局規划的路徑；紅色的箭頭是實施規划的路線，會不斷更新，有的時候會呈現很大的弧線，那是因為機器人在轉向的過程中盡量希望保持平穩的角度。如果覺得路徑規划的精度不夠，可以修改配置文件中的pdist_scale參數進行修正。然后我們可以認為的確定目標位置，點擊rviz上方的2D Nav Goal按鍵，然后左鍵選擇目標位置，機器人就開始自動導航了。

六、ArbotiX仿真——帶有障礙物的路徑規划

首先我們讓機器人走一個正方形的路線。先通過上面的命令，讓機器人回到原始位置（0，0，0），然后按reset按鍵，把所有的箭頭清除。接着運行走正方形路徑的代碼：

rosrun rbx1_nav move_base_square.py

 

 四個頂角的粉色圓盤就是我們設定的位置，正方形比較規則，可見定位還是比較准確的。然我們先來分析一下走正方形路線的代碼：

#!/usr/bin/env python
import roslib; roslib.load_manifest('rbx1_nav')
import rospy
import actionlib
from actionlib_msgs.msg import *
from geometry_msgs.msg import Pose, Point, Quaternion, Twist
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from tf.transformations import quaternion_from_euler
from visualization_msgs.msg import Marker
from math import radians, pi
 
class MoveBaseSquare():
    def __init__(self):
        rospy.init_node('nav_test', anonymous=False)
        
        rospy.on_shutdown(self.shutdown)
        
        # How big is the square we want the robot to navigate?
        # 設定正方形的尺寸，默認是一米
        square_size = rospy.get_param("~square_size", 1.0) # meters
        
        # Create a list to hold the target quaternions (orientations)
        # 創建一個列表，保存目標的角度數據
        quaternions = list()
        
        # First define the corner orientations as Euler angles
        # 定義四個頂角處機器人的方向角度（Euler angles:http://zh.wikipedia.org/wiki/%E6%AC%A7%E6%8B%89%E8%A7%92)
        euler_angles = (pi/2, pi, 3*pi/2, 0)
        
        # Then convert the angles to quaternions
        # 將上面的Euler angles轉換成Quaternion的格式
        for angle in euler_angles:
            q_angle = quaternion_from_euler(0, 0, angle, axes='sxyz')
            q = Quaternion(*q_angle)
            quaternions.append(q)
        
        # Create a list to hold the waypoint poses
        # 創建一個列表存儲導航點的位置
        waypoints = list()
        
        # Append each of the four waypoints to the list.  Each waypoint
        # is a pose consisting of a position and orientation in the map frame.
        # 創建四個導航點的位置（角度和坐標位置）
        waypoints.append(Pose(Point(square_size, 0.0, 0.0), quaternions[0]))
        waypoints.append(Pose(Point(square_size, square_size, 0.0), quaternions[1]))
        waypoints.append(Pose(Point(0.0, square_size, 0.0), quaternions[2]))
        waypoints.append(Pose(Point(0.0, 0.0, 0.0), quaternions[3]))
        
        # Initialize the visualization markers for RViz
        # 初始化可視化標記
        self.init_markers()
        
        # Set a visualization marker at each waypoint 
        # 給每個定點的導航點一個可視化標記(就是rviz中看到的粉色圓盤標記)
        for waypoint in waypoints:           
            p = Point()
            p = waypoint.position
            self.markers.points.append(p)
            
        # Publisher to manually control the robot (e.g. to stop it)
        # 發布TWist消息控制機器人
        self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist)
        
        # Subscribe to the move_base action server
        # 訂閱move_base服務器的消息
        self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
        
        rospy.loginfo("Waiting for move_base action server...")
        
        # Wait 60 seconds for the action server to become available
        # 等待move_base服務器建立
        self.move_base.wait_for_server(rospy.Duration(60))
        
        rospy.loginfo("Connected to move base server")
        rospy.loginfo("Starting navigation test")
        
        # Initialize a counter to track waypoints
        # 初始化一個計數器，記錄到達的頂點號
        i = 0
        
        # Cycle through the four waypoints
        # 主循環,環繞通過四個定點
        while i < 4 and not rospy.is_shutdown():
            # Update the marker display
            # 發布標記指示四個目標的位置，每個周期發布一起，確保標記可見
            self.marker_pub.publish(self.markers)
            
            # Intialize the waypoint goal
            # 初始化goal為MoveBaseGoal類型
            goal = MoveBaseGoal()
            
            # Use the map frame to define goal poses
            # 使用map的frame定義goal的frame id
            goal.target_pose.header.frame_id = 'map'
            
            # Set the time stamp to "now"
            # 設置時間戳
            goal.target_pose.header.stamp = rospy.Time.now()
            
            # Set the goal pose to the i-th waypoint
            # 設置目標位置是當前第幾個導航點
            goal.target_pose.pose = waypoints[i]
            
            # Start the robot moving toward the goal
            # 機器人移動
            self.move(goal)
            
            i += 1
        
    def move(self, goal):
            # Send the goal pose to the MoveBaseAction server
            # 把目標位置發送給MoveBaseAction的服務器
            self.move_base.send_goal(goal)
            
            # Allow 1 minute to get there
            # 設定1分鍾的時間限制
            finished_within_time = self.move_base.wait_for_result(rospy.Duration(60)) 
            
            # If we don't get there in time, abort the goal
            # 如果一分鍾之內沒有到達，放棄目標
            if not finished_within_time:
                self.move_base.cancel_goal()
                rospy.loginfo("Timed out achieving goal")
            else:
                # We made it!
                state = self.move_base.get_state()
                if state == GoalStatus.SUCCEEDED:
                    rospy.loginfo("Goal succeeded!")
                    
    def init_markers(self):
        # Set up our waypoint markers
        # 設置標記的尺寸
        marker_scale = 0.2
        marker_lifetime = 0 # 0 is forever
        marker_ns = 'waypoints'
        marker_id = 0
        marker_color = {'r': 1.0, 'g': 0.7, 'b': 1.0, 'a': 1.0}
        
        # Define a marker publisher.
        # 定義一個標記的發布者
        self.marker_pub = rospy.Publisher('waypoint_markers', Marker)
        
        # Initialize the marker points list.
        # 初始化標記點的列表
        self.markers = Marker()
        self.markers.ns = marker_ns
        self.markers.id = marker_id
        self.markers.type = Marker.SPHERE_LIST
        self.markers.action = Marker.ADD
        self.markers.lifetime = rospy.Duration(marker_lifetime)
        self.markers.scale.x = marker_scale
        self.markers.scale.y = marker_scale
        self.markers.color.r = marker_color['r']
        self.markers.color.g = marker_color['g']
        self.markers.color.b = marker_color['b']
        self.markers.color.a = marker_color['a']
        
        self.markers.header.frame_id = 'map'
        self.markers.header.stamp = rospy.Time.now()
        self.markers.points = list()
 
    def shutdown(self):
        rospy.loginfo("Stopping the robot...")
        # Cancel any active goals
        self.move_base.cancel_goal()
        rospy.sleep(2)
        # Stop the robot
        self.cmd_vel_pub.publish(Twist())
        rospy.sleep(1)
 
if __name__ == '__main__':
    try:
        MoveBaseSquare()
    except rospy.ROSInterruptException:
        rospy.loginfo("Navigation test finished.")

 但是，在實際情況中，往往需要讓機器人自動躲避障礙物。move_base包的一個強大的功能就是可以在全局規划的過程中自動躲避障礙物，而不影響全局路徑。障礙物可以是靜態的（比如牆、桌子等），也可以是動態的（比如人走過）。 現在我們嘗試在之前的正方形路徑中加入障礙物。把之前運行fake_move_base_blank_map.launch的中斷Ctrl-C掉，然后運行：

roslaunch rbx1_nav fake_move_base_obstacle.launch

然后就會看到在rviz中出現了障礙物。然后在運行之前走正方形路線的代碼：這回我們可以看到，在全局路徑規划的時候，機器人已經將障礙物繞過去了，下過如下圖：

 

在上圖中，黑色的線是障礙物，周圍淺色的橢圓形是根據配置文件中的inflation_radius參數計算出來的安全緩沖區。全局規划的路徑基本已經是最短路徑了。在仿真的過程中也可以動態重配置那四個配置文件，修改仿真參數。