def main(argv):
    rospy.init_node('head_laser_servo_tf')
    tty_device = rospy.get_param('~tty_device', '/dev/ttyACM0')
    acceleration = rospy.get_param('~acceleration', 20)
    speed = rospy.get_param('~speed', 10)
    min_val = rospy.get_param('~min_val', (885 * 4))
    min_deg = rospy.get_param('~min_deg', (- 90))
    max_val = rospy.get_param('~max_val', (1900 * 4))
    max_deg = rospy.get_param('~max_deg', 0)
    default_deg = rospy.get_param('~default_deg', (- 90))
    fixed_frame = rospy.get_param('~fixed_frame', 'head_laser_servo_base')
    rotating_frame = rospy.get_param('~rotating_frame', 'head_laser_servo_mount')
    time_adj = rospy.get_param('~time_adj', 0.125)
    tf_pub_rate = rospy.get_param('~tf_pub_rate', 20)
    dev = PololuSerial(tty_device)
    dev.SetAcceleration(acceleration)
    dev.SetSpeed(speed)
    tf_broadcaster = tf.TransformBroadcaster()
    disable_tf_publisher = [False]
    latest_deg = [min_deg]

    def MoveToDeg(target_deg):
        target = int(((((target_deg - min_deg) / (max_deg - min_deg)) * (max_val - min_val)) + min_val))
        dev.SetTarget(target)
        pos = float(dev.GetPos())
        disable_tf_publisher[0] = True
        while (pos != target):
            deg = ((((pos - min_val) / (max_val - min_val)) * (max_deg - min_deg)) + min_deg)
            tf_broadcaster.sendTransform((0, 0, 0), tf.transformations.quaternion_from_euler(0, (((- deg) / 180.0) * math.pi), 0), (rospy.Time.now() + rospy.Duration(time_adj)), rotating_frame, fixed_frame)
            latest_deg[0] = deg
            rospy.loginfo('Degree: %f, Value: %f', deg, pos)
            pos = float(dev.GetPos())
        disable_tf_publisher[0] = False

    def HeadLaserAngleCallback(data):
        angle = data.data
        if ((angle < min_deg) or (angle > max_deg)):
            rospy.logerr('%f is not between [%f, %f]', angle, min_deg, max_deg)
        else:
            MoveToDeg(angle)
    MoveToDeg(min_deg)
    MoveToDeg(max_deg)
    MoveToDeg(default_deg)
    rospy.Subscriber('/head_laser/angle', std_msgs.msg.Float64, HeadLaserAngleCallback)
    rospy.loginfo('Ready to serve.')
    tf_pub_rate_rospy_rate = rospy.Rate(tf_pub_rate)
    while (not rospy.is_shutdown()):
        if (not disable_tf_publisher[0]):
            tf_broadcaster.sendTransform((0, 0, 0), tf.transformations.quaternion_from_euler(0, (((- latest_deg[0]) / 180.0) * math.pi), 0), (rospy.Time.now() + rospy.Duration(time_adj)), rotating_frame, fixed_frame)
        tf_pub_rate_rospy_rate.sleep()
    dev.Close()
def main(argv):
    rospy.init_node('head_laser_servo_tf')
    tty_device = rospy.get_param('~tty_device', '/dev/ttyACM0')
    acceleration = rospy.get_param('~acceleration', 20)
    speed = rospy.get_param('~speed', 10)
    min_val = rospy.get_param('~min_val', (885 * 4))
    min_deg = rospy.get_param('~min_deg', (- 90))
    max_val = rospy.get_param('~max_val', (1900 * 4))
    max_deg = rospy.get_param('~max_deg', 0)
    default_deg = rospy.get_param('~default_deg', (- 90))
    fixed_frame = rospy.get_param('~fixed_frame', 'head_laser_servo_base')
    rotating_frame = rospy.get_param('~rotating_frame', 'head_laser_servo_mount')
    time_adj = rospy.get_param('~time_adj', 0.125)
    tf_pub_rate = rospy.get_param('~tf_pub_rate', 20)
    dev = PololuSerial(tty_device)
    dev.SetAcceleration(acceleration)
    dev.SetSpeed(speed)
    tf_broadcaster = tf.TransformBroadcaster()
    disable_tf_publisher = [False]
    latest_deg = [min_deg]

    def MoveToDeg(target_deg):
        target = int(((((target_deg - min_deg) / (max_deg - min_deg)) * (max_val - min_val)) + min_val))
        dev.SetTarget(target)
        pos = float(dev.GetPos())
        disable_tf_publisher[0] = True
        while (pos != target):
            deg = ((((pos - min_val) / (max_val - min_val)) * (max_deg - min_deg)) + min_deg)
            tf_broadcaster.sendTransform((0, 0, 0), tf.transformations.quaternion_from_euler(0, (((- deg) / 180.0) * math.pi), 0), (rospy.Time.now() + rospy.Duration(time_adj)), rotating_frame, fixed_frame)
            latest_deg[0] = deg
            rospy.loginfo('Degree: %f, Value: %f', deg, pos)
            pos = float(dev.GetPos())
        disable_tf_publisher[0] = False

    def HeadLaserAngleCallback(data):
        angle = data.data
        if ((angle < min_deg) or (angle > max_deg)):
            rospy.logerr('%f is not between [%f, %f]', angle, min_deg, max_deg)
        else:
            MoveToDeg(angle)
    MoveToDeg(min_deg)
    MoveToDeg(max_deg)
    MoveToDeg(default_deg)
    rospy.Subscriber('/head_laser/angle', std_msgs.msg.Float64, HeadLaserAngleCallback)
    rospy.loginfo('Ready to serve.')
    tf_pub_rate_rospy_rate = rospy.Rate(tf_pub_rate)
    while (not rospy.is_shutdown()):
        if (not disable_tf_publisher[0]):
            tf_broadcaster.sendTransform((0, 0, 0), tf.transformations.quaternion_from_euler(0, (((- latest_deg[0]) / 180.0) * math.pi), 0), (rospy.Time.now() + rospy.Duration(time_adj)), rotating_frame, fixed_frame)
        tf_pub_rate_rospy_rate.sleep()
    dev.Close()