#!/usr/bin/env python

PACKAGE = 'amcl'

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

gen = ParameterGenerator()

gen.add("min_particles", int_t, 0, "Minimum allowed number of particles.", 100, 0, 1000)
gen.add("max_particles", int_t, 0, "Mamimum allowed number of particles.", 5000, 0, 10000)

gen.add("kld_err",  double_t, 0, "Maximum error between the true distribution and the estimated distribution.", .01, 0, 1)
gen.add("kld_z", double_t, 0, "Upper standard normal quantile for (1 - p), where p is the probability that the error on the estimated distribution will be less than kld_err.", .99, 0, 1)

gen.add("update_min_d", double_t, 0, "Translational movement required before performing a filter update.", .2, 0, 5)
gen.add("update_min_a", double_t, 0, "Rotational movement required before performing a filter update.", pi/6, 0, 2*pi)

gen.add("resample_interval", int_t, 0, "Number of filter updates required before resampling.", 2, 0, 20)

gen.add("transform_tolerance", double_t, 0, "Time with which to post-date the transform that is published, to indicate that this transform is valid into the future.", .1, 0, 2)

gen.add("recovery_alpha_slow", double_t, 0, "Exponential decay rate for the slow average weight filter, used in deciding when to recover by adding random poses. A good value might be 0.001.", 0, 0, .5)
gen.add("recovery_alpha_fast", double_t, 0, "Exponential decay rate for the fast average weight filter, used in deciding when to recover by adding random poses. A good value might be 0.1.", 0, 0, 1)

gen.add("do_beamskip", bool_t, 0, "When true skips laser scans when a scan doesnt work for a majority of particles", False)
gen.add("beam_skip_distance", double_t, 0, "Distance from a valid map point before scan is considered invalid", 0.5, 0, 2)
gen.add("beam_skip_threshold", double_t, 0, "Ratio of samples for which the scans are valid to consider as valid scan", 0.3, 0, 1)

gen.add("tf_broadcast", bool_t, 0, "When true (the default), publish results via TF.  When false, do not.", True)
gen.add("gui_publish_rate", double_t, 0, "Maximum rate (Hz) at which scans and paths are published for visualization, -1.0 to disable.", -1, -1, 100)
gen.add("save_pose_rate", double_t, 0, "Maximum rate (Hz) at which to store the last estimated pose and covariance to the parameter server, in the variables ~initial_pose_* and ~initial_cov_*. This saved pose will be used on subsequent runs to initialize the filter. -1.0 to disable.", .5, -1, 10)

gen.add("use_map_topic", bool_t, 0, "When set to true, AMCL will subscribe to the map topic rather than making a service call to receive its map.", False)
gen.add("first_map_only", bool_t, 0, "When set to true, AMCL will only use the first map it subscribes to, rather than updating each time a new one is received.", False)

# Laser Model Parameters
gen.add("laser_min_range", double_t, 0, "Minimum scan range to be considered; -1.0 will cause the laser's reported minimum range to be used.", -1, -1, 1000)
gen.add("laser_max_range", double_t, 0, "Maximum scan range to be considered; -1.0 will cause the laser's reported maximum range to be used.", -1, -1, 1000)

gen.add("laser_max_beams", int_t, 0, "How many evenly-spaced beams in each scan to be used when updating the filter.", 30, 0, 250)

gen.add("laser_z_hit", double_t, 0, "Mixture weight for the z_hit part of the model.", .95, 0, 1)
gen.add("laser_z_short", double_t, 0, "Mixture weight for the z_short part of the model.", .1, 0, 1)
gen.add("laser_z_max", double_t, 0, "Mixture weight for the z_max part of the model.", .05, 0, 1)
gen.add("laser_z_rand", double_t, 0, "Mixture weight for the z_rand part of the model.", .05, 0, 1)

gen.add("laser_sigma_hit", double_t, 0, "Standard deviation for Gaussian model used in z_hit part of the model.", .2, 0, 10)
gen.add("laser_lambda_short", double_t, 0, "Exponential decay parameter for z_short part of model.", .1, 0, 10)
gen.add("laser_likelihood_max_dist", double_t, 0, "Maximum distance to do obstacle inflation on map, for use in likelihood_field model.", 2, 0, 20)

lmt = gen.enum([gen.const("beam_const", str_t, "beam", "Use beam laser model"), gen.const("likelihood_field_const", str_t, "likelihood_field", "Use likelihood_field laser model"), gen.const("likelihood_field_prob", str_t, "likelihood_field_prob", "Use likelihood_field_prob laser model")], "Laser Models")
gen.add("laser_model_type", str_t, 0, "Which model to use, either beam, likelihood_field or likelihood_field_prob.", "likelihood_field", edit_method=lmt)

# Odometry Model Parameters
odt = gen.enum([gen.const("diff_const", str_t, "diff", "Use diff odom model"),
                gen.const("omni_const", str_t, "omni", "Use omni odom model"),
                gen.const("diff_corrected_const", str_t, "diff-corrected", "Use corrected diff odom model"),
                gen.const("omni_corrected_const", str_t, "omni-corrected", "Use corrected omni odom model")],
               "Odom Models")
gen.add("odom_model_type", str_t, 0, "Which model to use, diff, omni, diff-corrected, or omni-corrected", "diff", edit_method=odt)

gen.add("odom_alpha1", double_t, 0, "Specifies the expected noise in odometry's rotation estimate from the rotational component of the robot's motion.", .2, 0, 10)
gen.add("odom_alpha2", double_t, 0, "Specifies the expected noise in odometry's rotation estimate from the translational component of the robot's motion.", .2, 0, 10)
gen.add("odom_alpha3", double_t, 0, "Specifies the expected noise in odometry's translation estimate from the translational component of the robot's motion.", .2, 0, 10)
gen.add("odom_alpha4", double_t, 0, "Specifies the expected noise in odometry's translation  estimate from the rotational component of the robot's motion.", .2, 0, 10)
gen.add("odom_alpha5", double_t, 0, "Translation-related noise parameter (only used if model is omni).", .2, 0, 10)

gen.add("odom_frame_id", str_t, 0, "Which frame to use for odometry.", "odom")
gen.add("base_frame_id", str_t, 0, "Which frame to use for the robot base.", "base_link")
gen.add("global_frame_id", str_t, 0, "The name of the coordinate frame published by the localization system.", "map")

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

exit(gen.generate(PACKAGE, "amcl_node", "AMCL"))