turtlebot入门教程-多个目标点自主导航
turtlebot入门教程-多个目标点自主导航
说明：
- 介绍如何实现多个目标点的自主导航
- 参考本站制作的github已经完成的multinav包
步骤：
- 生成multinav包：
cd ~/catkin_ws/src
catkin_create_pkg multinav std_msgs rospy roscpp
- 增加导航文件
cd ~/catkin_ws/src/multinav/src
touch nav.py
vim 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['home_kitchen'] = Pose(Point(-1.03, 7.28, 0.00), Quaternion(0.000, 0.000, 0.000, 1.000))
locations['home_hall'] = Pose(Point(-1.40, 4.30, 0.00), Quaternion(0.000, 0.000, 0.000, 1.000))
locations['home_sofa'] = Pose(Point(-2.56, 2.82, 0.00), Quaternion(0.000, 0.000, 0.000, 1.000))
locations['home_refrigerator'] = Pose(Point(-1.00, 6.88, 0.00), Quaternion(0.000, 0.000, 1.000, 0.000))
locations['home_door'] = Pose(Point(-2.80, 8.00, 0.00), Quaternion(0.000, 0.000, 0.000, 1.000))
locations['home_balcony'] = Pose(Point(-2.08, 4.57, 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.")
制作地图：
例如：制作地图保存为/home/nc/map/home.yaml
创建地图后，标记目标点位置和朝向
- 点击RVIZ工具栏的上的Publish point，鼠标移动到地图上的任意，查看左下角的坐标点：x,y,z 。
- 选择多组需要的坐标点，记录下来作为之后需要导航的地点。
- 朝向方便起见，都设置为1
制作导航字典：
- 位置：Point(x, y, z)分别是上面记录的数组，z默认都为0.00
- 朝向：Quaternion(0.000, 0.000, 0.000, 1.000)，默认都为1，可以自己设定。
字典格式：
locations['home_kitchen'] = Pose(Point(x, y, z), Quaternion(0.000, 0.000, 0.000, 1.000))
- 字典示例：
locations['home_kitchen'] = Pose(Point(-1.03, 7.28, 0.00), Quaternion(0.000, 0.000, 0.000, 1.000))
locations['home_hall'] = Pose(Point(-1.40, 4.30, 0.00), Quaternion(0.000, 0.000, 0.000, 1.000))
locations['home_sofa'] = Pose(Point(-2.56, 2.82, 0.00), Quaternion(0.000, 0.000, 0.000, 1.000))
locations['home_refrigerator'] = Pose(Point(-1.00, 6.88, 0.00), Quaternion(0.000, 0.000, 1.000, 0.000))
locations['home_balcony'] = Pose(Point(-2.08, 4.57, 0.00), Quaternion(0.000, 0.000, 0.000, 1.000))
- 更新到代码nav.py中，标识要替换的部分。
运行测试：
- 在turtlebot打开新终端，启动turtlebot。
roslaunch turtlebot_bringup minimal.launch
- 在turtlebot打开新终端，运行导航程序并加载地图。
roslaunch turtlebot_navigation amcl_demo.launch map_file:=/home/nc/map/home.yaml
- 在工作站打开新终端，实时监测。
roslaunch turtlebot_rviz_launchers view_navigation.launch
- 在turtlebot打开新终端，执行导航。
rosrun multinav nav.py
- 程序监听初始位姿，在rviz中通过2d pose estimate设定初始位姿。
- 成功设置后，程序就会按照设定的目标点逐一导航。
扩展：
目标点保存和读取：
- 写入文件，通过文件读取
- 写入数据库，通过数据库读取
准实时：
- 指定某个目标点，让机器人立即导航
- 思路：通过web实现读取和写入目标点到数据库指定的临时表，此表保存目标点数据，循环查询，有数据，即读取并执行导航。
参考：
- 《ros by examples》例程
- http://blog.csdn.net/a472725641/article/details/53537806
- https://segmentfault.com/a/1190000006262518
获取最新文章: 扫一扫右上角的二维码加入“创客智造”公众号