ROS探索总结(十五)——amcl(导航与定位)

  • 内容
  • 评论
  • 相关

        在理解了move_base的基础上,我们开始机器人的定位与导航。gmaping包是用来生成地图的,需要使用实际的机器人获取激光或者深度数据,所以我们先在已有的地图上进行导航与定位的仿真。

        amcl是移动机器人二维环境下的概率定位系统。它实现了自适应(或kld采样)的蒙特卡罗定位方法,其中针对已有的地图使用粒子滤波器跟踪一个机器人的姿态。

一、测试

        首先运行机器人节点:
        roslaunch rbx1_bringup fake_turtlebot.launch 
        然后运行amcl节点,使用测试地图:   
        roslaunch rbx1_nav fake_amcl.launch map:=test_map.yaml 
        可以看一下fake_amcl.launch这个文件的内容:

  1. <launch>  
  2.   <!-- Set the name of the map yaml file: can be overridden on the command line. -->  
  3.   <arg name="map" default="test_map.yaml" />  
  4.   <!-- Run the map server with the desired map -->  
  5.   <node name="map_server" pkg="map_server" type="map_server" args="$(find rbx1_nav)/maps/$(arg map)"/>  
  6.   <!-- The move_base node -->  
  7.   <include file="$(find rbx1_nav)/launch/fake_move_base.launch" />  
  8.  
  9.   <!-- Run fake localization compatible with AMCL output -->  
  10.   <node pkg="fake_localization" type="fake_localization"  name="fake_localization" output="screen" />  
  11.   <!-- For fake localization we need static transforms between /odom and /map and /map and /world -->  
  12.   <node pkg="tf" type="static_transform_publisher" name="odom_map_broadcaster"   
  13. args="0 0 0 0 0 0 /odom /map 100" />  
  14. </launch>

         这个lanuch文件作用是加载地图,并且调用fake_move_base.launch文件打开move_base节点并加载配置文件,最后运行amcl。
         然后运行rviz:
         rosrun rviz rviz -d `rospack find rbx1_nav`/nav_fuerte.vcg 
         这时在rvoiz中就应该显示出了地图和机器人:

        现在就可以通过rviz在地图上选择目标位置了,然后就会看到机器人自动规划出一条全局路径,并且导航前进:

二、自主导航

        在实际应用中,我们往往希望机器人能够自主进行定位和导航,不需要认为的干预,这样才更智能化。在这一节的测试中,我们让目标点在地图中随机生成,然后机器人自动导航到达目标。
        这里运行的主要文件是:fake_nav_test.launch,让我们来看一下这个文件的内容:  

  1. <launch>  
  2.  
  3.   <param name="use_sim_time" value="false" />  
  4.  
  5.   <!-- Start the ArbotiX controller -->  
  6.   <include file="$(find rbx1_bringup)/launch/fake_turtlebot.launch" />  
  7.  
  8.   <!-- Run the map server with the desired map -->  
  9.   <node name="map_server" pkg="map_server" type="map_server" args="$(find rbx1_nav)/maps/test_map.yaml"/>  
  10.  
  11.   <!-- The move_base node -->  
  12.   <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">  
  13.     <rosparam file="$(find rbx1_nav)/config/fake/costmap_common_params.yaml" command="load" ns="global_costmap" />  
  14.     <rosparam file="$(find rbx1_nav)/config/fake/costmap_common_params.yaml" command="load" ns="local_costmap" />  
  15.     <rosparam file="$(find rbx1_nav)/config/fake/local_costmap_params.yaml" command="load" />  
  16.     <rosparam file="$(find rbx1_nav)/config/fake/global_costmap_params.yaml" command="load" />  
  17.     <rosparam file="$(find rbx1_nav)/config/fake/base_local_planner_params.yaml" command="load" />  
  18.     <rosparam file="$(find rbx1_nav)/config/nav_test_params.yaml" command="load" />  
  19.   </node>  
  20.  
  21.   <!-- Run fake localization compatible with AMCL output -->  
  22.   <node pkg="fake_localization" type="fake_localization" name="fake_localization" output="screen" />  
  23.  
  24.   <!-- For fake localization we need static transform between /odom and /map -->  
  25.   <node pkg="tf" type="static_transform_publisher" name="map_odom_broadcaster" args="0 0 0 0 0 0 /map /odom 100" />  
  26.  
  27.   <!-- Start the navigation test -->  
  28.   <node pkg="rbx1_nav" type="nav_test.py" name="nav_test" output="screen">  
  29.     <param name="rest_time" value="1" />  
  30.     <param name="fake_test" value="true" />  
  31.   </node>  
  32.  
  33. </launch>


 

        这个lanuch的功能比较多:
      (1) 加载机器人驱动
      (2) 加载地图
      (3) 启动move_base节点,并且加载配置文件
      (4) 运行amcl节点
      (5) 然后加载nav_test.py执行文件,进行随机导航
        相当于是把我们之前实验中的多个lanuch文件合成了一个文件。
        现在开始进行测试,先运行ROS:

        roscore

        然后我们运行一个监控的窗口,可以实时看到机器人发送的数据:
        rxconsole 

        接着运行lanuch文件,并且在一个新的终端中打开rviz:

  1. roslaunch rbx1_nav fake_nav_test.launch  
  2. (打开新终端)  
  3. rosrun rviz rviz -d `rospack find rbx1_nav`/nav_test_fuerte.vcg


        好了,此时就看到了机器人已经放在地图当中了。然后我们点击rviz上的“2D Pose Estimate”按键,然后左键在机器人上单击,让绿色的箭头和黄色的箭头重合,机器人就开始随机选择目标导航了:
       

         在监控窗口中,我们可以看到机器人发送的状态信息:


        其中包括距离信息、状态信息、目标的编号、成功率和速度等信息。

三、导航代码分析

  1. #!/usr/bin/env python  
  2.  
  3. import roslib; roslib.load_manifest('rbx1_nav')  
  4. import rospy  
  5. import actionlib  
  6. from actionlib_msgs.msg import *  
  7. from geometry_msgs.msg import Pose, PoseWithCovarianceStamped, Point, Quaternion, Twist  
  8. from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal  
  9. from random import sample  
  10. from math import pow, sqrt  
  11.  
  12. class NavTest():  
  13.     def __init__(self):  
  14.         rospy.init_node('nav_test', anonymous=True)  
  15.  
  16.         rospy.on_shutdown(self.shutdown)  
  17.  
  18.         # How long in seconds should the robot pause at each location?  
  19.         # 在每个目标位置暂停的时间  
  20.         self.rest_time = rospy.get_param("~rest_time", 10)  
  21.  
  22.         # Are we running in the fake simulator?  
  23.         # 是否仿真?  
  24.         self.fake_test = rospy.get_param("~fake_test", False)  
  25.  
  26.         # Goal state return values  
  27.         # 到达目标的状态  
  28.         goal_states = ['PENDING', 'ACTIVE', 'PREEMPTED',   
  29.                        'SUCCEEDED', 'ABORTED', 'REJECTED',  
  30.                        'PREEMPTING', 'RECALLING', 'RECALLED',  
  31.                        'LOST']  
  32.  
  33.         # Set up the goal locations. Poses are defined in the map frame.    
  34.         # An easy way to find the pose coordinates is to point-and-click  
  35.         # Nav Goals in RViz when running in the simulator.  
  36.         # Pose coordinates are then displayed in the terminal  
  37.         # that was used to launch RViz.  
  38.         # 设置目标点的位置  
  39.         # 如果想要获得某一点的坐标,在rviz中点击 2D Nav Goal 按键,然后单机地图中一点  
  40.         # 在终端中就会看到坐标信息  
  41.         locations = dict()  
  42.  
  43.         locations['hall_foyer'] = Pose(Point(0.643, 4.720, 0.000), Quaternion(0.000, 0.000, 0.223, 0.975))  
  44.         locations['hall_kitchen'] = Pose(Point(-1.994, 4.382, 0.000), Quaternion(0.000, 0.000, -0.670, 0.743))  
  45.         locations['hall_bedroom'] = Pose(Point(-3.719, 4.401, 0.000), Quaternion(0.000, 0.000, 0.733, 0.680))  
  46.         locations['living_room_1'] = Pose(Point(0.720, 2.229, 0.000), Quaternion(0.000, 0.000, 0.786, 0.618))  
  47.         locations['living_room_2'] = Pose(Point(1.471, 1.007, 0.000), Quaternion(0.000, 0.000, 0.480, 0.877))  
  48.         locations['dining_room_1'] = Pose(Point(-0.861, -0.019, 0.000), Quaternion(0.000, 0.000, 0.892, -0.451))  
  49.  
  50.         # Publisher to manually control the robot (e.g. to stop it)  
  51.         # 发布控制机器人的消息  
  52.         self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist)  
  53.  
  54.         # Subscribe to the move_base action server  
  55.         # 订阅move_base服务器的消息  
  56.         self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)  
  57.  
  58.         rospy.loginfo("Waiting for move_base action server...")  
  59.  
  60.         # Wait 60 seconds for the action server to become available  
  61.         # 60s等待时间限制  
  62.         self.move_base.wait_for_server(rospy.Duration(60))  
  63.  
  64.         rospy.loginfo("Connected to move base server")  
  65.  
  66.         # A variable to hold the initial pose of the robot to be set by   
  67.         # the user in RViz  
  68.         # 保存机器人的在rviz中的初始位置  
  69.         initial_pose = PoseWithCovarianceStamped()  
  70.  
  71.         # Variables to keep track of success rate, running time,  
  72.         # and distance traveled  
  73.         # 保存成功率、运行时间、和距离的变量  
  74.         n_locations = len(locations)  
  75.         n_goals = 0  
  76.         n_successes = 0  
  77.         i = n_locations  
  78.         distance_traveled = 0  
  79.         start_time = rospy.Time.now()  
  80.         running_time = 0  
  81.         location = ""  
  82.         last_location = ""  
  83.  
  84.         # Get the initial pose from the user  
  85.         # 获取初始位置(仿真中可以不需要)  
  86.         rospy.loginfo("*** Click the 2D Pose Estimate button in RViz to set the robot's initial pose...")  
  87.         rospy.wait_for_message('initialpose', PoseWithCovarianceStamped)  
  88.         self.last_location = Pose()  
  89.         rospy.Subscriber('initialpose', PoseWithCovarianceStamped, self.update_initial_pose)  
  90.  
  91.         # Make sure we have the initial pose  
  92.         # 确保有初始位置  
  93.         while initial_pose.header.stamp == "":  
  94.             rospy.sleep(1)  
  95.  
  96.         rospy.loginfo("Starting navigation test")  
  97.  
  98.         # Begin the main loop and run through a sequence of locations  
  99.         # 开始主循环,随机导航  
  100.         while not rospy.is_shutdown():  
  101.             # If we've gone through the current sequence,  
  102.             # start with a new random sequence  
  103.             # 如果已经走完了所有点,再重新开始排序  
  104.             if i == n_locations:  
  105.                 i = 0  
  106.                 sequence = sample(locations, n_locations)  
  107.                 # Skip over first location if it is the same as  
  108.                 # the last location  
  109.                 # 如果最后一个点和第一个点相同,则跳过  
  110.                 if sequence[0] == last_location:  
  111.                     i = 1  
  112.  
  113.             # Get the next location in the current sequence  
  114.             # 在当前的排序中获取下一个目标点  
  115.             location = sequence[i]  
  116.  
  117.             # Keep track of the distance traveled.  
  118.             # Use updated initial pose if available.  
  119.             # 跟踪形式距离  
  120.             # 使用更新的初始位置  
  121.             if initial_pose.header.stamp == "":  
  122.                 distance = sqrt(pow(locations[location].position.x -   
  123.                                     locations[last_location].position.x, 2) +  
  124.                                 pow(locations[location].position.y -   
  125.                                     locations[last_location].position.y, 2))  
  126.             else:  
  127.                 rospy.loginfo("Updating current pose.")  
  128.                 distance = sqrt(pow(locations[location].position.x -   
  129.                                     initial_pose.pose.pose.position.x, 2) +  
  130.                                 pow(locations[location].position.y -   
  131.                                     initial_pose.pose.pose.position.y, 2))  
  132.                 initial_pose.header.stamp = ""  
  133.  
  134.             # Store the last location for distance calculations  
  135.             # 存储上一次的位置,计算距离  
  136.             last_location = location  
  137.  
  138.             # Increment the counters  
  139.             # 计数器加1  
  140.             i += 1  
  141.             n_goals += 1  
  142.  
  143.             # Set up the next goal location  
  144.             # 设定下一个目标点  
  145.             self.goal = MoveBaseGoal()  
  146.             self.goal.target_pose.pose = locations[location]  
  147.             self.goal.target_pose.header.frame_id = 'map'  
  148.             self.goal.target_pose.header.stamp = rospy.Time.now()  
  149.  
  150.             # Let the user know where the robot is going next  
  151.             # 让用户知道下一个位置  
  152.             rospy.loginfo("Going to: " + str(location))  
  153.  
  154.             # Start the robot toward the next location  
  155.             # 向下一个位置进发  
  156.             self.move_base.send_goal(self.goal)  
  157.  
  158.             # Allow 5 minutes to get there  
  159.             # 五分钟时间限制  
  160.             finished_within_time = self.move_base.wait_for_result(rospy.Duration(300))   
  161.  
  162.             # Check for success or failure  
  163.             # 查看是否成功到达  
  164.             if not finished_within_time:  
  165.                 self.move_base.cancel_goal()  
  166.                 rospy.loginfo("Timed out achieving goal")  
  167.             else:  
  168.                 state = self.move_base.get_state()  
  169.                 if state == GoalStatus.SUCCEEDED:  
  170.                     rospy.loginfo("Goal succeeded!")  
  171.                     n_successes += 1  
  172.                     distance_traveled += distance  
  173.                     rospy.loginfo("State:" + str(state))  
  174.                 else:  
  175.                   rospy.loginfo("Goal failed with error code: " + str(goal_states[state]))  
  176.  
  177.             # How long have we been running?  
  178.             # 运行所用时间  
  179.             running_time = rospy.Time.now() - start_time  
  180.             running_time = running_time.secs / 60.0  
  181.  
  182.             # Print a summary success/failure, distance traveled and time elapsed  
  183.             # 输出本次导航的所有信息  
  184.             rospy.loginfo("Success so far: " + str(n_successes) + "/" +   
  185.                           str(n_goals) + " = " +   
  186.                           str(100 * n_successes/n_goals) + "%")  
  187.             rospy.loginfo("Running time: " + str(trunc(running_time, 1)) +   
  188.                           " min Distance: " + str(trunc(distance_traveled, 1)) + " m")  
  189.             rospy.sleep(self.rest_time)  
  190.  
  191.     def update_initial_pose(self, initial_pose):  
  192.         self.initial_pose = initial_pose  
  193.  
  194.     def shutdown(self):  
  195.         rospy.loginfo("Stopping the robot...")  
  196.         self.move_base.cancel_goal()  
  197.         rospy.sleep(2)  
  198.         self.cmd_vel_pub.publish(Twist())  
  199.         rospy.sleep(1)  
  200.  
  201. def trunc(f, n):  
  202.     # Truncates/pads a float f to n decimal places without rounding  
  203.     slen = len('%.*f' % (n, f))  
  204.     return float(str(f)[:slen])  
  205.  
  206. if __name__ == '__main__':  
  207.     try:  
  208.         NavTest()  
  209.         rospy.spin()  
  210.     except rospy.ROSInterruptException:  
  211.         rospy.loginfo("AMCL navigation test finished.")



原创文章,转载请注明: 转载自古月居

本文链接地址: ROS探索总结(十五)——amcl(导航与定位)

微信 OR 支付宝 扫描二维码
为本文作者 打个赏
pay_weixinpay_weixin

评论

27条评论
  1. Gravatar 头像

    Peggy 回复

    我的地图是激光雷达扫描获取的,我要怎么进行这一步导航和定位呢?

    • 古月

      古月 回复

      @Peggy 很多书上有现成的案例,在launch文件里加载自己的地图即可

  2. Gravatar 头像

    齐欢 回复

    你好!请问怎么调低turtlebot在自主导航时的行驶速度,感觉有点太 😉 快

    • Gravatar 头像

      齐欢 回复

      @齐欢 已经知道了不用了

  3. Gravatar 头像

    Dasrio 回复

    博主你好,我想问下rviz里面的map,global map,local map有什么区别,为什么我用map_server载入我自己的地图之后,map中的地图是官方demo地图,而global map才是我自己的地图呢?

  4. Gravatar 头像

    小咔星 回复

    我也是在做绘制地图,不过要求在绘制过程中检测到规定的图片后在该位置画出一个图形进行标记,我的思路是检测到图片后发布一个话题,然后接收后在地图中小车当前位置标记,不过我不知道怎么得到小车在地图中当前的坐标,博主能指点一下吗

  5. Gravatar 头像

    hoshi 回复

    大神,请问这种昂贵的激光传感器还有什么替代品么,因为还只是学生,所以经费有限。如果实在不能用别的替代,可以用ros手动绘制地图么?谢谢大神~期待您的回复

    • 古月

      古月 回复

      @hoshi 可以用Kinect,几百块钱,在淘宝上就可以买到。ROS生成的栅格地图也是可以手动绘制的,用图片编辑软件就可以。另外,也可以使用gazebo仿真激光传感器,零成本

  6. Gravatar 头像

    周志敏 回复

    古大神你好,我通过sudo apt-get install ros-indigo-navigation安装了move_base, gmapping和map_server等一系列跟SLAM相关的安装包,不过不知道为什么我这边的map_server安装包有问题,我输入rosrun map_server map_server的时候提示我couldn't find executable named map_server,我在launch file里面想要launch也不行,提示我can't locate node map_server in package map_server。ps:我用roscd map_server是可以进入到map_server包的。这个问题卡住我好久了。。古大神你知道怎么回事吗?

    • Gravatar 头像

      周志敏 回复

      @周志敏 ps:我也是在看ros by example这个教程,然后看到第八章,要输入指令roslaunch rbx1_nav fake_move_base_blank_map.launch的时候提示出错

    • Gravatar 头像

      ncnynl 回复

      @周志敏 rosrun map_server 你是保存地图的话,应该是map_saver

  7. Gravatar 头像

    柚子Betty 回复

    博主,你好,最近开始接触SLAM,进而知道ROS,想问下ROS里面的explore package能不能进行自主探索,而不是人为操作机器人进行地图构建?

    • 古月

      古月 回复

      @柚子Betty 可以实现,机器人可以在SLAM的过程中自主避障,可以参考ROS wiki中的huskey机器人仿真,有实现类似的功能

  8. Gravatar 头像

    浪迹江北 回复

    博主您好,我最近在看您的博客文章。环境Ubuntu14.04 + ROS (Indigo)。运行 roslaunch rbx1_nav fake_amcl.launch map:=test_map.yaml 报错
    [move_base-2] process has died [pid 3741, exit code -11, cmd /home/ntrqz/rbe_ws/devel/lib/move_base/move_base __name:=move_base __log:=/home/ntrqz/.ros/log/9ccff348-2928-11e6-a5ce-20689de2cfb9/move_base-2.log].
    log file: /home/ntrqz/.ros/log/9ccff348-2928-11e6-a5ce-20689de2cfb9/move_base-2*.log
    这种错误要怎么弄,求博主点拨啊。

  9. Gravatar 头像

    黄建辉 回复

    博主你好
    运行 roslaunch rbx1_nav fake_amcl.launch map:=test_map.yaml
    出错
    [move_base-2] process has died [pid 31982, exit code -11, cmd /home/ntrqz/rbe_ws/devel/lib/move_base/move_base __name:=move_base __log:=/home/ntrqz/.ros/log/e6b53aec-289f-11e6-928f-20689de2cfb9/move_base-2.log].
    这个是什么情况?
    log file: /home/ntrqz/.ros/log/e6b53aec-289f-11e6-928f-20689de2cfb9/move_base-2*.log

  10. Gravatar 头像

    QuanQhow 回复

    博主你好,我在用Learning ROS for Robotics Programming 第二版书的代码创建地图时候运行rosrun map_server map_saver -f map
    出现错误:
    Waiting for the map
    而且在rviz中显示warn:no map received.我的QQ841691229,可以加个好友一起交流下吗?谢谢!

    • 古月

      古月 回复

      @QuanQhow 这个命令是用来保存建立好的地图,你的地图已经建立完毕了么?

  11. Gravatar 头像

    郑锐钊 回复

    您好,我是最近开始学习ROS的,用的是indigo的版本。我看了您的教程,收获颇丰。在这里向请教一个问题。
    我是自己兴趣学习的ROS,所以按照《ROS机器人程序设计》这本书里的教程和参考网上的各种资料在学习,但是在学习的到地图构建部分,需呀奥用到激光雷达传感器,如果不买设备,只是使用仿真的话,能够在仿真软件中获取到数据吗?
    我使用的是gazebo仿真,激光雷达能够仿真出来,但是我不清楚是否能够正常收集到数据,还有如何在rviz中得到地图?

    • 古月

      古月 回复

      @郑锐钊 可以的,在gazebo中可以仿真激光传感器,和真实设备的使用一样。使用gamapping可以在rviz中看到地图。

      • Gravatar 头像

        郑锐钊 回复

        @古月 能否出一个简单的教程呢?

        • 古月

          古月 回复

          @郑锐钊 后续有时间可能会总结一下这部分内容,你可以先参考一下《Mastering ROS for RoboticsProgramming》

          • Gravatar 头像

            郑锐钊 回复

            @古月 恩恩。非常感谢!再请教一个问题,如果我的机器人在启动时加载进一张地图,那我如何确定机器人启动时是位于地图中的哪个位置?关于这方面的,是否有哪些教程或者方法推荐。

            • 古月

              古月 回复

              @郑锐钊 每个地图都有一个配置文件,里边可以设置机器人的初始位置,默认就是建图时的原点位置

发表评论

电子邮件地址不会被公开。 必填项已用*标注