#!/usr/bin/env python
# DWA Planner configuration

from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, double_t, int_t, bool_t
from local_planner_limits import add_generic_localplanner_params

gen = ParameterGenerator()

# This unusual line allows to reuse existing parameter definitions
# that concern all localplanners
add_generic_localplanner_params(gen)

gen.add("sim_time", double_t, 0, "The amount of time to roll trajectories out for in seconds", 1.7, 0)
gen.add("sim_granularity", double_t, 0, "The granularity with which to check for collisions along each trajectory in meters", 0.025, 0)
gen.add("angular_sim_granularity", double_t, 0, "The granularity with which to check for collisions for rotations in radians", 0.1, 0)

gen.add("path_distance_bias", double_t, 0, "The weight for the path distance part of the cost function", 0.6, 0.0)
gen.add("goal_distance_bias", double_t, 0, "The weight for the goal distance part of the cost function", 0.8, 0.0)
gen.add("occdist_scale", double_t, 0, "The weight for the obstacle distance part of the cost function", 0.01, 0.0)
gen.add("twirling_scale", double_t, 0, "The weight for penalizing any changes in robot heading", 0.0, 0.0)

gen.add("stop_time_buffer", double_t, 0, "The amount of time that the robot must stop before a collision in order for a trajectory to be considered valid in seconds", 0.2, 0)
gen.add("oscillation_reset_dist", double_t, 0, "The distance the robot must travel before oscillation flags are reset, in meters", 0.05, 0)
gen.add("oscillation_reset_angle", double_t, 0, "The angle the robot must turn before oscillation flags are reset, in radians", 0.2, 0)

gen.add("forward_point_distance", double_t, 0, "The distance from the center point of the robot to place an additional scoring point, in meters", 0.325)

gen.add("scaling_speed", double_t, 0, "The absolute value of the velocity at which to start scaling the robot's footprint, in m/s", 0.25, 0)
gen.add("max_scaling_factor", double_t, 0, "The maximum factor to scale the robot's footprint by", 0.2, 0)

gen.add("vx_samples", int_t, 0, "The number of samples to use when exploring the x velocity space", 3, 1)
gen.add("vy_samples", int_t, 0, "The number of samples to use when exploring the y velocity space", 10, 1)
gen.add("vth_samples", int_t, 0, "The number of samples to use when exploring the theta velocity space", 20, 1)

gen.add("use_dwa", bool_t, 0, "Use dynamic window approach to constrain sampling velocities to small window.", True)

gen.add("restore_defaults", bool_t, 0, "Restore to the original configuration.", False)

exit(gen.generate("dwa_local_planner", "dwa_local_planner", "DWAPlanner"))