ROS探索总结(十四)——move_base(路径规划)

 在上一篇的博客中,我们一起学习了ROS定位于导航的总体框架,这一篇我们主要研究其中最重要的move_base包。


           在总体框架图中可以看到,move_base提供了ROS导航的配置、运行、交互接口,它主要包括两个部分:
      (1) 全局路径规划(global planner):根据给定的目标位置进行总体路径的规划;
      (2) 本地实时规划(local planner):根据附近的障碍物进行躲避路线规划。

一、数据结构

        ROS中定义了MoveBaseActionGoal数据结构来存储导航的目标位置数据,其中最重要的就是位置坐标(position)和方向(orientation)。
[plain]  view plain copy
  1. rosmsg show MoveBaseActionGoal  
  2.   
  3. [move_base_msgs/MoveBaseActionGoal]:  
  4. std_msgs/Header header  
  5.   uint32 seq  
  6.   time stamp  
  7.   string frame_id  
  8. actionlib_msgs/GoalID goal_id  
  9.   time stamp  
  10.   string id  
  11. move_base_msgs/MoveBaseGoal goal  
  12.   geometry_msgs/PoseStamped target_pose  
  13.     std_msgs/Header header  
  14.       uint32 seq  
  15.       time stamp  
  16.       string frame_id  
  17.     geometry_msgs/Pose pose  
  18.       geometry_msgs/Point position  
  19.         float64 x  
  20.         float64 y  
  21.         float64 z  
  22.       geometry_msgs/Quaternion orientation  
  23.         float64 x  
  24.         float64 y  
  25.         float64 z  
  26.         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*算法。
        具体见:http://www.ros.org/wiki/navfn?distro=fuerte

四、本地实时规划(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) 重复上面过程。
      具体参见:http://www.ros.org/wiki/base_local_planner?distro=groovy

五、ArbotiX仿真——手动设定目标

        在这一步,我们暂时使用空白地图(blank_map.pgm),就在空地上进行无障碍仿真。
        首先运行ArbotiX节点,并且加载机器人的URDF文件。
[plain]  view plain copy
  1. roslaunch rbx1_bringup fake_turtlebot.launch  

         然后运行move_base和加载空白地图的launch文件(fake_move_base_blank_map.launch):
[plain]  view plain copy
  1. roslaunch rbx1_nav fake_move_base_blank_map.launch  

        该文件的具体内容如下:
[plain]  view plain copy
  1. <launch>  
  2.   <!-- Run the map server with a blank map -->  
  3.   <node name="map_server" pkg="map_server" type="map_server" args="$(find rbx1_nav)/maps/blank_map.yaml"/>  
  4.       
  5.   <include file="$(find rbx1_nav)/launch/fake_move_base.launch" />  
  6.   
  7.   <!-- Run a static transform between /odom and /map -->  
  8.   <node pkg="tf" type="static_transform_publisher" name="odom_map_broadcaster" args="0 0 0 0 0 0 /map /odom 100" />  
  9.   
  10. </launch>  

        其中调用了fake_move_base.launch文件,是运行move_base节点并进行参数配置。
        然后调用rviz就可以看到机器人了。
[plain]  view plain copy
  1. rosrun rviz rviz -d `rospack find rbx1_nav`/nav_fuerte.vcg  

        
        我们先以1m的速度进行一下测试:
        让机器人前进一米:
[plain]  view plain copy
  1. rostopic pub /move_base_simple/goal geometry_msgs/PoseStamped \  
  2. '{ header: { frame_id: "base_link" }, pose: { position: { x: 1.0, y: 0, z: 0 }, orientation: { x: 0, y: 0, z: 0, w: 1 } } }'  

        让机器人后退一米,回到原来的位置:
[plain]  view plain copy
  1. rostopic pub /move_base_simple/goal geometry_msgs/PoseStamped \  
  2. '{ 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按键,把所有的箭头清除。接着运行走正方形路径的代码:
[plain]  view plain copy
  1. rosrun rbx1_nav move_base_square.py  

        在rviz中可以看到:

        四个顶角的粉色圆盘就是我们设定的位置,正方形比较规则,可见定位还是比较准确的。然我们先来分析一下走正方形路线的代码:
[python]  view plain copy
  1. #!/usr/bin/env python  
  2. import roslib; roslib.load_manifest('rbx1_nav')  
  3. import rospy  
  4. import actionlib  
  5. from actionlib_msgs.msg import *  
  6. from geometry_msgs.msg import Pose, Point, Quaternion, Twist  
  7. from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal  
  8. from tf.transformations import quaternion_from_euler  
  9. from visualization_msgs.msg import Marker  
  10. from math import radians, pi  
  11.   
  12. class MoveBaseSquare():  
  13.     def __init__(self):  
  14.         rospy.init_node('nav_test', anonymous=False)  
  15.           
  16.         rospy.on_shutdown(self.shutdown)  
  17.           
  18.         # How big is the square we want the robot to navigate?  
  19.         # 设定正方形的尺寸,默认是一米  
  20.         square_size = rospy.get_param("~square_size"1.0# meters  
  21.           
  22.         # Create a list to hold the target quaternions (orientations)  
  23.         # 创建一个列表,保存目标的角度数据  
  24.         quaternions = list()  
  25.           
  26.         # First define the corner orientations as Euler angles  
  27.         # 定义四个顶角处机器人的方向角度(Euler angles:http://zh.wikipedia.org/wiki/%E6%AC%A7%E6%8B%89%E8%A7%92)  
  28.         euler_angles = (pi/2, pi, 3*pi/20)  
  29.           
  30.         # Then convert the angles to quaternions  
  31.         # 将上面的Euler angles转换成Quaternion的格式  
  32.         for angle in euler_angles:  
  33.             q_angle = quaternion_from_euler(00, angle, axes='sxyz')  
  34.             q = Quaternion(*q_angle)  
  35.             quaternions.append(q)  
  36.           
  37.         # Create a list to hold the waypoint poses  
  38.         # 创建一个列表存储导航点的位置  
  39.         waypoints = list()  
  40.           
  41.         # Append each of the four waypoints to the list.  Each waypoint  
  42.         # is a pose consisting of a position and orientation in the map frame.  
  43.         # 创建四个导航点的位置(角度和坐标位置)  
  44.         waypoints.append(Pose(Point(square_size, 0.00.0), quaternions[0]))  
  45.         waypoints.append(Pose(Point(square_size, square_size, 0.0), quaternions[1]))  
  46.         waypoints.append(Pose(Point(0.0, square_size, 0.0), quaternions[2]))  
  47.         waypoints.append(Pose(Point(0.00.00.0), quaternions[3]))  
  48.           
  49.         # Initialize the visualization markers for RViz  
  50.         # 初始化可视化标记  
  51.         self.init_markers()  
  52.           
  53.         # Set a visualization marker at each waypoint   
  54.         # 给每个定点的导航点一个可视化标记(就是rviz中看到的粉色圆盘标记)  
  55.         for waypoint in waypoints:             
  56.             p = Point()  
  57.             p = waypoint.position  
  58.             self.markers.points.append(p)  
  59.               
  60.         # Publisher to manually control the robot (e.g. to stop it)  
  61.         # 发布TWist消息控制机器人  
  62.         self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist)  
  63.           
  64.         # Subscribe to the move_base action server  
  65.         # 订阅move_base服务器的消息  
  66.         self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)  
  67.           
  68.         rospy.loginfo("Waiting for move_base action server...")  
  69.           
  70.         # Wait 60 seconds for the action server to become available  
  71.         # 等待move_base服务器建立  
  72.         self.move_base.wait_for_server(rospy.Duration(60))  
  73.           
  74.         rospy.loginfo("Connected to move base server")  
  75.         rospy.loginfo("Starting navigation test")  
  76.           
  77.         # Initialize a counter to track waypoints  
  78.         # 初始化一个计数器,记录到达的顶点号  
  79.         i = 0  
  80.           
  81.         # Cycle through the four waypoints  
  82.         # 主循环,环绕通过四个定点  
  83.         while i < 4 and not rospy.is_shutdown():  
  84.             # Update the marker display  
  85.             # 发布标记指示四个目标的位置,每个周期发布一起,确保标记可见  
  86.             self.marker_pub.publish(self.markers)  
  87.               
  88.             # Intialize the waypoint goal  
  89.             # 初始化goal为MoveBaseGoal类型  
  90.             goal = MoveBaseGoal()  
  91.               
  92.             # Use the map frame to define goal poses  
  93.             # 使用map的frame定义goal的frame id  
  94.             goal.target_pose.header.frame_id = 'map'  
  95.               
  96.             # Set the time stamp to "now"  
  97.             # 设置时间戳  
  98.             goal.target_pose.header.stamp = rospy.Time.now()  
  99.               
  100.             # Set the goal pose to the i-th waypoint  
  101.             # 设置目标位置是当前第几个导航点  
  102.             goal.target_pose.pose = waypoints[i]  
  103.               
  104.             # Start the robot moving toward the goal  
  105.             # 机器人移动  
  106.             self.move(goal)  
  107.               
  108.             i += 1  
  109.           
  110.     def move(self, goal):  
  111.             # Send the goal pose to the MoveBaseAction server  
  112.             # 把目标位置发送给MoveBaseAction的服务器  
  113.             self.move_base.send_goal(goal)  
  114.               
  115.             # Allow 1 minute to get there  
  116.             # 设定1分钟的时间限制  
  117.             finished_within_time = self.move_base.wait_for_result(rospy.Duration(60))   
  118.               
  119.             # If we don't get there in time, abort the goal  
  120.             # 如果一分钟之内没有到达,放弃目标  
  121.             if not finished_within_time:  
  122.                 self.move_base.cancel_goal()  
  123.                 rospy.loginfo("Timed out achieving goal")  
  124.             else:  
  125.                 # We made it!  
  126.                 state = self.move_base.get_state()  
  127.                 if state == GoalStatus.SUCCEEDED:  
  128.                     rospy.loginfo("Goal succeeded!")  
  129.                       
  130.     def init_markers(self):  
  131.         # Set up our waypoint markers  
  132.         # 设置标记的尺寸  
  133.         marker_scale = 0.2  
  134.         marker_lifetime = 0 # 0 is forever  
  135.         marker_ns = 'waypoints'  
  136.         marker_id = 0  
  137.         marker_color = {'r'1.0'g'0.7'b'1.0'a'1.0}  
  138.           
  139.         # Define a marker publisher.  
  140.         # 定义一个标记的发布者  
  141.         self.marker_pub = rospy.Publisher('waypoint_markers', Marker)  
  142.           
  143.         # Initialize the marker points list.  
  144.         # 初始化标记点的列表  
  145.         self.markers = Marker()  
  146.         self.markers.ns = marker_ns  
  147.         self.markers.id = marker_id  
  148.         self.markers.type = Marker.SPHERE_LIST  
  149.         self.markers.action = Marker.ADD  
  150.         self.markers.lifetime = rospy.Duration(marker_lifetime)  
  151.         self.markers.scale.x = marker_scale  
  152.         self.markers.scale.y = marker_scale  
  153.         self.markers.color.r = marker_color['r']  
  154.         self.markers.color.g = marker_color['g']  
  155.         self.markers.color.b = marker_color['b']  
  156.         self.markers.color.a = marker_color['a']  
  157.           
  158.         self.markers.header.frame_id = 'map'  
  159.         self.markers.header.stamp = rospy.Time.now()  
  160.         self.markers.points = list()  
  161.   
  162.     def shutdown(self):  
  163.         rospy.loginfo("Stopping the robot...")  
  164.         # Cancel any active goals  
  165.         self.move_base.cancel_goal()  
  166.         rospy.sleep(2)  
  167.         # Stop the robot  
  168.         self.cmd_vel_pub.publish(Twist())  
  169.         rospy.sleep(1)  
  170.   
  171. if __name__ == '__main__':  
  172.     try:  
  173.         MoveBaseSquare()  
  174.     except rospy.ROSInterruptException:  
  175.         rospy.loginfo("Navigation test finished.")  

        但是,在实际情况中,往往需要让机器人自动躲避障碍物。move_base包的一个强大的功能就是可以在全局规划的过程中自动躲避障碍物,而不影响全局路径。障碍物可以是静态的(比如墙、桌子等),也可以是动态的(比如人走过)。
        现在我们尝试在之前的正方形路径中加入障碍物。把之前运行fake_move_base_blank_map.launch的中断Ctrl-C掉,然后运行:
[plain]  view plain copy
  1. roslaunch rbx1_nav fake_move_base_obstacle.launch  


        然后就会看到在rviz中出现了障碍物。然后在运行之前走正方形路线的代码:
[plain]  view plain copy
  1. rosrun rbx1_nav move_base_square.py   


        这回我们可以看到,在全局路径规划的时候,机器人已经将障碍物绕过去了,下过如下图:
        在上图中,黑色的线是障碍物,周围浅色的椭圆形是根据配置文件中的inflation_radius参数计算出来的安全缓冲区。全局规划的路径基本已经是最短路径了。在仿真的过程中也可以动态重配置那四个配置文件,修改仿真参数。

----------------------------------------------------------------

欢迎大家转载我的文章。

转载请注明:转自古-月

http://blog.csdn.net/hcx25909

欢迎继续关注我的博客

  • 0
    点赞
  • 3
    收藏
    觉得还不错? 一键收藏
  • 0
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值