#!/usr/bin/env python

PACKAGE = 'pose_follower'

from math import pi

from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, double_t, int_t, bool_t, str_t

gen = ParameterGenerator()

gen.add("max_vel_lin", double_t, 0, "Maximum linear velocity in m/s", 0.9, 0.0, 10.0)
gen.add("max_vel_th", double_t, 0, "Maximum angular velocity in rad/s", 1.4, 0.0, 10.0)
gen.add("min_vel_lin", double_t, 0, "Minimum linear velocity for the robot in m/s", 0.1, 0, 10.0)
gen.add("min_vel_th", double_t, 0, "Minimum angular velocity for the robot in rad/s", 0.0, 0, 10.0)
gen.add("min_in_place_vel_th", double_t, 0, "If we're rotating in place, go at least this fast to avoid getting stuck", 0.0, 0, 10.0)
gen.add("in_place_trans_vel", double_t, 0, "When we're near the end and would be trying to go no faster than this translationally, just rotate in place instead", 0.0, 0, 10.0)
gen.add("trans_stopped_velocity", double_t, 0, "The robot is stopped if the velocities are lower than this", 1e-4, 0, 10.0)
gen.add("rot_stopped_velocity", double_t, 0, "The robot is stopped if the velocities are lower than this", 1e-4, 0, 10.0)

gen.add("tolerance_trans", double_t, 0, "Translational tolerance for the goal", 0.02, 0, 10.0)
gen.add("tolerance_rot", double_t, 0, "Angular tolerance for the goal", 0.04, 0, pi)
gen.add("tolerance_timeout", double_t, 0, "We've reached our goal only if we're within range for this long and stopped", 0.5, 0, 20.0)

gen.add("samples", int_t, 0, "Number of samples (scaling factors of our current desired twist)", 10, 0, 20)
gen.add("allow_backwards", bool_t, 0, "Allow backwards movement", False)
gen.add("turn_in_place_first", bool_t, 0, "If true, turn in place to face the new goal instead of arching towards it", False)
gen.add("max_heading_diff_before_moving", double_t, 0, "If turn_in_place_first is true, turn in place if our heading is more than this far from facing the goal location", 0.17, 0, pi)


gen.add("k_trans", double_t, 0, "Gain factor for translation component of output velocities", 2.0, 0.0, 20.0)
gen.add("k_rot", double_t, 0, "Gain factor for rotation component of output velocities", 2.0, 0.0, 20.0)

exit(gen.generate(PACKAGE, "pose_follower", "PoseFollower"))
