turtlebot實現在多個目標點之間自主導航
阿新 • • 發佈：2019-01-28
turtlebot實現在幾個目標點之間自主導航的任務，關鍵是指定turtlebot的初始位姿後，設定多個目標點。
在~/catkin_ws/src/simple_navigation_gola/src目錄下建立nav_test.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 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. locations = dict() locations['hall_foyer'] = Pose(Point(1.714, 0.515, 0.000), Quaternion(0.000, 0.000, -0.309, 0.951)) locations['hall_kitchen'] = Pose(Point(-0.809, -2.141, 0.000), Quaternion(0.000, 0.000, -0.816, 0.578)) locations['hall_bedroom'] = Pose(Point(3.457, -1.495, 0.000), Quaternion(0.000, 0.000, -0.003, 1.000)) #locations['hall_foyer'] = Pose(Point(1.719, 0.409, 0.000), # Quaternion(0.000, 0.000, 0.468, 0.884)) #locations['hall_kitchen'] = Pose(Point(0.856, 2.858, 0.000), # Quaternion(0.000, 0.000, 0.192, 0.981)) #locations['hall_bedroom'] = Pose(Point(1.781, 1.856, 0.000), # Quaternion(0.000, 0.000, 0.000, 1.000)) #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, 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: NavTest() rospy.spin() except rospy.ROSInterruptException: rospy.loginfo("AMCL navigation test finished.")
其中，在通過以下語句設定目標點位置和朝向：
locations['hall_foyer'] = Pose(Point(1.714, 0.515, 0.000), Quaternion(0.000, 0.000, -0.309, 0.951)) locations['hall_kitchen'] = Pose(Point(-0.809, -2.141, 0.000), Quaternion(0.000, 0.000, -0.816, 0.578)) locations['hall_bedroom'] = Pose(Point(3.457, -1.495, 0.000), Quaternion(0.000, 0.000, -0.003, 1.000))
目標點位置和朝向引數通過在rviz中顯示地圖，並通過2d nav goal設定目標，並在開啟rviz的terminal中檢視在地圖的實際位姿。
設定完成後執行命令：
chmod +x nav_test.py然後執行以下命令：
下面就可以看到turtlebot在設定的目標點之間反覆自主導航。roslaunch turtlebot_bringup minimal.launch //啟動turtlebot roslaunh turtlebot_navigation amcl_demo.launch map_file:=/home/turtlebot/Downloads/map.yaml //執行導航程式並載入已經建立好的地圖 roslaunch turtlebot_rviz_launchers view_navigation.launch //在rviz中實時監測導航過程 rosrun simple_navigation_goal nav_test.py //執行設定目標點的程式，並在rviz中通過2d pose estimate設定初始位姿。