#!/usr/bin/env python

import rospy
from turtlesim.msg import Pose

from drawing_control.srv import DrawFile
from drawing_control.laser_ctrl1 import LaserController1

def listener():

    # In ROS, nodes are uniquely named. If two nodes with the same
    # name are launched, the previous one is kicked off. The
    # anonymous=True flag means that rospy will choose a unique
    # name for our 'listener' node so that multiple listeners can
    # run simultaneously.
    rospy.init_node('laser_controller1', anonymous=True)

    topic_base = '{}/'.format(rospy.get_name())

    base_vel = rospy.get_param("~base_vel")
    calibration_history_len = rospy.get_param("~calibration_history_len", 100)
    resolution = rospy.get_param("~resolution", 0.05)
    cmd_spacing = rospy.get_param("~cmd_spacing", 0.5)
    pose_topic = rospy.get_param("~pose_in_topic")
    position_service = rospy.get_param("~position_service")

    ctrl = LaserController1(base_vel, cmd_spacing, resolution, calibration_history_len, position_service)

    rospy.Service(topic_base + 'draw_file', DrawFile, ctrl.draw_file)

    rospy.Subscriber(pose_topic, Pose, ctrl.pose_callback)
    

    # spin() simply keeps python from exiting until this node is stopped
    rospy.spin()

if __name__ == '__main__':
    listener()
