Reject dynamic kinematic parameter updates when speed limit is active

[Arnav-panjla]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	Closes #5790
Primary OS tested on	Ubuntu
Robotic platform tested on	N/A
Does this PR contain AI generated software?	No
Was this PR description generated by AI software?	No

---

Description of contribution in a few bullet points

• Added guard condition to prevent dynamic parameter updates from unintentionally resetting an active speed limit in MPPI controller.
• When a speed limit is active (constraints != base_constraints), dynamic updates to kinematic parameters are rejected.
• Added isSpeedLimitActive() helper method to detect when speed limit has modified constraints

Description of documentation updates required from your changes

• No change

Description of how this change was tested

• Built and verified controller behavior locally.
• Confirmed speed limit state is preserved when dynamic parameter updates occur.

---

Future work that may be required in bullet points

• A more flexible policy can be applied.

For Maintainers:

• Check that any new parameters added are updated in docs.nav2.org
• Check that any significant change is added to the migration guide
• Check that any new features OR changes to existing behaviors are reflected in the tuning guide
• Check that any new functions have Doxygen added
• Check that any new features have test coverage
• Check that any new plugins is added to the plugins page
• If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists
• Should this be backported to current distributions? If so, tag with backport-*.

[mini-1235]

Alternatively, we could use the addPreCallback function in ParameterHandler. When dynamic parameters are set, we check whether the user is trying to modify kinematic parameters. If s.constraints != s.base_constraints, we reject the update; otherwise, we allow it to proceed and handle it in the post-callback as usual

[mini-1235]

This would reject dynamic reconfiguration of vx_max and other settings even when s.constraints == s.base_constraints, which is probably not what we want

[Arnav-panjla]

agreed !!
I’ll adjust the logic to ensure we only block updates in the active speed-limit case.

[codecov]

Codecov Report

❌ Patch coverage is 93.75000% with 1 line in your changes missing coverage. Please review.

Files with missing lines	Patch %	Lines
nav2_mppi_controller/src/optimizer.cpp	93.75%	1 Missing ⚠️

Files with missing lines	Coverage Δ
...troller/include/nav2_mppi_controller/optimizer.hpp	100.00% <ø> (ø)
nav2_mppi_controller/src/optimizer.cpp	96.82% <93.75%> (-0.17%)	⬇️

... and 23 files with indirect coverage changes

🚀 New features to boost your workflow:

• ❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

[mini-1235]

@Arnav-panjla, is this ready for another round of review? Please also add a unit test to make sure it's working as expected :)

[mini-1235]

I might be wrong, but I don't think it works. Did you check the parameter value after it was rejected?

[Arnav-panjla]

I checked ParameterHandler
These callbacks run inside dynamicParamsCallback (the ROS2 on-set handler),
so returning successful = false does prevent the parameter from being stored.

To make this unambiguous, I’ll explicitly initialize result.successful = true and only flip it to false when rejecting.

Do let me know if am wrong

[mini-1235]

Let's say we are entering a speed filter, so at that point we should reject any changes to the kinematic parameters.

If you set them the first time, it is probably rejected. But if you try to set them again, will it be rejected this time?

Also note that the log appears twice, once as a warning and once as an error, which is probably not ideal.

[component_container_isolated-9] [WARN 1767807521.540716534] [controller_server]: Rejected dynamic parameter update to 'vx_max': a speed limit is currently active. Kinematic parameters cannot be changed while speed limit modifies constraints. (operator()() at /root/nav2_ws/src/navigation2/nav2_mppi_controller/src/optimizer.cpp:124)
[component_container_isolated-9] [INFO 1767807521.541442244] [controller_server]: Optimizer reset (reset() at /root/nav2_ws/src/navigation2/nav2_mppi_controller/src/optimizer.cpp:223)
[component_container_isolated-9] [ERROR 1767807521.541469324] [controller_server]: Rejected dynamic update to 'vx_max': speed limit is active (constraints != base_constraints). Clear the speed limit first. (dynamicParamsCallback() at /root/nav2_ws/src/navigation2/nav2_mppi_controller/src/parameters_handler.cpp:93)

Earlier, I suggested adding this to the pre-callback because we already have a parameter callback there by default. This would also log the updated parameter values in verbose mode, which I think we would want to keep.

navigation2/nav2_mppi_controller/include/nav2_mppi_controller/tools/parameters_handler.hpp

Lines 239 to 271 in e461d7f

	void ParametersHandler::setParamCallback(
	T & setting, const std::string & name, ParameterType param_type)
	{
	if (get_param_callbacks_.find(name) != get_param_callbacks_.end()) {
	return;
	}
	auto dynamic_callback =
	[this, &setting, name](
	const rclcpp::Parameter & param, rcl_interfaces::msg::SetParametersResult & /*result*/) {
	setting = as<T>(param);
	if (verbose_) {
	RCLCPP_INFO(logger_, "Dynamic parameter changed: %s", std::to_string(param).c_str());
	}
	};
	auto static_callback =
	[this, &setting, name](
	const rclcpp::Parameter & param, rcl_interfaces::msg::SetParametersResult & result) {
	std::string reason = "Rejected change to static parameter: " + std::to_string(param);
	result.successful = false;
	if (!result.reason.empty()) {
	result.reason += "\n";
	}
	result.reason += reason;
	};
	if (param_type == ParameterType::Dynamic) {
	addParamCallback(name, dynamic_callback);
	} else {
	addParamCallback(name, static_callback);
	}
	}

[Pana1v]

a new if (result.successful) was added by @Arnav-panjla . This should mitigate repeated attempted changes.

[mini-1235]

@Arnav-panjla, could you please fix the unit test? It is currently failing in our CI

/opt/overlay_ws/src/navigation2/nav2_mppi_controller/test/optimizer_unit_tests.cpp: In member function ‘virtual void OptimizerTests_SpeedLimitDynamicParameterGuard_Test::TestBody()’:
/opt/overlay_ws/src/navigation2/nav2_mppi_controller/test/optimizer_unit_tests.cpp:812:20: error: ‘void rclcpp::spin_some(node_interfaces::NodeBaseInterface::SharedPtr)’ is deprecated: use SingleThreadedExecutor::spin_some instead [-Werror=deprecated-declarations]
  812 |   rclcpp::spin_some(node->get_node_base_interface());
      |   ~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
In file included from /opt/ros/rolling/include/rclcpp/rclcpp/rclcpp.hpp:170,
                 from /opt/overlay_ws/src/navigation2/nav2_mppi_controller/test/optimizer_unit_tests.cpp:19:
/opt/ros/rolling/include/rclcpp/rclcpp/executors.hpp:90:1: note: declared here
   90 | spin_some(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr);
      | ^~~~~~~~~
/opt/overlay_ws/src/navigation2/nav2_mppi_controller/test/optimizer_unit_tests.cpp:829:20: error: ‘void rclcpp::spin_some(node_interfaces::NodeBaseInterface::SharedPtr)’ is deprecated: use SingleThreadedExecutor::spin_some instead [-Werror=deprecated-declarations]
  829 |   rclcpp::spin_some(node->get_node_base_interface());
      |   ~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

I will take a deeper look again on this PR tomorrow

[mini-1235]

@Arnav-panjla, just in case you missed the notifications, I have actually commented here: #5832 (comment) 😄

[Arnav-panjla]

Ah, my bad! 😄
I’ll take a look at the issue and get back to you shortly.

[mini-1235]

@Arnav-panjla any updates? :)

[mini-1235]

@Arnav-panjla any updates?

[Pana1v]

@mini-1235 If time is a constraint, I'd like to work on the CI tests and any other concerns left to fix

[Arnav-panjla]

Hi @Pana1v I could use some help,
especially around the CI failures and any remaining concerns.
I’ll push my latest commits, and we can coordinate from there if you’re open to it.

[mini-1235]

Hi, feel free to work on this / collaborate, please ping me again when this is ready for review :)

[Pana1v]

Hi, feel free to work on this / collaborate, please ping me again when this is ready for review :)

Hi, I cannot push yet, can you add me as a collaborator to your fork. Pushing through the indentation and I guess your last commit address the previous concerns, will look out for any more.

[SteveMacenski]

@Arnav-panjla you could also fork off of their fork (and/or cherry pick the commits to your main nav2 fork) and work from there and open another PR. Just tell us in the PR body it supersedes this one and we can close this out to continue :-)

[mini-1235]

I am currently working on closing #4907, as part of this effort, we are refactoring the dynamic parameter pattern in each package to align with the new parameter api:

The first is known as a “pre set parameter” callback, and can be set by calling add_pre_set_parameters_callback from the node API. This callback is passed a list of the Parameter objects that are being changed, and returns nothing. When it is called, it can modify the Parameter list to change, add, or remove entries. As an example, if parameter2 should change anytime that parameter1 changes, that can be implemented with this callback.

The second is known as a “set parameter” callback, and can be set by calling add_on_set_parameters_callback from the node API. The callback is passed a list of immutable Parameter objects, and returns an rcl_interfaces/msg/SetParametersResult. The main purpose of this callback is to give the user the ability to inspect the upcoming change to the parameter and explicitly reject the change.

The third type of callback is known as an “post set parameter” callback, and can be set by calling add_post_set_parameters_callback from the node API. The callback is passed a list of immutable Parameter objects, and returns nothing. The main purpose of this callback is to give the user the ability to react to changes from parameters that have successfully been accepted.

full context in https://docs.ros.org/en/rolling/Concepts/Basic/About-Parameters.html

At this point, I think it makes more sense to directly follow the new parameter api pattern rather than continuing with the previous approach I suggested, see more in #5964. This would also simplify this PR and make it easier to implement, I think.

Also please pull in / rebase main, the build error is likely due to the underlay repositories not being up to date, we have updated in a few weeks ago :)

[mergify]

This pull request is in conflict. Could you fix it @Arnav-panjla?

[mini-1235]

With #5964 now merged, I think this PR can be simplified. Basically, we just need to add pre-callbacks for the relevant parameters, without modifying the implementation inside parameter_handler. See the example here

navigation2/nav2_mppi_controller/include/nav2_mppi_controller/tools/parameters_handler.hpp

Line 282 in 9132053

addPreCallback(name, static_callback);

@Arnav-panjla @Pana1v would either of you be interested in picking this up so we can move this issue/PR toward closure?

[Pana1v]

sure, will work on it over this weekend

[mergify]

This pull request is in conflict. Could you fix it @Arnav-panjla?

[Pana1v]

PTAL @mini-1235

[mini-1235]

I left a few comments, but overall LGTM!

For the DCO, I think you need to add @Arnav-panjla as coauthor in your commits?

[mini-1235]

If you take a look at the diff, you probably did something wrong when rebasing

[Pana1v]

yeah, working on it

[mini-1235]

I will approve once DCO passes :)

If you run into any trouble fixing it, it might be easier to open a new PR instead

[mergify]

This pull request is in conflict. Could you fix it @Arnav-panjla?

[mini-1235]

Thanks! I will pass this along to @SteveMacenski for a final review to see if he has any additional suggestions

[Pana1v]

Thanks @mini-1235!

[SteveMacenski]

Otherwise very clever and well done