文章目录
我已经建好图之后需要进行定位，并且自主行驶。
教程在此
roslaunch turtlebot_navigation amcl_demo.launch map_file:=/tmp/my_map.yaml
如果是之前我的rplidar+turtlebot2+kartoslam建图的话，之间运行
显然
是会报错的
报错如下:
[ INFO] [1578461816.522543748]: No matching device found.... waiting for devices. Reason: std::__cxx11::string openni2_wrapper::OpenNI2Driver::resolveDeviceURI(const string&) @ /tmp/binarydeb/ros-kinetic-openni2-camera-0.4.2/src/openni2_driver.cpp @ 737 : Invalid device number 1, there are 0 devices connected.
[ INFO] [1578461819.522868425]: No matching device found.... waiting for devices. Reason: std::__cxx11::string openni2_wrapper::OpenNI2Driver::resolveDeviceURI(const string&) @ /tmp/binarydeb/ros-kinetic-openni2-camera-0.4.2/src/openni2_driver.cpp @ 737 : Invalid device number 1, there are 0 devices connected.
OpenNI2Driver?
[ WARN] [1578461821.301283372]: Timed out waiting for transform from base_footprint to map to become available before running costmap, tf error: canTransform: target_frame map does not exist.. canTransform returned after 0.10052 timeout was 0.1
一开始没看launch文件内容还百度了很多
Timed out waiting for transform from base_footprint to map to become available before running costm
:雷达前面20厘米以内放置障碍阻挡
[ WARN] [1555379888.006898613]: Timed out waiting for transform from base_footprint to map to become
:
- 在应用中推荐把provide_odom_frame参数置为false。当该参数为true时，其发布的数据为map->odom的tf，而非机器人的位姿。当将provide_odom_frame参数置为false时，可以获得map->base_footprint的tf数据，即机器人相对于map的位姿
- 修改global和local_costmap_params.yaml中的transform_tolerance（坐标系间的转换可以忍受的最大延时），我的最初默认为１，我依次增加到８的时候不出现错误了，但８的话已经没有任何意义了。
- 雷达信息不正确
其实不是雷达信息不正确，首先我们看一下我们运行的launch文件是什么
rosed turtlebot_navigation amcl_demo.launch
<launch>
<!-- 3D sensor -->
<arg name="3d_sensor" default="$(env TURTLEBOT_3D_SENSOR)"/> <!-- r200, kinect, asus_xtion_pro -->
<!-- 这里的$(env估计是为了得到环境变量 但是我env |grep TURTKEBOT_3D_SENSOR 发现啥都没有
但是发现bashrc中有export TURTLEBOT_3D_SENSOR=kinect
-->
<include file="$(find turtlebot_bringup)/launch/3dsensor.launch">
<arg name="rgb_processing" value="false" />
<arg name="depth_registration" value="false" />
<arg name="depth_processing" value="false" />
<!-- We must specify an absolute topic name because if not it will be prefixed by "$(arg camera)".
Probably is a bug in the nodelet manager: https://github.com/ros/nodelet_core/issues/7 -->
<arg name="scan_topic" value="/scan" />
</include>
<!-- Map server，这里我env |grep TURTLEBOT_MAP_FILE出现
TURTLEBOT_MAP_FILE=/opt/ros/kinetic/share/turtlebot_navigation/maps/willow-2010-02-18-0.10.yaml 所以必须要在参数里面更改自己地图所在目录-->
<arg name="map_file" default="$(env TURTLEBOT_MAP_FILE)"/>
<node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" />
<!-- AMCL -这里就更不可能了都不知道3d_sensor的arg是什么 -->
<arg name="custom_amcl_launch_file" default="$(find turtlebot_navigation)/launch/includes/amcl/$(arg 3d_sensor)_amcl.launch.xml"/>
<arg name="initial_pose_x" default="0.0"/> <!-- Use 17.0 for willow's map in simulation -->
<arg name="initial_pose_y" default="0.0"/> <!-- Use 17.0 for willow's map in simulation -->
<arg name="initial_pose_a" default="0.0"/>
<include file="$(arg custom_amcl_launch_file)">
<arg name="initial_pose_x" value="$(arg initial_pose_x)"/>
<arg name="initial_pose_y" value="$(arg initial_pose_y)"/>
<arg name="initial_pose_a" value="$(arg initial_pose_a)"/>
</include>
<!-- Move base -->
<!--
ls |grep _costmap
astra_costmap_params.yaml
asus_xtion_pro_costmap_params.yaml
asus_xtion_pro_offset_costmap_params.yaml
global_costmap_params.yaml
kinect_costmap_params.yaml
local_costmap_params.yaml
r200_costmap_params.yaml
你看 也没有与rplidar相关的costmap的存在
-->
<arg name="custom_param_file" default="$(find turtlebot_navigation)/param/$(arg 3d_sensor)_costmap_params.yaml"/>
<include file="$(find turtlebot_navigation)/launch/includes/move_base.launch.xml">
<arg name="custom_param_file" value="$(arg custom_param_file)"/>
</include>
</launch>
换教程!
Turbot-SLAM入门教程-实现AMCL自主导航(Rplidar A2版)
其代码库
，我们下下来看，看到
laser_amcl_demo.launch
<launch>
<!-- Define laser type-->
<arg name="laser_type" default="$(env TURTLEBOT_LASER_SENSOR)" />
<!-- laser driver -->
<include file="$(find turtlebot_navigation)/laser/driver/$(arg laser_type)_laser.launch" />
<!-- Map server -->
<arg name="map_file" default="$(env TURTLEBOT_MAP_FILE)"/>
<node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" />
<!-- AMCL -->
<arg name="custom_amcl_launch_file" default="$(find turtlebot_navigation)/launch/includes/amcl/$(arg laser_type)_amcl.launch.xml"/>
<arg name="initial_pose_x" default="0.0"/> <!-- Use 17.0 for willow's map in simulation -->
<arg name="initial_pose_y" default="0.0"/> <!-- Use 17.0 for willow's map in simulation -->
<arg name="initial_pose_a" default="0.0"/>
<include file="$(arg custom_amcl_launch_file)">
<arg name="initial_pose_x" value="$(arg initial_pose_x)"/>
<arg name="initial_pose_y" value="$(arg initial_pose_y)"/>
<arg name="initial_pose_a" value="$(arg initial_pose_a)"/>
</include>
<!-- Move base -->
<arg name="custom_param_file" default="$(find turtlebot_navigation)/param/$(arg laser_type)_costmap_params.yaml"/>
<include file="$(find turtlebot_navigation)/launch/includes/move_base.launch.xml">
<arg name="custom_param_file" value="$(arg custom_param_file)"/>
</include>
</launch>
export TURTLEBOT_LASER_SENSOR=rplidar
然后去看
turtlebot_apps增强包：为Turtlebot_navigation 增加多种雷达支持
的代码
rplidar_amcl.launch.xml
<launch>
<arg name="use_map_topic" default="false"/>
<arg name="scan_topic" default="scan"/>
<arg name="initial_pose_x" default="0.0"/>
<arg name="initial_pose_y" default="0.0"/>
<arg name="initial_pose_a" default="0.0"/>
<arg name="odom_frame_id" default="odom"/>
<arg name="base_frame_id" default="base_footprint"/>
<arg name="global_frame_id" default="map"/>
<node pkg="amcl" type="amcl" name="amcl">
<param name="use_map_topic" value="$(arg use_map_topic)"/>
<!-- Publish scans from best pose at a max of 10 Hz -->
<param name="odom_model_type" value="diff"/>
<param name="odom_alpha5" value="0.1"/>
<param name="gui_publish_rate" value="10.0"/>
<param name="laser_max_beams" value="60"/>
<param name="laser_max_range" value="12.0"/>
<param name="min_particles" value="500"/>
<param name="max_particles" value="2000"/>
<param name="kld_err" value="0.05"/>
<param name="kld_z" value="0.99"/>
<param name="odom_alpha1" value="0.2"/>
<param name="odom_alpha2" value="0.2"/>
<!-- translation std dev, m -->
<param name="odom_alpha3" value="0.2"/>
<param name="odom_alpha4" value="0.2"/>
<param name="laser_z_hit" value="0.5"/>
<param name="laser_z_short" value="0.05"/>
<param name="laser_z_max" value="0.05"/>
<param name="laser_z_rand" value="0.5"/>
<param name="laser_sigma_hit" value="0.2"/>
<param name="laser_lambda_short" value="0.1"/>
<param name="laser_model_type" value="likelihood_field"/>
<!-- <param name="laser_model_type" value="beam"/> -->
<param name="laser_likelihood_max_dist" value="2.0"/>
<param name="update_min_d" value="0.25"/>
<param name="update_min_a" value="0.2"/>
<param name="odom_frame_id" value="$(arg odom_frame_id)"/>
<param name="base_frame_id" value="$(arg base_frame_id)"/>
<param name="global_frame_id" value="$(arg global_frame_id)"/>
<param name="resample_interval" value="1"/>
<!-- Increase tolerance because the computer can get quite busy -->
<param name="transform_tolerance" value="1.0"/>
<param name="recovery_alpha_slow" value="0.0"/>
<param name="recovery_alpha_fast" value="0.0"/>
<param name="initial_pose_x" value="$(arg initial_pose_x)"/>
<param name="initial_pose_y" value="$(arg initial_pose_y)"/>
<param name="initial_pose_a" value="$(arg initial_pose_a)"/>
<remap from="scan" to="$(arg scan_topic)"/>
</node>
</launch>
move_base.launch.xml
<!--
ROS navigation stack with velocity smoother and safety (reactive) controller
-->
<launch>
<include file="$(find turtlebot_navigation)/launch/includes/velocity_smoother.launch.xml"/>
<include file="$(find turtlebot_navigation)/launch/includes/safety_controller.launch.xml"/>
<arg name="odom_frame_id" default="odom"/>
<arg name="base_frame_id" default="base_footprint"/>
<arg name="global_frame_id" default="map"/>
<arg name="odom_topic" default="odom" />
<arg name="laser_topic" default="scan" />
<arg name="custom_param_file" default="$(find turtlebot_navigation)/param/dummy.yaml"/>
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
<rosparam file="$(find turtlebot_navigation)/param/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find turtlebot_navigation)/param/costmap_common_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find turtlebot_navigation)/param/local_costmap_params.yaml" command="load" />
<rosparam file="$(find turtlebot_navigation)/param/global_costmap_params.yaml" command="load" />
<rosparam file="$(find turtlebot_navigation)/param/dwa_local_planner_params.yaml" command="load" />
<rosparam file="$(find turtlebot_navigation)/param/move_base_params.yaml" command="load" />
<rosparam file="$(find turtlebot_navigation)/param/global_planner_params.yaml" command="load" />
<rosparam file="$(find turtlebot_navigation)/param/navfn_global_planner_params.yaml" command="load" />
<!-- external params file that could be loaded into the move_base namespace -->
<rosparam file="$(arg custom_param_file)" command="load" />
<!-- reset frame_id parameters using user input data -->
<param name="global_costmap/global_frame" value="$(arg global_frame_id)"/>
<param name="global_costmap/robot_base_frame" value="$(arg base_frame_id)"/>
<param name="local_costmap/global_frame" value="$(arg odom_frame_id)"/>
<param name="local_costmap/robot_base_frame" value="$(arg base_frame_id)"/>
<param name="DWAPlannerROS/global_frame_id" value="$(arg odom_frame_id)"/>
<remap from="cmd_vel" to="navigation_velocity_smoother/raw_cmd_vel"/>
<remap from="odom" to="$(arg odom_topic)"/>
<remap from="scan" to="$(arg laser_topic)"/>
</node>
</launch>
(find turtlebot_navigation)/param/$(arg laser_type)_costmap_params.yaml
那个rplidar_costmap_params.yam的内容是
#A dummy file loaded when no custom param file is given
OK 运行
roslaunch turtlebot_bringup minimal.launch
roslaunch turtlebot_navigation laser_amcl_demo.launch map_file:=/map/rplidar_karto3.yaml
如果你看到 odom received! 说明已经正常运行。
如果你收到一个警告：Waiting on transform…，先重启minimal.launch，重新启动amcl_demo.lauch。您可能需要尝试重新启动几次。同时，关闭Kobuki基站并重开。
turtlebot成功运行，在工作站运行：
roslaunch turtlebot_rviz_launchers view_navigation.launch –screen
rviz应该公开展示你的地图。turtlebot不能够估计在启动姿态，尽管它可以在你初始化它的姿态。
选择“2D Pose Estimate”在地图上的turtlebot位置点击并按住。当你握着鼠标的时候，一个箭头会出现在鼠标指针的下方，用这个来估计它的方向。
设置后估计的姿态，选择“2D Nav Goal”，点击你想让turtlebot去的地方。您还可以指定一个目标方向，使用相同的技术如“2D Pose Estimate”。
turtlebot现在应该朝着你的目录自主前进。尝试指定一个目标，并走在前面，看看它如何反应的动态障碍。
过程中发现，并不会实时检测障碍，但是如果前面的碰撞传感器被出发，会发动recovery模式
recovery tips
如果说碰到障碍物就会开始Rotate recovery behavior
[ WARN] [1578482248.616522787]: Rotate recovery behavior started.
[ERROR] [1578482248.616763792]: Rotate recovery can’t rotate in place because there is a potential collision. Cost: -1.00
[ INFO] [1578482249.016519404]: Got new plan
[ WARN] [1578482249.018526199]: DWA planner failed to produce path.
[ERROR] [1578482249.216536333]: Aborting because a valid control could not be found. Even after executing all recovery behaviors
此处有
Rotate recovery can’t rotate in place because there is a potential collision. Cost: -1.00遮盖ERROR的WIKI
What outputs these warning and error? I means which source file says error and warning? and What kind of problem happens on my plan?
Could anyone give me any clues? Thank you in advance
什么输出这些警告和报错的，那个源文件在什么发生的情况下输出这些东西？
Yes, this in the rotate_recovery behavior inside the navigation stack. However, the actual cause of this is typically more complex. If your robot has an odd-shaped footprint, than that can be the cause. But if you are using a turtlebot, with a circular footprint and centered drive wheels, you should pretty much always be able to rotate in place – unless localization drifts/jumps, thus putting you on top of an obstacle that was already in the cost map.
旋转修复行为在navigation 的stack里面，但是真正引起遮盖的原因更加复杂，如果能的机器人有一个奇怪的形状（覆盖区），就可以产生，但是如果你用TB，你总会在原地旋转，除非定位跳变了，着牙你就会显示在costmap的障碍区域上，报错啦。
实现单点导航
#!/usr/bin/env python
'''
Copyright (c) 2015, Mark Silliman
All rights reserved.
Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
'''
# TurtleBot must have minimal.launch & amcl_demo.launch
# running prior to starting this script
# For simulation: launch gazebo world & amcl_demo prior to run this script
import rospy
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
import actionlib
from actionlib_msgs.msg import *
from geometry_msgs.msg import Pose, Point, Quaternion
class GoToPose():
def __init__(self):
self.goal_sent = False
# What to do if shut down (e.g. Ctrl-C or failure)
rospy.on_shutdown(self.shutdown)
# Tell the action client that we want to spin a thread by default
self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
rospy.loginfo("Wait for the action server to come up")
# Allow up to 5 seconds for the action server to come up
self.move_base.wait_for_server(rospy.Duration(5))
def goto(self, pos, quat):
# Send a goal
self.goal_sent = True
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = 'map'
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose = Pose(Point(pos['x'], pos['y'], 0.000),
Quaternion(quat['r1'], quat['r2'], quat['r3'], quat['r4']))
# Start moving
self.move_base.send_goal(goal)
# Allow TurtleBot up to 60 seconds to complete task
success = self.move_base.wait_for_result(rospy.Duration(60))
state = self.move_base.get_state()
result = False
if success and state == GoalStatus.SUCCEEDED:
# We made it!
result = True
else:
self.move_base.cancel_goal()
self.goal_sent = False
return result
def shutdown(self):
if self.goal_sent:
self.move_base.cancel_goal()
rospy.loginfo("Stop")
rospy.sleep(1)
if __name__ == '__main__':
try:
rospy.init_node('nav_test', anonymous=False)
navigator = GoToPose()
# Customize the following values so they are appropriate for your location
position = {'x': 1.22, 'y' : 2.56}
quaternion = {'r1' : 0.000, 'r2' : 0.000, 'r3' : 0.000, 'r4' : 1.000}
rospy.loginfo("Go to (%s, %s) pose", position['x'], position['y'])
success = navigator.goto(position, quaternion)
if success:
rospy.loginfo("Hooray, reached the desired pose")
else:
rospy.loginfo("The base failed to reach the desired pose")
# Sleep to give the last log messages time to be sent
rospy.sleep(1)
except rospy.ROSInterruptException:
rospy.loginfo("Ctrl-C caught. Quitting")
实现多点定位
获取坐标的方法文章里面也有。
原点(0，0)
第二个点(-1.43，2.52)
第三个点(-2.01，3.99)
第四个测量点(-11.1,3.82)
第五个测量点(-10.2，-0.176)
第六个点(-15.1，2.64)
第七个点(-16.5,6.17)
locations['station1'] = Pose(Point(-1.43, 2.52, 0.00), Quaternion(0.000, 0.000, 0.000, 1.000))
locations['station2'] = Pose(Point(-2.01, 3.99, 0.00), Quaternion(0.000, 0.000, 0.000, 1.000))
locations['cross1'] = Pose(Point(-11.1,3.82, 0.00), Quaternion(0.000, 0.000, 0.000, 1.000))
locations['elevator port'] = Pose(Point(-10.2,-0.176, 0.00), Quaternion(0.000, 0.000, 1.000, 0.000))
locations['cross2'] = Pose(Point(-15.1,2.64, 0.00), Quaternion(0.000, 0.000, 0.000, 1.000))
locations['WC'] = Pose(Point(-16.5,6.17, 0.00), Quaternion(0.000, 0.000, 0.000, 1.000))
roslaunch turtlebot_bringup minimal.launch
roslaunch turtlebot_navigation laser_amcl_demo.launch map_file:=/map/rplidar_karto3.yaml
如果你看到 odom received! 说明已经正常运行。
如果你收到一个警告：Waiting on transform…，先重启minimal.launch，重新启动amcl_demo.lauch。您可能需要尝试重新启动几次。同时，关闭Kobuki基站并重开。
turtlebot成功运行，在工作站运行：
roslaunch turtlebot_rviz_launchers view_navigation.launch –screen
然后TB2上
rosrun multinav nav.py
多点定位代码
#!/usr/bin/env python
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 MultiNav():
def __init__(self):
rospy.init_node('MultiNav', 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.
locations = dict()
# 替代为自己的地图上对应的位置，朝向默认为1
locations['station1'] = Pose(Point(-1.43, 2.52, 0.00), Quaternion(0.000, 0.000, 0.000, 1.000))
locations['station2'] = Pose(Point(-2.01, 3.99, 0.00), Quaternion(0.000, 0.000, 0.000, 1.000))
locations['cross1'] = Pose(Point(-11.1,3.82, 0.00), Quaternion(0.000, 0.000, 0.000, 1.000))
locations['elevator port'] = Pose(Point(-10.2,-0.176, 0.00), Quaternion(0.000, 0.000, 1.000, 0.000))
locations['cross2'] = Pose(Point(-15.1,2.64, 0.00), Quaternion(0.000, 0.000, 0.000, 1.000))
locations['WC'] = Pose(Point(-16.5,6.17, 0.00), Quaternion(0.000, 0.000, 0.000, 1.000))
# Publisher to manually control the robot (e.g. to stop it)
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)
# Subscribe to the move_base action server
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
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
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 on the map in RViz to set the intial 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
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
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:
MultiNav()
rospy.spin()
except rospy.ROSInterruptException:
rospy.loginfo("AMCL navigation test finished.")
代码比较易懂，不作解释
版权声明：本文为kuizhao8951原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。