ROS探索总结-15.amcl(导航与定位)

ROS1/一代机器人系统 少儿编程 792浏览 0评论
ROS探索总结

本文转载自古月居,原作者古月,原文链接:https://www.guyuehome.com/273。
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这个文件的内容:

<launch>  

  <!-- Set the name of the map yaml file: can be overridden on the command line. -->  

  <arg name="map" default="test_map.yaml" />  

  <!-- Run the map server with the desired map -->  

  <node name="map_server" pkg="map_server" type="map_server" args="$(find rbx1_nav)/maps/$(arg map)"/>  

  <!-- The move_base node -->  

  <include file="$(find rbx1_nav)/launch/fake_move_base.launch" />  

 

  <!-- Run fake localization compatible with AMCL output -->  

  <node pkg="fake_localization" type="fake_localization"  name="fake_localization" output="screen" />  

  <!-- For fake localization we need static transforms between /odom and /map and /map and /world -->  

  <node pkg="tf" type="static_transform_publisher" name="odom_map_broadcaster"   

args="0 0 0 0 0 0 /odom /map 100" />  

</launch>

这个lanuch文件作用是加载地图,并且调用fake_move_base.launch文件打开move_base节点并加载配置文件,最后运行amcl。

然后运行rviz:

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

注意:对于indigo版本使用:

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

这时在rviz中就应该显示出了地图和机器人:
ROS探索总结-15.amcl(导航与定位)

现在就可以通过rviz在地图上选择目标位置了,然后就会看到机器人自动规划出一条全局路径,并且导航前进:
ROS探索总结-15.amcl(导航与定位)

二、自主导航

在实际应用中,我们往往希望机器人能够自主进行定位和导航,不需要认为的干预,这样才更智能化。在这一节的测试中,我们让目标点在地图中随机生成,然后机器人自动导航到达目标。

这里运行的主要文件是:fake_nav_test.launch,让我们来看一下这个文件的内容:

<launch>  
  <param name="use_sim_time" value="false" />  

 

  <!-- Start the ArbotiX controller -->  

  <include file="$(find rbx1_bringup)/launch/fake_turtlebot.launch" />  

 

  <!-- Run the map server with the desired map -->  

  <node name="map_server" pkg="map_server" type="map_server" args="$(find rbx1_nav)/maps/test_map.yaml"/>  

 

  <!-- The move_base node -->  

  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">  

    <rosparam file="$(find rbx1_nav)/config/fake/costmap_common_params.yaml" command="load" ns="global_costmap" />  

    <rosparam file="$(find rbx1_nav)/config/fake/costmap_common_params.yaml" command="load" ns="local_costmap" />  

    <rosparam file="$(find rbx1_nav)/config/fake/local_costmap_params.yaml" command="load" />  

    <rosparam file="$(find rbx1_nav)/config/fake/global_costmap_params.yaml" command="load" />  

    <rosparam file="$(find rbx1_nav)/config/fake/base_local_planner_params.yaml" command="load" />  

    <rosparam file="$(find rbx1_nav)/config/nav_test_params.yaml" command="load" />  

  </node>  

 

  <!-- Run fake localization compatible with AMCL output -->  

  <node pkg="fake_localization" type="fake_localization" name="fake_localization" output="screen" />  

 

  <!-- For fake localization we need static transform between /odom and /map -->  

  <node pkg="tf" type="static_transform_publisher" name="map_odom_broadcaster" args="0 0 0 0 0 0 /map /odom 100" />  

 

  <!-- Start the navigation test -->  

  <node pkg="rbx1_nav" type="nav_test.py" name="nav_test" output="screen">  

    <param name="rest_time" value="1" />  

    <param name="fake_test" value="true" />  

  </node>  

 

</launch>

这个lanuch的功能比较多:

(1) 加载机器人驱动
(2) 加载地图
(3) 启动move_base节点,并且加载配置文件
(4) 运行amcl节点
(5) 然后加载nav_test.py执行文件,进行随机导航

相当于是把我们之前实验中的多个lanuch文件合成了一个文件。

现在开始进行测试,先运行ROS:

roscore

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

rxconsole 

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

roslaunch rbx1_nav fake_nav_test.launch  

(打开新终端)

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

注意:对于indigo版本使用:

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

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

在监控窗口中,我们可以看到机器人发送的状态信息:
ROS探索总结-15.amcl(导航与定位)

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

三、导航代码分析

#!/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, PoseWithCovarianceStamped, Point, Quaternion, Twist  

from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal  

from random import sample  

from math import pow, sqrt  

 

class NavTest():  

    def __init__(self):  

        rospy.init_node('nav_test', anonymous=True)  

 

        rospy.on_shutdown(self.shutdown)  

 

        # How long in seconds should the robot pause at each location?  

        # 在每个目标位置暂停的时间  

        self.rest_time = rospy.get_param("~rest_time", 10)  

 

        # Are we running in the fake simulator?  

        # 是否仿真?  

        self.fake_test = rospy.get_param("~fake_test", False)  

 

        # Goal state return values  

        # 到达目标的状态  

        goal_states = ['PENDING', 'ACTIVE', 'PREEMPTED',   

                       'SUCCEEDED', 'ABORTED', 'REJECTED',  

                       'PREEMPTING', 'RECALLING', 'RECALLED',  

                       'LOST']  

 

        # Set up the goal locations. Poses are defined in the map frame.    

        # An easy way to find the pose coordinates is to point-and-click  

        # Nav Goals in RViz when running in the simulator.  

        # Pose coordinates are then displayed in the terminal  

        # that was used to launch RViz.  

        # 设置目标点的位置  

        # 如果想要获得某一点的坐标,在rviz中点击 2D Nav Goal 按键,然后单机地图中一点  

        # 在终端中就会看到坐标信息  

        locations = dict()  

 

        locations['hall_foyer'] = Pose(Point(0.643, 4.720, 0.000), Quaternion(0.000, 0.000, 0.223, 0.975))  

        locations['hall_kitchen'] = Pose(Point(-1.994, 4.382, 0.000), Quaternion(0.000, 0.000, -0.670, 0.743))  

        locations['hall_bedroom'] = Pose(Point(-3.719, 4.401, 0.000), Quaternion(0.000, 0.000, 0.733, 0.680))  

        locations['living_room_1'] = Pose(Point(0.720, 2.229, 0.000), Quaternion(0.000, 0.000, 0.786, 0.618))  

        locations['living_room_2'] = Pose(Point(1.471, 1.007, 0.000), Quaternion(0.000, 0.000, 0.480, 0.877))  

        locations['dining_room_1'] = Pose(Point(-0.861, -0.019, 0.000), Quaternion(0.000, 0.000, 0.892, -0.451))  

 

        # Publisher to manually control the robot (e.g. to stop it)  

        # 发布控制机器人的消息  

        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  

        # 60s等待时间限制  

        self.move_base.wait_for_server(rospy.Duration(60))  

 

        rospy.loginfo("Connected to move base server")  

 

        # A variable to hold the initial pose of the robot to be set by   

        # the user in RViz  

        # 保存机器人的在rviz中的初始位置  

        initial_pose = PoseWithCovarianceStamped()  

 

        # Variables to keep track of success rate, running time,  

        # and distance traveled  

        # 保存成功率、运行时间、和距离的变量  

        n_locations = len(locations)  

        n_goals = 0  

        n_successes = 0  

        i = n_locations  

        distance_traveled = 0  

        start_time = rospy.Time.now()  

        running_time = 0  

        location = ""  

        last_location = ""  

 

        # Get the initial pose from the user  

        # 获取初始位置(仿真中可以不需要)  

        rospy.loginfo("*** Click the 2D Pose Estimate button in RViz to set the robot's initial pose...")  

        rospy.wait_for_message('initialpose', PoseWithCovarianceStamped)  

        self.last_location = Pose()  

        rospy.Subscriber('initialpose', PoseWithCovarianceStamped, self.update_initial_pose)  

 

        # Make sure we have the initial pose  

        # 确保有初始位置  

        while initial_pose.header.stamp == "":  

            rospy.sleep(1)  

 

        rospy.loginfo("Starting navigation test")  

 

        # Begin the main loop and run through a sequence of locations  

        # 开始主循环,随机导航  

        while not rospy.is_shutdown():  

            # If we've gone through the current sequence,  

            # start with a new random sequence  

            # 如果已经走完了所有点,再重新开始排序  

            if i == n_locations:  

                i = 0  

                sequence = sample(locations, n_locations)  

                # Skip over first location if it is the same as  

                # the last location  

                # 如果最后一个点和第一个点相同,则跳过  

                if sequence[0] == last_location:  

                    i = 1  

 

            # Get the next location in the current sequence  

            # 在当前的排序中获取下一个目标点  

            location = sequence[i]  

 

            # Keep track of the distance traveled.  

            # Use updated initial pose if available.  

            # 跟踪形式距离  

            # 使用更新的初始位置  

            if initial_pose.header.stamp == "":  

                distance = sqrt(pow(locations[location].position.x -   

                                    locations[last_location].position.x, 2) +  

                                pow(locations[location].position.y -   

                                    locations[last_location].position.y, 2))  

            else:  

                rospy.loginfo("Updating current pose.")  

                distance = sqrt(pow(locations[location].position.x -   

                                    initial_pose.pose.pose.position.x, 2) +  

                                pow(locations[location].position.y -   

                                    initial_pose.pose.pose.position.y, 2))  

                initial_pose.header.stamp = ""  

 

            # Store the last location for distance calculations  

            # 存储上一次的位置,计算距离  

            last_location = location  

 

            # Increment the counters  

            # 计数器加1  

            i += 1  

            n_goals += 1  

 

            # Set up the next goal location  

            # 设定下一个目标点  

            self.goal = MoveBaseGoal()  

            self.goal.target_pose.pose = locations[location]  

            self.goal.target_pose.header.frame_id = 'map'  

            self.goal.target_pose.header.stamp = rospy.Time.now()  

 

            # Let the user know where the robot is going next  

            # 让用户知道下一个位置  

            rospy.loginfo("Going to: " + str(location))  

 

            # Start the robot toward the next location  

            # 向下一个位置进发  

            self.move_base.send_goal(self.goal)  

 

            # Allow 5 minutes to get there  

            # 五分钟时间限制  

            finished_within_time = self.move_base.wait_for_result(rospy.Duration(300))   

 

            # Check for success or failure  

            # 查看是否成功到达  

            if not finished_within_time:  

                self.move_base.cancel_goal()  

                rospy.loginfo("Timed out achieving goal")  

            else:  

                state = self.move_base.get_state()  

                if state == GoalStatus.SUCCEEDED:  

                    rospy.loginfo("Goal succeeded!")  

                    n_successes += 1  

                    distance_traveled += distance  

                    rospy.loginfo("State:" + str(state))  

                else:  

                  rospy.loginfo("Goal failed with error code: " + str(goal_states[state]))  

 

            # How long have we been running?  

            # 运行所用时间  

            running_time = rospy.Time.now() - start_time  

            running_time = running_time.secs / 60.0  

 

            # Print a summary success/failure, distance traveled and time elapsed  

            # 输出本次导航的所有信息  

            rospy.loginfo("Success so far: " + str(n_successes) + "/" +   

                          str(n_goals) + " = " +   

                          str(100 * n_successes/n_goals) + "%")  

            rospy.loginfo("Running time: " + str(trunc(running_time, 1)) +   

                          " min Distance: " + str(trunc(distance_traveled, 1)) + " m")  

            rospy.sleep(self.rest_time)  

 

    def update_initial_pose(self, initial_pose):  

        self.initial_pose = initial_pose  

 

    def shutdown(self):  

        rospy.loginfo("Stopping the robot...")  

        self.move_base.cancel_goal()  

        rospy.sleep(2)  

        self.cmd_vel_pub.publish(Twist())  

        rospy.sleep(1)  

 

def trunc(f, n):  

    # Truncates/pads a float f to n decimal places without rounding  

    slen = len('%.*f' % (n, f))  

    return float(str(f)[:slen])  

 

if __name__ == '__main__':  

    try:  

        NavTest()  

        rospy.spin()  

    except rospy.ROSInterruptException:  

        rospy.loginfo("AMCL navigation test finished.")

您必须 登录 才能发表评论!