generator.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 import yaml
4 import rospy
5 import geometry_msgs.msg as geometry_msgs
6 
7 class WaypointGenerator(object):
8 
9  def __init__(self, filename):
10  self._sub_pose = rospy.Subscriber('clicked_point', geometry_msgs.PointStamped, self._process_pose, queue_size=1)
11  self._waypoints = []
12  self._filename = filename
13 
14  def _process_pose(self, msg):
15  p = msg.point
16 
17  data = {}
18  data['frame_id'] = msg.header.frame_id
19  data['pose'] = {}
20  data['pose']['position'] = {'x': p.x, 'y': p.y, 'z': 0.0}
21  data['pose']['orientation'] = {'x': 0, 'y': 0, 'z': 0, 'w':1}
22  data['name'] = '%s_%s' % (p.x, p.y)
23 
24  self._waypoints.append(data)
25  rospy.loginfo("Clicked : (%s, %s, %s)" % (p.x, p.y, p.z))
26 
27  def _write_file(self):
28  ways = {}
29  ways['waypoints'] = self._waypoints
30  with open(self._filename, 'w') as f:
31  f.write(yaml.dump(ways, default_flow_style=False))
32 
33  def spin(self):
34  rospy.spin()
35  self._write_file()
36 
37 
38 if __name__ == '__main__':
39 
40  rospy.init_node('waypoint_generator')
41  filename = rospy.get_param('~filename', 'output.txt')
42 
43  g = WaypointGenerator(filename)
44  rospy.loginfo('Initialized')
45  g.spin()
46  rospy.loginfo('ByeBye')
def _process_pose(self, msg)
Definition: generator.py:14
def __init__(self, filename)
Definition: generator.py:9


waypoint_generator
Author(s):
autogenerated on Mon Jun 10 2019 15:49:14