Reject dynamic kinematic parameter updates when speed limit is active

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

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<std::string> 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<geometry_msgs::msg::TwistStamped, Eigen::ArrayXXf> Optimizer::evalControl(
   const geometry_msgs::msg::PoseStamped & robot_pose,
   const geometry_msgs::msg::Twist & robot_speed,

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<nav2::LifecycleNode>("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<nav2_costmap_2d::Costmap2DROS>(
+    "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<tf2_ros::Buffer>(node->get_clock());
+  optimizer_tester.initialize(node, "mppic", costmap_ros, tf_buffer, &param_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);