generator.py

Go to the documentation of this file.

#!/usr/bin/env python

import yaml
import rospy
import geometry_msgs.msg as geometry_msgs

class WaypointGenerator(object):

    def __init__(self, filename):
        self._sub_pose = rospy.Subscriber('clicked_point', geometry_msgs.PointStamped, self._process_pose, queue_size=1)
        self._waypoints = []
        self._filename = filename

    def _process_pose(self, msg):
        p = msg.point

        data = {}
        data['frame_id'] = msg.header.frame_id
        data['pose'] = {}
        data['pose']['position'] = {'x': p.x, 'y': p.y, 'z': 0.0}
        data['pose']['orientation'] = {'x': 0, 'y': 0, 'z': 0, 'w':1}
        data['name'] = '%s_%s' % (p.x, p.y)

        self._waypoints.append(data)
        rospy.loginfo("Clicked : (%s, %s, %s)" % (p.x, p.y, p.z))

    def _write_file(self):
        ways = {}
        ways['waypoints'] = self._waypoints
        with open(self._filename, 'w') as f:
            f.write(yaml.dump(ways, default_flow_style=False))

    def spin(self):
        rospy.spin()
        self._write_file()


if __name__ == '__main__':

    rospy.init_node('waypoint_generator')
    filename = rospy.get_param('~filename', 'output.txt')

    g = WaypointGenerator(filename)
    rospy.loginfo('Initialized')
    g.spin()
    rospy.loginfo('ByeBye')