博客转自:https://blog.csdn.net/hcx25909/article/details/12110959
在理解了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/kinetic
rosrun rviz rviz -d `rospack find rbx1_nav`/nav.rviz
这时在rviz中就应该显示出了地图和机器人:
现在就可以通过rviz在地图上选择目标位置了,然后就会看到机器人自动规划出一条全局路径,并且导航前进:
二、自主导航
在实际应用中,我们往往希望机器人能够自主进行定位和导航,不需要认为的干预,这样才更智能化。在这一节的测试中,我们让目标点在地图中随机生成,然后机器人自动导航到达目标。 这里运行的主要文件是: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的功能比较多:
- 加载机器人驱动
- 加载地图
- 启动move_base节点,并且加载配置文件
- 运行amcl节点
- 然后加载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/kinetic
//todo
好了,此时就看到了机器人已经放在地图当中了。然后我们点击rviz上的“2D Pose Estimate”按键,然后左键在机器人上单击,让绿色的箭头和黄色的箭头重合,机器人就开始随机选择目标导航了:
在监控窗口中,我们可以看到机器人发送的状态信息:
其中包括距离信息、状态信息、目标的编号、成功率和速度等信息。
三、导航代码分析
#!/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.")