博客轉自：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.")