python读取msg文件_ROS学习 Python读写文本文件

importrandomimportrospyimportactionlibimportwaypoint_touring.utils as utilsimportmove_base_msgs.msg as move_base_msgsimportvisualization_msgs.msg as viz_msgsclassTourMachine(object):def __init__(self, filename, random_visits=False, repeat=False):

self._waypoints=utils.get_waypoints(filename)

action_name= 'move_base'self._ac_move_base=actionlib.SimpleActionClient(action_name, move_base_msgs.MoveBaseAction)

rospy.loginfo('Wait for %s server' %action_name)

self._ac_move_base.wait_for_server

self._counter=0

self._repeat=repeat

self._random_visits=random_visitsifself._random_visits:

random.shuffle(self._waypoints)

self._pub_viz_marker= rospy.Publisher('viz_waypoints', viz_msgs.MarkerArray, queue_size=1, latch=True)

self._viz_markers=utils.create_viz_markers(self._waypoints)defmove_to_next(self):

p=self._get_next_destination()if notp:

rospy.loginfo("Finishing Tour")returnTrue

goal=utils.create_move_base_goal(p)

rospy.loginfo("Move to %s" % p['name'])

self._ac_move_base.send_goal(goal)

self._ac_move_base.wait_for_result()

result=self._ac_move_base.get_result()

rospy.loginfo("Result : %s" %result)returnFalsedef_get_next_destination(self):if self._counter ==len(self._waypoints):ifself._repeat:

self._counter=0ifself._random_visits:

random.shuffle(self._waypoints)else:

next_destination=None

next_destination=self._waypoints[self._counter]

self._counter= self._counter + 1

returnnext_destinationdefspin(self):

rospy.sleep(1.0)

self._pub_viz_marker.publish(self._viz_markers)

finished=Falsewhile not rospy.is_shutdown() and notfinished:

finished=self.move_to_next()

rospy.sleep(2.0)if __name__ == '__main__':

rospy.init_node('tour')

filename= rospy.get_param('~filename')

random_visits= rospy.get_param('~random', False)

repeat= rospy.get_param('~repeat', False)

m=TourMachine(filename, random_visits, repeat)

rospy.loginfo('Initialized')

m.spin()

rospy.loginfo('Bye Bye')