#!/usr/bin/env python
""" nav_test.py - Version 1.1 2013-12-20
命令机器人在地图框架中定义的多个目标位置之间自主移动。
在每一轮上，选择一个新的随机序列位置，然后尝试相继移动到每个位置。
跟踪成功率、时间流逝和总旅行距离。
为Pi机器人项目创建：HTTP://www. PIROBOT.ORG
版权所有（C）2012帕特里克戈贝尔。版权所有。
这个程序是免费软件，你可以重新分配它和/或修改它。
根据GNU通用公共许可证的条款
自由软件基金会；许可证的第2版；或（按你的选择）以后的版本5。
这个程序是分布式的，希望它是有用的，
但没有任何保证，甚至没有默示保证。
适销性或适合某一特定目的的适销性。见
GNU通用公共许可证的更多细节：
http://www.gnu.org/licenses/gpl.html
"""
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)
# 在目标位置停留的时间
self.rest_time = rospy.get_param("~rest_time", 10)
# 是否使用模拟器
self.fake_test = rospy.get_param("~fake_test", False)
# 目标状态返回值
goal_states = ['PENDING', 'ACTIVE', 'PREEMPTED',
'SUCCEEDED', 'ABORTED', 'REJECTED',
'PREEMPTING', 'RECALLING', 'RECALLED',
'LOST']
# 设置目标位置
# 目标位置被储存为一个python字典
# 可以通过点击Rviz中的2D Nav Goal 按钮，然后在终端中查看你指定目标位置数据
# 或者运行rqt_console查看你指定目标位置数据
# 数据包含目标位置的坐标和方向
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, queue_size=5)
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()
# 跟踪记录成功率，运行时间和
# 和行进距离
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 = ""
# 等待用户设置小车初始位置
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)
# 确保我们设置了初始位置
while initial_pose.header.stamp == "":
rospy.sleep(1)
rospy.loginfo("Starting navigation test")
# 循环运行直到用户终止了这个应用
while not rospy.is_shutdown():
# 如果这个目标位置都去过了
# 就重新随机排序列表
if i == n_locations:
i = 0
sequence = sample(locations, n_locations)
# 如果目标位置就是现在的位置
# 就跳到下个目标
if sequence[0] == last_location:
i = 1
# 从当前的顺序字典中取出目标位置
location = sequence[i]
# 跟踪行驶距离
# 检测是否有新的初始位置
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 = ""
# 存储最后一个位置进行距离计算
last_location = location
# Increment the counters
i += 1
n_goals += 1
# 设置下一个目标位置
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()
# 把下一个目标打印出来
rospy.loginfo("Going to: " + str(location))
# 发送目标位置
self.move_base.send_goal(self.goal)
# 到达目标位置的时限为300s
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):
# 截断/填充一个浮点数到n个小数点而不舍入
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.")
网友评论