Reconfigure path planner on the fly

[ebretl]

Goal
We want to be able to change the parameters of the path planner on the fly so that we can tune it's performance faster. The planner has many parameters, from the planning horizon, to the vehicle dynamics, to weights in the cost function, to the optimization hyper-parameters. Probably the easiest and best way to manage this tuning is using dynamic_reconfigure, a ROS package distributed by the ROS developers.

Notes
dynamic_reconfigure consists of several parts. First, a .cfg file specifies the configurable fields, their types, and their valid ranges. Second, a GUI uses these configurations to populate sliders and drop-downs and to save sets of values. Third, the configured node must have a callback function to receive the parameter adjustments and apply them to the algorithm.

Planner parameters that we want to tune

• Path representation
  • Number of path segments (1-10)
  • Discrete step size for forward path simulation (0.01-1.0)
  • Number of discrete steps per path segment (1-100)
• Vehicle model
  • Max forward acceleration (0.5-20.0)
  • Max lateral acceleration (0.5-10.0)
  • Steering rate (0.1-30.0)
  • Max steering angle (0.01-1.5)
  • Slip compensation gain (0.8-1.5)
• Cost function coefficients
  • Speed (0.0-10.0)
  • Obstacle clearance (0.0-10.0)
  • Heading (0.0-10.0)
  • Forward distance (0.0-100.0)
  • Collision (0.0-500.0)
• Simulated annealing hyper-parameters
  • Start temperature (0.0-1.0)
  • End temperature (0.0-0.1)
  • Iterations (10-5000)
  • Cost scale factor (0.0-1.0)

Things to create

• dynamic_reconfigure cfg file (rr_common/cfg/planner_configuration.cfg)

Things to edit

• Add dynamic reconfigure server and callback to rr_common/src/planner/planner_node.cpp
• Adjust AnnealingPlanner and BicycleModel such that their parameters can be set (perhaps as a bunch of getter/setter methods, or as one function that takes in a struct of params to set)
• Save a YAML file from rqt_reconfigure
• Edit rr_iarrc’s test_circuit_sim.launch so that it uses “dynparam load” to set the configuration from a this YAML file