#!/usr/bin/env python

PACKAGE = 'base_local_planner'

from math import pi

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

gen = ParameterGenerator()

# gen.add("inscribed_radius", double_t, 0, "The radius of the inscribed circle of the robot", 1, 0)
# gen.add("circumscribed_radius", double_t, 0, "The radius of the circumscribed circle of the robot", 1, 0)

gen.add("acc_lim_x", double_t, 0, "The acceleration limit of the robot in the x direction", 2.5, 0, 20.0)
gen.add("acc_lim_y", double_t, 0, "The acceleration limit of the robot in the y direction", 2.5, 0, 20.0)
gen.add("acc_lim_theta", double_t, 0, "The acceleration limit of the robot in the theta direction", 3.2, 0, 20.0)

gen.add("max_vel_x", double_t, 0, "The maximum x velocity for the robot in m/s", 0.55, 0, 20.0)
gen.add("min_vel_x", double_t, 0, "The minimum x velocity for the robot in m/s", 0.0, 0, 20.0)

gen.add("max_vel_theta", double_t, 0, "The absolute value of the maximum rotational velocity for the robot in rad/s",  1.0, 0, 20.0)
gen.add("min_vel_theta", double_t, 0, "The absolute value of the minimum rotational velocity for the robot in rad/s", -1.0, -20.0, 0.0)
gen.add("min_in_place_vel_theta", double_t, 0, "The absolute value of the minimum in-place rotational velocity the controller will explore", 0.4, 0, 20.0)

gen.add("sim_time", double_t, 0, "The amount of time to roll trajectories out for in seconds", 1.7, 0, 10)
gen.add("sim_granularity", double_t, 0, "The granularity with which to check for collisions along each trajectory in meters", 0.025, 0, 5)
gen.add("angular_sim_granularity", double_t, 0, "The distance between simulation points for angular velocity should be small enough that the robot doesn't hit things", 0.025, 0, pi/2)

gen.add("path_distance_bias", double_t, 0, "The weight for the path distance part of the cost function", 0.6, 0, 5)
gen.add("goal_distance_bias", double_t, 0, "The weight for the goal distance part of the cost function", 0.8, 0, 5)
gen.add("occdist_scale", double_t, 0, "The weight for the obstacle distance part of the cost function", 0.01, 0, 5)

gen.add("oscillation_reset_dist", double_t, 0, "The distance the robot must travel before oscillation flags are reset, in meters", 0.05, 0, 5)
gen.add("escape_reset_dist", double_t, 0, "The distance the robot must travel before oscillation flags are reset, in meters", 0.10, 0, 5)
gen.add("escape_reset_theta", double_t, 0, "The distance the robot must travel before oscillation flags are reset, in meters", pi/2, 0, 5)

gen.add("vx_samples", int_t, 0, "The number of samples to use when exploring the x velocity space", 20, 1, 300)
gen.add("vtheta_samples", int_t, 0, "The number of samples to use when exploring the theta velocity space", 20, 1, 300)

gen.add("heading_lookahead", double_t, 0, "How far the robot should look ahead of itself when differentiating between different rotational velocities", 0.325, 0, 5)

gen.add("holonomic_robot", bool_t, 0, "Set this to true if the robot being controlled can take y velocities and false otherwise", True)

gen.add("escape_vel", double_t, 0, "The velocity to use while backing up", -0.1, -2, 2)

gen.add("dwa", bool_t, 0, "Set this to true to use the Dynamic Window Approach, false to use acceleration limits", False)

gen.add("heading_scoring", bool_t, 0, "Set this to true to use the Dynamic Window Approach, false to use acceleration limits", False)
gen.add("heading_scoring_timestep", double_t, 0, "How far to look ahead in time when we score heading based trajectories", 0.1, 0, 1)

gen.add("simple_attractor", bool_t, 0, "Set this to true to allow simple attraction to a goal point instead of intelligent cost propagation", False)

gen.add("y_vels", str_t, 0, "A comma delimited list of the y velocities the controller will explore", "-0.3,-0.1,0.1,-0.3")

gen.add("restore_defaults",  bool_t, 0, "Retore to the default configuration", False)

exit(gen.generate(PACKAGE, "base_local_planner", "BaseLocalPlanner"))