Re-enable runtime configuration of key parameters

[crdelsey]

Feature request

Feature description

The lifecycle changes disabled all runtime reconfiguration of parameters. However, certain key parameters need to be dynamically adjusted to allow users to easily tune the stack for their use cases.

These key parameters need to be identified and support added to make them changeable while in the active state.

[bpwilcox]

Here is a class diagram I am proposing to address support for dynamic parameters in the stack.

[bpwilcox]

ros2/rclcpp#829 submitted to add additional support for parameter event subscriptions for dynamic parameter callbacks

[SteveMacenski]

Major TODO list (tunable behaviors / algorithms)

• DWB
• costmap layers (voxel, inflation, obstacle, range, static)
• costmap_2d Add dynamic parameters do Costmap2DROS #2592 (review)
• navfn [NavFn] Make the 3 parameters changeable at runtime #2181
• Theta* planner Add support for reconfiguring params using GUI in ThetaStarPlanner #2665
• smac planner 2D [SmacPlanner2D] make tolerance parameter dynamic #2475
• smac planner Hybrid Enabled runtime configuration of parameters for Hybrid A* #2540
• smac planner Lattice State Lattice Planner Work Revival #2582
• amcl
• regulated pure pursuit adding dynamic params to RPP and cleaningup style in others #2607
• Goal checkers
• Progress checkers

Minor TODO list (lower impact, unlikely to be actually needed to change for tuning)

• waypoint follower adding dynamic parameters support for controller, planner, and waypoint servers #2704
• BT navigator all parameters are integral on initialization for the behavior tree, no promise that user-driven plugins would accept / be aware of changes. These are all static set frames and libraries that would not change at runtime safely
• lifecycle manager safety shouldn't allow this, and all params are integral on initialization
• planner server adding dynamic parameters support for controller, planner, and waypoint servers #2704
• controller server adding dynamic parameters support for controller, planner, and waypoint servers #2704
• map server All parameters are overridable in the actual service requests to load / save so no need to overload defaults in dynamic parameters

[doisyg]

Moving the discussion from #2565

If you could offer a code snippet of how one of the simple changes would look like, that would help. You don't have to do every parameter in some node, just a couple to get the idea of what you're proposing looks like. The changing parameters APIs of ROS 2 keeps changing so my guess is that wasn't on the mind when we started. I know there's an API for registering callbacks for individual parameters (which I think this is asking for) but there's also a parameter filter object floating around too https://docs.ros2.org/foxy/api/rclcpp/classrclcpp_1_1ParameterEventsFilter.html. There was also a design discussion around topic filters for parameters so that it only triggers callbacks for the node of interest instead of all nodes, but that's not tangible yet.

Short term solution of a callback triggered only when a parameter is changed on the hosting node:
In the header:

  // Dynamic parameters handler
  OnSetParametersCallbackHandle::SharedPtr dyn_params_handler;

  /**
   * @brief Callback executed when a paramter change is detected
   * @param parameters list of changed parameters
   */
  rcl_interfaces::msg::SetParametersResult
    dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);

In the constructor:

  // Add callback for dynamic parameters
  dyn_params_handler = this->add_on_set_parameters_callback(
    std::bind(&Costmap2DROS::dynamicParametersCallback, this, _1));

Callback:

rcl_interfaces::msg::SetParametersResult
  Costmap2DROS::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters)
{
  auto result = rcl_interfaces::msg::SetParametersResult();
  for (auto parameter : parameters) {
    const auto & type = parameter.get_type();
    const auto & name = parameter.get_name();

    if (name == "my_param"
        && type == ParameterType::PARAMETER_DOUBLE) {
      my_param_ = parameter.as_double();
    }
  }
  result.successful = true;
  return result;
}

If you make these changes, may I suggest another change at the same time to make our lives easier in the future. It's not unusual for systems to have a separate parameters struct (and hpp/cpp file) of the parameters in a system and are gotten there. In this case, then we could move all the dynamic parameter callbacks to this file and out of the main algorithm file. Values from the configuration are then "gotten" from the struct param_struct.my_param that is stored as a class member instance. We could have a get() function to get the values or something. Internal to this structure, we could have the locking mechanisms for the parameter changes so that its not exposed to the larger object (e.g. plan() isn't blocked by a parameter callabck, but getting value my_param is blocked within plan() while my_param's callback function is executing or use atomics so that we need no locking)

Do you have examples of such a structure ? A bit like Teb https://github.com/rst-tu-dortmund/teb_local_planner/blob/ros2-master/teb_local_planner/include/teb_local_planner/teb_config.h ? (which is a bit messy, off topic I have a couple maintenance actions to take)

[SteveMacenski]

dynamicParametersCallback This looks pretty much the same as before, no? Just a different API to call it (and only does for the specific node). But from the docs

Note that the callback is called when declare_parameter() and its variants are called, and so you cannot assume the parameter has been set before this callback, so when checking a new value against the existing one, you must account for the case where the parameter is not yet set.

That makes this seem like a real pain in the butt.

Does this also work when the parameters are changed on the commandline? It's not clear to me that it does allow dynamic parameter changes from outside of the node itself .

[doisyg]

`` This looks pretty much the same as before, no?

Yes but not called evey time a parameter is change anywhere. Only when a parameter is changed on the hosting node, which is the behavior I thought we had with the other method.

Note that the callback is called when declare_parameter() and its variants are called, and so you cannot assume the parameter has been set before this callback, so when checking a new value against the existing one, you must account for the case where the parameter is not yet set.

That makes this seem like a real pain in the butt.

I am not sure I understand this. When will this be a problem ? If the callback is registered before calls to declare_parameter() ? (which I don't see a use case for)

Does this also work when the parameters are changed on the commandline? It's not clear to me that it does allow dynamic parameter changes from outside of the node itself .

Yes, tested with cli and also from other nodes in cpp

[SteveMacenski]

Back from vacation.

If the callback is registered before calls to declare_parameter()

I suppose, yes. I don't see a use for it, but I see something we need to defend against to make sure we're not doing things out of order, since it would trigger if it did happen.

👍 on commandline. I think this works in general, just need to take the appropriate precautions. If you submitted a trial PR to one of the nodes for us to look over together, I think we could make these changes to all of them .

[doisyg]

Welcome back, here it is: https://github.com/ros-planning/navigation2/pull/2576/files

[doisyg]

And now a PR making the same changes everywhere: #2585

[doisyg]

Costmap2DROS dyn params discussion here : #2566

[SteveMacenski]

Only AMCL and the obstacle layer remaining...

[doisyg]

If nobody takes care of it before Humble, I ll try to do it

[SteveMacenski]

I think @allenh1 mentioned he might be interested if he can find some spare time for AMCL.

[allenh1]

Yep -- I can take a look at this soon.

[SteveMacenski]

@doisyg, I have a contributor working on the obstacle layer, its just AMCL left if you wanted to jump in with that. The Humble release date is May 23rd, so about a month from now

Unless @allenh1 made some progress?

[allenh1]

@SteveMacenski I have not made any progress here, feel free to pass it on to someone who can get it done sooner.

[doisyg]

No promise but I am waiting for Humble branch from Rolling (next Monday) to setup a Humble test environment (with a real robot). From there and if everything goes smooth, I ll have a look.

[SteveMacenski]

Just AMCL left!

[padhupradheep]

I can take AMCL..

[SteveMacenski]

@doisyg I did a review, there's still work to be done on that PR #2932 but I think its ready enough for a second set of eyes to make sure I didn't miss anything. There's alot going on here so a second opinion would be valuable.

[SteveMacenski]

Finally complete! Thanks everyone for their time and help in getting dynamic parameter support in Nav2. It was a real team effort over a couple of years and happy to have it in now!

[dylan-gonzalez]

Is there any plan to have observation_sources in the obstacle layer a dynamic parameter?
#2592 (comment)