python可以用来写导航吗_Turbot与python编程-实现多点导航

Turbot与python教程-实现多点导航

说明：

介绍如何实现通过python控制turbot实现多点导航

代码：

实现代码：

#!/usr/bin/env python

# TurtleBot must have minimal.launch & amcl_demo.launch

# navMultiPoints.py

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(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))

location