#!/usr/bin/env python
PACKAGE = 'docking'

from dynamic_reconfigure.parameter_generator_catkin import *

gen = ParameterGenerator()

gen.add("world_frame",    str_t,    0, "Name of world frame",  "world")
gen.add("robot_frame",    str_t,    0, "Name of robot frame",  "base_link")
gen.add("dock_frame",    str_t,    0, "Name of dock frame",  "dock_truth")
gen.add("dock_pose_topic",    str_t,    0, "Topic of calculated dock pose",  "docking/dock_pose")
gen.add("dock_gazebo_pose_topic",    str_t,    0, "Topic of gazebo truth dock pose",  "dock_pose_gazebo")
gen.add("use_calculated_pose",    bool_t,    0, "Switch between gazebo truth pose or calculated pose",  True)
gen.add("goal_dist_tolerance", double_t, 0, "Max Goal Distance Error Tolerance (m)", 0.03, 0.01, 1)
gen.add("goal_orientation_tolerance", double_t, 0, "Max Goal Orientation Error Tolerance (rad)", 0.1, 0.01, 1)
gen.add("use_steps",   bool_t,   0, "Limit number of steps",  False)
gen.add("path_steps", int_t, 0, "Steps in pose path", 10, 1, 369)
gen.add("max_steps", int_t, 0, "Max steps in pose path", 10, 1, 10000)
gen.add("time_step", double_t, 0, "Time per step in pose path", 0.1, 0.1, 1)
gen.add("v", double_t, 0, "Linear Velocity", 0.1 , 0, 1)
gen.add("omega_max", double_t, 0, "Max Rotational Velocity", 0.3, 0, 2)
gen.add("kPhi", double_t, 0, "Rate of Change of Theta to R", 20, 0,   100)
gen.add("kDelta", double_t, 0, "Rate to Converge to Slow Subsystem", 1,  0, 20)
gen.add("curvature_max", double_t, 0, "Max Curvature ", 2,  0, 20)
gen.add("beta", double_t, 0, "LinVel Gain Inversely Proportional to Curvature", 0.2,  0, 1)
gen.add("lambda", double_t, 0, "LinVel Gain Exponentially Inversely Proportional to Curvature", 2,  0, 5)

gen.add("entrance_dist", double_t, 0, "Distance to entrance checkpoint from inner dock", 0.3, -0.01, 1)
gen.add("frequency", int_t, 0, "Controller frequency", 10, 1, 30)

exit(gen.generate(PACKAGE, "docking", "PlannerNode"))
