Go to the documentation of this file.00001
00002
00003 import yaml
00004 import rospy
00005 import geometry_msgs.msg as geometry_msgs
00006
00007 class WaypointGenerator(object):
00008
00009 def __init__(self, filename):
00010 self._sub_pose = rospy.Subscriber('clicked_point', geometry_msgs.PointStamped, self._process_pose, queue_size=1)
00011 self._waypoints = []
00012 self._filename = filename
00013
00014 def _process_pose(self, msg):
00015 p = msg.point
00016
00017 data = {}
00018 data['frame_id'] = msg.header.frame_id
00019 data['pose'] = {}
00020 data['pose']['position'] = {'x': p.x, 'y': p.y, 'z': 0.0}
00021 data['pose']['orientation'] = {'x': 0, 'y': 0, 'z': 0, 'w':1}
00022 data['name'] = '%s_%s' % (p.x, p.y)
00023
00024 self._waypoints.append(data)
00025 rospy.loginfo("Clicked : (%s, %s, %s)" % (p.x, p.y, p.z))
00026
00027 def _write_file(self):
00028 ways = {}
00029 ways['waypoints'] = self._waypoints
00030 with open(self._filename, 'w') as f:
00031 f.write(yaml.dump(ways, default_flow_style=False))
00032
00033 def spin(self):
00034 rospy.spin()
00035 self._write_file()
00036
00037
00038 if __name__ == '__main__':
00039
00040 rospy.init_node('waypoint_generator')
00041 filename = rospy.get_param('~filename', 'output.txt')
00042
00043 g = WaypointGenerator(filename)
00044 rospy.loginfo('Initialized')
00045 g.spin()
00046 rospy.loginfo('ByeBye')