ROS源码阅读日记——nav

ROS源码阅读日记——nav

#这段代码的作用是实现在地图中随机导航。在ros下需要先加载机器人和相关配置，详请参阅 古-月 的博客
#这里主要是用于自己来理解这段代码

#!/usr/bin/env pythonimport 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, sqrtclass NavTest():def __init__(self):rospy.init_node('nav_test', anonymous=_shutdown(self.shutdown)#rospyAPI：on_shutdown()# How long in seconds should the robot pause at each location?#在每个目标点停留的时间st_time = _param("~rest_time", 10)#API get_param# Are we running in the fake simulator?#是否是在模拟环境中运行self.fake_test = _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()#这里定义了六个定位点的姿态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)#发布控制机器人的消息d_vel_pub = rospy.Publisher('cmd_vel', Twist)# Subscribe to the move_base action server#订阅move_base动作服务ve_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)rospy.loginfo("Waiting for move_base ")# Wait 60 seconds for the action server to become available#等ve_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 #用户在rviz中设置机器人的初始位姿的变量# the user in RVizinitial_pose = PoseWithCovarianceStamped()# Variables to keep track of success rate, running time,#保存成功率、运行时间、运动距离的变量们# and distance traveledn_locations = len(locations)n_goals = 0n_successes = 0i = n_locationsdistance_traveled = 0start_time = w()running_time = 0location = ""last_location = ""# Get the initial pose from the user#由用户处获取初始位姿rospy.loginfo("*** Click the 2D Pose Estimate button in RViz to set the robot's ")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 sequenceif i == n_locations:i = 0sequence = 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#计数器增加1i += 1n_goals += 1# Set up the next goal location#设置下一个目标点al = MoveBaseGoal()###al.target_pose.pose = locations[al.target_pose.header.frame_id = 'map&#al.target_pose.header.stamp = w()# Let the user know where the robot is going next#告诉用户下一个位置rospy.loginfo("Going to: " + str(location))# Start the robot toward the next location#开始向下一个位置前进ve_base.send_al)#move_base.send_goal()# Allow 5 minutes to get there#设置时间，5分钟之内到达finished_within_time = ve_base.wait_for_result(rospy.Duration(300)) # Check for success or failure#检查是否成功到达if not finished_within_ve_base.cancel_goal()#move_base.cancle_goal()rospy.loginfo("Timed out achieving goal")else:state = _state()#_state()if state == GoalStatus.SUCCEEDED:rospy.loginfo("Goal succeeded!")n_successes += 1distance_traveled += distancerospy.loginfo("State:" + str(state))else:rospy.loginfo("Goal failed with error code: " + str(goal_states[state]))# How long have we been running?#已经运行了多长时间running_time = w() - start_timerunning_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.st_time)#更新初始位姿的函数        def update_initial_pose(self, initial_pose):self.initial_pose = initial_pose#关机处理函数def shutdown(self):rospy.loginfo("Stopping ")ve_base.cancel_goal()rospy.sleep(d_vel_pub.publish(Twist())rospy.sleep(1)def trunc(f, n):# Truncates/pads a float f to n decimal places without rounding#拉长/缩短一个浮点数f到一个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.")

其中还有些API需要进一步了解一下。

其中包括 有：

move_base.wait_for_server()

move_base.send_goal()

move_base.cancle_goal()

PoseWithCovarianceStamped()

actionlib.SimpleActionClient()

实现导航主要依赖于amcl和move_base两个节点。通过运行这两个节点及相关navigation的节点，就可以进行手动导航。nav_test.py代码从某种意义上讲是实现了自动导航。在地图上设置几个点，然后调用ros的这几个节点提供的API来实现。