#!/usr/bin/env python

PACKAGE = 'bitbots_dynup'
import roslib

roslib.load_manifest(PACKAGE)

from dynamic_reconfigure.parameter_generator_catkin import *

gen = ParameterGenerator()

group_engine = gen.add_group("Engine", type="tab")
group_engine.add("engine_rate", int_t, 1,
                 "How often the engine updates motor goals",
                 100, min=1, max=500)
group_engine.add("foot_distance", double_t, 2,
                 "How far apart the feet should be from each other. This is only relevant during the initial phase "
                 "when the foot is not yet moving towards the ball",
                 0.18)
group_engine.add("rise_time", double_t, 2,
                 "Time to rise and shine! [s]",
                 1.0)
group_engine.add("trunk_height", double_t, 2,
                 "Height of the trunk at the end [m]",
                 0.487)
group_engine.add("trunk_x", double_t, 2,
                 "Height of the trunk at the end [m]",
                 0.016)
group_engine.add("trunk_pitch", double_t, 2,
                 "Pitch of the trunk at the end [rad]",
                 0.1)

group_stabilizing = gen.add_group("Stabilizer", type="tab")
group_stabilizing.add("stabilizing", bool_t, 4,
                      "Whether to use automatic stabilizing or not",
                      True)
group_stabilizing.add("minimal_displacement", bool_t, 4,
                      "Try to stabilize with as little movement as possible",
                      False)
group_stabilizing.add("stabilizing_weight", double_t, 4,
                      "How important bio_ik should regard keeping the robot in a stable position",
                      0.44, min=0, max=1)
group_stabilizing.add("flying_weight", double_t, 4,
                      "How important bio_ik should regard the flying foots position",
                      0.5, min=0, max=1)
group_stabilizing.add("trunk_orientation_weight", double_t, 4,
                      "How important bio_ik should regard keeping the trunk orientation",
                      1, min=0, max=1)
group_stabilizing.add("stabilizing_point_x", double_t, 4,
                      "Where to stabilize over in x direction. Measured from the support foots *_sole frame",
                      0.015, min=-0.5, max=0.5)
group_stabilizing.add("stabilizing_point_y", double_t, 4,
                      "Where to stabilize over in y direction. Measured from the support foots *_sole frame. "
                      "Be aware that - always represents the direction towards base_footprint",
                      -0.023, min=-0.5, max=0.5)

exit(gen.generate(PACKAGE, 'dynup', 'DynUp'))
