#!/usr/bin/env python
PACKAGE = "reactive_planner"

from dynamic_reconfigure.parameter_generator_catkin import *

gen = ParameterGenerator()

attract = gen.add_group("Attract")
attract.add("c11", double_t, 0, "attract_theta = kg1*theta0*exp(-c11*dg + c21)", 0.001, 0, 1.5)
attract.add("c21", double_t, 0, "attract_theta = kg1*theta0*exp(-c11*dg + c21)", 0.3, 0, 1.5)
attract.add("c12", double_t, 0, "attract_phi = kg1*theta0*exp(-c12*dg + C22)", 0.1, 0, 50.0)
attract.add("c22", double_t, 0, "attract_phi = kg1*theta0*exp(-c12*dg + C22)", 0.1, 0, 50.0)
attract.add("kg1", double_t, 0, "attract_theta = kg1*theta0*exp(-c11*dg + c21)", 5.0, 0, 100.0)
attract.add("kg2", double_t, 0, "attract_phi = kg2*phi0*exp(-c12*dg + c22)", 0.1, 0, 50.0)

repulse = gen.add_group("Repulse")
repulse.add("s1", double_t, 0, "repulse_t = sgn(t0) * sig(s1(1-abs(p0)/s2)) * exp(-c3*d0-c4*abs(t0))", 0.1, 0, 50.0)
repulse.add("s2", double_t, 0, "repulse_t = sgn(t0) * sig(s1(1-abs(p0)/s2)) * exp(-c3*d0-c4*abs(t0))", 0.1, 0, 50.0)
repulse.add("t1", double_t, 0, "repulse_phi = sgn(p0) * sig(t1(1-abs(t0)/t2)) * exp(-c3*d0-c4*abs(p0))", 0.1, 0, 50.0)
repulse.add("t2", double_t, 0, "repulse_phi = sgn(p0) * sig(t1(1-abs(t0)/t2)) * exp(-c3*d0-c4*abs(p0))", 0.1, 0, 50.0)
repulse.add("c3", double_t, 0, "repulse_phi = sgn(p0) * sig(t1(1-abs(t0)/t2)) * exp(-c3*d0-c4*abs(p0))", 1.0, 0, 5.0)
repulse.add("c4", double_t, 0, "repulse_phi = sgn(p0) * sig(t1(1-abs(t0)/t2)) * exp(-c3*d0-c4*abs(p0))", 1.0, 0, 5.0)
repulse.add("ko1", double_t, 0, "repulse_theta_gain", 35.0, 0, 100.0)
repulse.add("ko2", double_t, 0, "repulse phi gain", 0.2, 0, 50.0)

forward = gen.add_group("Forward")
forward.add("v0", double_t, 0, "nominal forward speed", 4.8, 0, 40)
forward.add("k_speed", double_t, 0, "gain on speed", 1.0, 0, 1.0)
forward.add("horizontal_search", double_t, 0, "horizontal range to search (rad)", 10.0*3.14519/180.0, 0, 3.14159/2.0)
forward.add("vertical_search", double_t, 0, "vertical range to search (rad)", 10.0*3.14519/180.0, 0, 3.14159/2.0)

horizontal = gen.add_group("horizontal")
horizontal.add("k_strafe", double_t, 0, "gain on strafing", 0.2, 0, 1.0)
horizontal.add("max_strafe", double_t, 0, "max strafing speed (m/s)", 8.0, 0, 20.0)

yaw = gen.add_group("yaw")
yaw.add("max_yaw_rate", double_t, 0, "max commanded yaw rate (rad/s)", 6.4, 0, 20)

exit(gen.generate(PACKAGE, "reactive_planner", "Planner"))
