#!/usr/bin/env python

PACKAGE = 'laser_scan_matcher'

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

gen = ParameterGenerator()

# Scan matching 
gen.add("max_angular_correction_deg", double_t, 0, "max_angular_correction_deg", 45.0, 0, 360.0)
gen.add("max_linear_correction", double_t, 0, "max_linear_correction", 0.5, 0, 5.0)
gen.add("max_iterations", int_t, 0, "max_iterations", 10, 0, 20)
gen.add("epsilon_xy", double_t, 0, "epsilon_xy", 0.000001, 0, 0.00001)
gen.add("epsilon_theta", double_t, 0, "epsilon_theta", 0.000001, 0, 0.00001)
gen.add("max_correspondence_dist", double_t, 0, "max_correspondence_dist", 0.3, 0, 1.0)
gen.add("sigma", double_t, 0, "sigma", 0.010, 0, 0.1)
gen.add("use_corr_tricks", int_t, 0, "use_corr_tricks", 1, 0, 5)
#gen.add("restart", int_t, 0, "restart", 0, 0, 1)
gen.add("restart_threshold_mean_error", double_t, 0, "restart_threshold_mean_error", 0.01, 0, 0.1)
gen.add("restart_dt", double_t, 0, "restart_dt", 1.0, 0, 5.0)
gen.add("restart_dtheta", double_t, 0, "restart_dtheta", 0.1, 0, 1.0)
gen.add("clustering_threshold", double_t, 0, "clustering_threshold", 0.25, 0, 1.0)
gen.add("orientation_neighbourhood", int_t, 0, "orientation_neighbourhood", 20, 0, 40)
gen.add("use_point_to_line_distance", int_t, 0, "use_point_to_line_distance", 1, 0, 10)
gen.add("do_alpha_test", int_t, 0, "do_alpha_test", 0, 0, 1)
gen.add("do_alpha_test_thresholdDeg", double_t, 0, "do_alpha_test_thresholdDeg", 20.0, 0, 360.0)
gen.add("outliers_maxPerc", double_t, 0, "outliers_maxPerc", 0.90, 0, 1.00)
gen.add("outliers_adaptive_order", double_t, 0, "outliers_adaptive_order", 0.7, 0, 1.0)
gen.add("outliers_adaptive_mult", double_t, 0, "outliers_adaptive_mult", 2.0, 0, 3.0)
gen.add("do_visibility_test", int_t, 0, "do_visibility_test", 0, 0, 1)
gen.add("outliers_remove_doubles", int_t, 0, "outliers_remove_doubles", 1, 0, 3)
gen.add("do_compute_covariance", int_t, 0, "do_compute_covariance", 0, 0, 1)
gen.add("debug_verify_tricks", int_t, 0, "debug_verify_tricks", 0, 0, 1)
gen.add("use_ml_weights", int_t, 0, "use_ml_weights", 0, 0, 1)
gen.add("use_sigma_weights", int_t, 0, "use_sigma_weights", 0, 0, 1)

# Coordinate frames
gen.add("base_frame", str_t, 0, "The fixed frame .", "base_link")
gen.add("fixed_frame", str_t, 0, "The base frame of the robot .", "world")

# Pointcloud input
gen.add("use_cloud_input", bool_t, 0, "use_cloud_input", False)
gen.add("cloud_range_min", double_t, 0, "cloud_range_min", 0.1, 0, 100.0)
gen.add("cloud_range_max", double_t, 0, "cloud_range_max", 50.0, 0, 1000.0)

# Keyframes
gen.add("kf_dist_linear", double_t, 0, "kf_dist_linear", 0.01, 0, 100.00)
gen.add("kf_dist_angular", double_t, 0, "kf_dist_angular", 0.175, 0, 6.28)

# Motion predition
gen.add("use_imu", bool_t, 0, "use_imu", True)
gen.add("use_odom", bool_t, 0, "use_odom", True)
gen.add("use_vel", bool_t, 0, "use_vel", False)

# Output
gen.add("publish_tf", bool_t, 0, "publish_tf", True)
gen.add("publish_pose", bool_t, 0, "publish_pose", True)
gen.add("publish_pose_stamped", bool_t, 0, "publish_pose_stamped", False)
gen.add("publish_pose_with_covariance", bool_t, 0, "publish_pose_with_covariance", False)
gen.add("publish_pose_with_covariance_stamped", bool_t, 0, "publish_pose_with_covariance_stamped", False)


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

exit(gen.generate(PACKAGE, "laser_scan_matcher_node", "CSM"))
