Re-enable runtime configuration of key parameters

crdelsey commented 5 years ago

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 commented 5 years ago

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

bpwilcox commented 5 years ago

https://github.com/ros2/rclcpp/pull/829 submitted to add additional support for parameter event subscriptions for dynamic parameter callbacks

SteveMacenski commented 3 years ago

Major TODO list (tunable behaviors / algorithms)

[x] DWB
[x] costmap layers (voxel, inflation, obstacle, range, static)
[x] costmap_2d https://github.com/ros-planning/navigation2/pull/2592#pullrequestreview-779145070
[x] navfn https://github.com/ros-planning/navigation2/pull/2181
[x] Theta* planner https://github.com/ros-planning/navigation2/pull/2665
[x] smac planner 2D https://github.com/ros-planning/navigation2/pull/2475
[x] smac planner Hybrid #2540
[x] smac planner Lattice https://github.com/ros-planning/navigation2/pull/2582
[ ] amcl
[x] regulated pure pursuit https://github.com/ros-planning/navigation2/pull/2607
[x] Goal checkers
[x] Progress checkers

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

[x] waypoint follower https://github.com/ros-planning/navigation2/pull/2704
[x] ~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
[x] ~lifecycle manager~ safety shouldn't allow this, and all params are integral on initialization
[x] planner server https://github.com/ros-planning/navigation2/pull/2704
[x] controller server https://github.com/ros-planning/navigation2/pull/2704
[x] ~map server~ All parameters are overridable in the actual service requests to load / save so no need to overload defaults in dynamic parameters

doisyg commented 3 years ago

Moving the discussion from https://github.com/ros-planning/navigation2/issues/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 commented 3 years ago

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 commented 3 years ago

`` 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 commented 3 years ago

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.

:+1: 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 commented 3 years ago

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

doisyg commented 3 years ago

And now a PR making the same changes everywhere: https://github.com/ros-planning/navigation2/pull/2585

doisyg commented 3 years ago

Costmap2DROS dyn params discussion here : https://github.com/ros-planning/navigation2/issues/2566

SteveMacenski commented 2 years ago

Only AMCL and the obstacle layer remaining...

doisyg commented 2 years ago

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

SteveMacenski commented 2 years ago

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

allenh1 commented 2 years ago

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

SteveMacenski commented 2 years ago

@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 commented 2 years ago

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

doisyg commented 2 years ago

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 commented 2 years ago

Just AMCL left!

padhupradheep commented 2 years ago

I can take AMCL..

SteveMacenski commented 2 years ago

@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 commented 2 years ago

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 commented 2 months ago

Is there any plan to have observation_sources in the obstacle layer a dynamic parameter? https://github.com/ros-navigation/navigation2/pull/2592#issuecomment-944080039

ros-navigation / navigation2

Re-enable runtime configuration of key parameters #956

Feature request

Feature description