diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp index 8d737137a10..44acff89af8 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp @@ -131,6 +131,12 @@ class Optimizer */ void reset(bool reset_dynamic_speed_limits = true); + /** + * @brief Check if a dynamic speed limit is currently active + * @return True if constraints differ from base_constraints (speed limit active) + */ + bool isSpeedLimitActive() const; + /** * @brief Get the motion model time step * @return Time step of the model diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp index 6f568170d1f..c16c953d90d 100644 --- a/nav2_mppi_controller/src/optimizer.cpp +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -80,6 +80,27 @@ void Optimizer::getParams() auto & s = settings_; auto getParam = parameters_handler_->getParamGetter(name_); auto getParentParam = parameters_handler_->getParamGetter(""); + + // Reject dynamic updates to kinematic params when speed limit is active + auto kinematic_guard = [this]( + const rclcpp::Parameter & param, + rcl_interfaces::msg::SetParametersResult & result) { + if (isSpeedLimitActive()) { + result.successful = false; + if (!result.reason.empty()) { + result.reason += "\n"; + } + result.reason += "Rejected dynamic update to '" + param.get_name() + + "': speed limit is active. Clear the speed limit first."; + } + }; + + const std::vector kinematic_params = { + "vx_max", "vx_min", "vy_max", "wz_max"}; + for (const auto & p : kinematic_params) { + parameters_handler_->addPreCallback(name_ + "." + p, kinematic_guard); + } + getParam(s.model_dt, "model_dt", 0.05f); getParam(s.time_steps, "time_steps", 56); getParam(s.batch_size, "batch_size", 1000); @@ -184,6 +205,18 @@ bool Optimizer::isHolonomic() const return motion_model_->isHolonomic(); } +bool Optimizer::isSpeedLimitActive() const +{ + // Speed limit is active when current constraints differ from base constraints. + // This occurs when setSpeedLimit() has modified the velocity/acceleration limits. + const auto & base = settings_.base_constraints; + const auto & curr = settings_.constraints; + return base.vx_max != curr.vx_max || + base.vx_min != curr.vx_min || + base.vy != curr.vy || + base.wz != curr.wz; +} + std::tuple Optimizer::evalControl( const geometry_msgs::msg::PoseStamped & robot_pose, const geometry_msgs::msg::Twist & robot_speed, diff --git a/nav2_mppi_controller/test/optimizer_unit_tests.cpp b/nav2_mppi_controller/test/optimizer_unit_tests.cpp index a1056531d91..c95b7bf54ff 100644 --- a/nav2_mppi_controller/test/optimizer_unit_tests.cpp +++ b/nav2_mppi_controller/test/optimizer_unit_tests.cpp @@ -18,6 +18,7 @@ #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" #include "nav2_mppi_controller/optimizer.hpp" +#include "nav2_costmap_2d/costmap_filters/filter_values.hpp" #include "tf2_ros/buffer.hpp" // Tests main optimizer functions @@ -148,6 +149,16 @@ class OptimizerTester : public Optimizer return settings_.constraints; } + models::ControlConstraints & getBaseConstraints() + { + return settings_.base_constraints; + } + + bool isSpeedLimitActiveWrapper() const + { + return isSpeedLimitActive(); + } + void applyControlSequenceConstraintsWrapper() { return applyControlSequenceConstraints(); @@ -763,6 +774,68 @@ TEST(OptimizerTests, Omni_openLoopMppiTest) optimizer_tester.getSettings().model_dt * optimizer_tester.getControlConstraints().ay_max); } +TEST(OptimizerTests, SpeedLimitDynamicParameterGuard) +{ + // This test verifies that kinematic parameters (vx_max, etc.) are rejected + // when a speed limit is active, but allowed when no speed limit is active. + auto node = std::make_shared("my_node"); + OptimizerTester optimizer_tester; + node->declare_parameter("controller_frequency", rclcpp::ParameterValue(30.0)); + node->declare_parameter("mppic.batch_size", rclcpp::ParameterValue(1000)); + node->declare_parameter("mppic.time_steps", rclcpp::ParameterValue(50)); + node->declare_parameter("mppic.vx_max", rclcpp::ParameterValue(0.5)); + node->declare_parameter("mppic.vx_min", rclcpp::ParameterValue(-0.35)); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", true); + // Use "mppic" to match the optimizer's name_ so ParametersHandler correctly routes callbacks + std::string name = "mppic"; + ParametersHandler param_handler(node, name); + rclcpp_lifecycle::State lstate; + costmap_ros->on_configure(lstate); + auto tf_buffer = std::make_shared(node->get_clock()); + optimizer_tester.initialize(node, "mppic", costmap_ros, tf_buffer, ¶m_handler); + + // Start the parameter handler to enable dynamic parameter callbacks + param_handler.start(); + + // Verify initial state: no speed limit active + EXPECT_FALSE(optimizer_tester.isSpeedLimitActiveWrapper()); + + // Store original base_constraints value + float original_vx_max = optimizer_tester.getBaseConstraints().vx_max; + EXPECT_EQ(original_vx_max, 0.5f); + + // Test 1: Dynamic parameter update SHOULD SUCCEED when no speed limit is active + auto result1 = node->set_parameter(rclcpp::Parameter("mppic.vx_max", 0.8)); + EXPECT_TRUE(result1.successful); + // Verify the parameter was updated + EXPECT_EQ(optimizer_tester.getBaseConstraints().vx_max, 0.8f); + + // Apply a speed limit (50% reduction) + optimizer_tester.setSpeedLimit(50.0, true); + EXPECT_TRUE(optimizer_tester.isSpeedLimitActiveWrapper()); + + // Verify constraints differ from base_constraints now + EXPECT_NE( + optimizer_tester.getControlConstraints().vx_max, + optimizer_tester.getBaseConstraints().vx_max); + + // Test 2: Dynamic parameter update SHOULD BE REJECTED when speed limit is active + auto result2 = node->set_parameter(rclcpp::Parameter("mppic.vx_max", 1.0)); + EXPECT_FALSE(result2.successful); + // Verify the base_constraints value was NOT updated + EXPECT_EQ(optimizer_tester.getBaseConstraints().vx_max, 0.8f); + + // Clear the speed limit using NO_SPEED_LIMIT constant + optimizer_tester.setSpeedLimit(nav2_costmap_2d::NO_SPEED_LIMIT, false); + EXPECT_FALSE(optimizer_tester.isSpeedLimitActiveWrapper()); + + // Test 3: Dynamic parameter update SHOULD SUCCEED again after clearing speed limit + auto result3 = node->set_parameter(rclcpp::Parameter("mppic.vx_max", 1.0)); + EXPECT_TRUE(result3.successful); + EXPECT_EQ(optimizer_tester.getBaseConstraints().vx_max, 1.0f); +} + int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv);