MPPI Check Namespace.

Signed-off-by: Nils-Christian Iseke <nilsmailiseke@gmail.com>

Author: Nils-ChristianIseke

nav2_mppi_controller/benchmark/optimizer_benchmark.cpp

@@ -79,7 +79,8 @@ void prepareAndRunBenchmark(
 
   printInfo(optimizer_settings, path_settings, critics);
   auto node = getDummyNode(optimizer_settings, critics);
-  auto parameters_handler = std::make_unique<mppi::ParametersHandler>(node);
+  std::string name = "test";
+  auto parameters_handler = std::make_unique<mppi::ParametersHandler>(node, name);
   auto optimizer = getDummyOptimizer(node, costmap_ros, parameters_handler.get());
 
   // evalControl args

nav2_mppi_controller/include/nav2_mppi_controller/tools/parameters_handler.hpp

@@ -45,7 +45,7 @@ class ParametersHandler
     rcl_interfaces::msg::SetParametersResult & result);
   using post_callback_t = void ();
   using pre_callback_t = void ();
-
+  std::string name_;
   /**
     * @brief Constructor for mppi::ParametersHandler
     */
@@ -56,7 +56,7 @@ class ParametersHandler
     * @param parent Weak ptr to node
     */
   explicit ParametersHandler(
-    const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent);
+    const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, std::string & name);
 
   /**
     * @brief Destructor for mppi::ParametersHandler

nav2_mppi_controller/src/controller.cpp

@@ -31,7 +31,7 @@ void MPPIController::configure(
   costmap_ros_ = costmap_ros;
   tf_buffer_ = tf;
   name_ = name;
-  parameters_handler_ = std::make_unique<ParametersHandler>(parent);
+  parameters_handler_ = std::make_unique<ParametersHandler>(parent, name_);
 
   auto node = parent_.lock();
   // Get high-level controller parameters

nav2_mppi_controller/src/optimizer.cpp

@@ -62,7 +62,7 @@ void Optimizer::getParams()
 
   auto & s = settings_;
   auto getParam = parameters_handler_->getParamGetter(name_);
-  auto getParentParam = parameters_handler_->getParamGetter("");
+  auto getParentParam = parameters_handler_->getParamGetter("test");
   getParam(s.model_dt, "model_dt", 0.05f);
   getParam(s.time_steps, "time_steps", 56);
   getParam(s.batch_size, "batch_size", 1000);

nav2_mppi_controller/src/parameters_handler.cpp

@@ -18,12 +18,13 @@ namespace mppi
 {
 
 ParametersHandler::ParametersHandler(
-  const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent)
+  const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, std::string & name)
 {
   node_ = parent;
   auto node = node_.lock();
   node_name_ = node->get_name();
   logger_ = node->get_logger();
+  name_ = name;
 }
 
 ParametersHandler::~ParametersHandler()
@@ -64,7 +65,9 @@ ParametersHandler::dynamicParamsCallback(
 
   for (auto & param : parameters) {
     const std::string & param_name = param.get_name();
-
+    if (param_name.find(name_ + ".") != 0) {
+      continue;
+    }
     if (auto callback = get_param_callbacks_.find(param_name);
       callback != get_param_callbacks_.end())
     {

nav2_mppi_controller/test/critic_manager_test.cpp

@@ -95,7 +95,8 @@ TEST(CriticManagerTests, BasicCriticOperations)
   auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
   auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
     "dummy_costmap", "", true);
-  ParametersHandler param_handler(node);
+  std::string name = "test";
+ParametersHandler param_handler(node, name);
   rclcpp_lifecycle::State lstate;
   costmap_ros->on_configure(lstate);
 
@@ -134,7 +135,8 @@ TEST(CriticManagerTests, CriticLoadingTest)
     rclcpp::ParameterValue(std::vector<std::string>{"ConstraintCritic", "PreferForwardCritic"}));
   auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
     "dummy_costmap", "", true);
-  ParametersHandler param_handler(node);
+  std::string name = "test";
+ParametersHandler param_handler(node, name);
   rclcpp_lifecycle::State state;
   costmap_ros->on_configure(state);
 

nav2_mppi_controller/test/critics_tests.cpp

@@ -62,7 +62,8 @@ TEST(CriticTests, ConstraintsCritic)
   auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
   auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
     "dummy_costmap", "", true);
-  ParametersHandler param_handler(node);
+  std::string name = "test";
+ParametersHandler param_handler(node, name);
   rclcpp_lifecycle::State lstate;
   costmap_ros->on_configure(lstate);
 
@@ -130,7 +131,8 @@ TEST(CriticTests, ObstacleCriticMisalignedParams) {
   auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
   auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
     "dummy_costmap", "", true);
-  ParametersHandler param_handler(node);
+  std::string name = "test";
+ParametersHandler param_handler(node, name);
   auto getParam = param_handler.getParamGetter("critic");
   bool consider_footprint;
   getParam(consider_footprint, "consider_footprint", true);
@@ -150,7 +152,8 @@ TEST(CriticTests, ObstacleCriticAlignedParams) {
   auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
   auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
     "dummy_costmap", "", true);
-  ParametersHandler param_handler(node);
+  std::string name = "test";
+ParametersHandler param_handler(node, name);
   auto getParam = param_handler.getParamGetter("critic");
   bool consider_footprint;
   getParam(consider_footprint, "consider_footprint", false);
@@ -169,7 +172,8 @@ TEST(CriticTests, CostCriticMisAlignedParams) {
   auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
   auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
     "dummy_costmap", "", true);
-  ParametersHandler param_handler(node);
+  std::string name = "test";
+ParametersHandler param_handler(node, name);
   rclcpp_lifecycle::State lstate;
    auto getParam = param_handler.getParamGetter("critic");
   bool consider_footprint;
@@ -189,7 +193,8 @@ TEST(CriticTests, CostCriticAlignedParams) {
   auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
   auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
     "dummy_costmap", "", true);
-  ParametersHandler param_handler(node);
+  std::string name = "test";
+ParametersHandler param_handler(node, name);
   rclcpp_lifecycle::State lstate;
    auto getParam = param_handler.getParamGetter("critic");
   bool consider_footprint;
@@ -207,7 +212,8 @@ TEST(CriticTests, GoalAngleCritic)
   auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
   auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
     "dummy_costmap", "", true);
-  ParametersHandler param_handler(node);
+  std::string name = "test";
+ParametersHandler param_handler(node, name);
   rclcpp_lifecycle::State lstate;
   costmap_ros->on_configure(lstate);
 
@@ -266,7 +272,8 @@ TEST(CriticTests, GoalCritic)
   auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
   auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
     "dummy_costmap", "", true);
-  ParametersHandler param_handler(node);
+  std::string name = "test";
+ParametersHandler param_handler(node, name);
   rclcpp_lifecycle::State lstate;
   costmap_ros->on_configure(lstate);
 
@@ -318,7 +325,8 @@ TEST(CriticTests, PathAngleCritic)
   auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
   auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
     "dummy_costmap", "", true);
-  ParametersHandler param_handler(node);
+  std::string name = "test";
+ParametersHandler param_handler(node, name);
   rclcpp_lifecycle::State lstate;
   costmap_ros->on_configure(lstate);
 
@@ -436,7 +444,8 @@ TEST(CriticTests, PreferForwardCritic)
   auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
   auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
     "dummy_costmap", "", true);
-  ParametersHandler param_handler(node);
+  std::string name = "test";
+ParametersHandler param_handler(node, name);
   rclcpp_lifecycle::State lstate;
   costmap_ros->on_configure(lstate);
 
@@ -492,7 +501,8 @@ TEST(CriticTests, TwirlingCritic)
   auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
   auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
     "dummy_costmap", "", true);
-  ParametersHandler param_handler(node);
+  std::string name = "test";
+ParametersHandler param_handler(node, name);
   rclcpp_lifecycle::State lstate;
   costmap_ros->on_configure(lstate);
 
@@ -556,7 +566,8 @@ TEST(CriticTests, PathFollowCritic)
   auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
   auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
     "dummy_costmap", "", true);
-  ParametersHandler param_handler(node);
+  std::string name = "test";
+ParametersHandler param_handler(node, name);
   rclcpp_lifecycle::State lstate;
   costmap_ros->on_configure(lstate);
 
@@ -607,7 +618,8 @@ TEST(CriticTests, PathAlignCritic)
   auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
   auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
     "dummy_costmap", "", true);
-  ParametersHandler param_handler(node);
+  std::string name = "test";
+ParametersHandler param_handler(node, name);
   rclcpp_lifecycle::State lstate;
   costmap_ros->on_configure(lstate);
 
@@ -717,7 +729,8 @@ TEST(CriticTests, VelocityDeadbandCritic)
   auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
   auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
     "dummy_costmap", "", true);
-  ParametersHandler param_handler(node);
+  std::string name = "test";
+ParametersHandler param_handler(node, name);
   auto getParam = param_handler.getParamGetter("critic");
   std::vector<double> deadband_velocities_;
   getParam(deadband_velocities_, "deadband_velocities", std::vector<double>{0.08, 0.08, 0.08});

nav2_mppi_controller/test/motion_model_tests.cpp

@@ -127,7 +127,8 @@ TEST(MotionModelTests, AckermannTest)
   control_sequence.reset(timesteps);  // populates with zeros
   state.reset(batches, timesteps);  // populates with zeros
   auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
-  ParametersHandler param_handler(node);
+  std::string name = "test";
+  ParametersHandler param_handler(node, name);
   std::unique_ptr<AckermannMotionModel> model =
     std::make_unique<AckermannMotionModel>(&param_handler, node->get_name());
 
@@ -185,7 +186,8 @@ TEST(MotionModelTests, AckermannReversingTest)
   control_sequence2.reset(timesteps);  // populates with zeros
   state.reset(batches, timesteps);  // populates with zeros
   auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
-  ParametersHandler param_handler(node);
+  std::string name = "test";
+  ParametersHandler param_handler(node, name);
   std::unique_ptr<AckermannMotionModel> model =
     std::make_unique<AckermannMotionModel>(&param_handler, node->get_name());
 

nav2_mppi_controller/test/noise_generator_test.cpp

@@ -38,7 +38,8 @@ TEST(NoiseGeneratorTest, NoiseGeneratorLifecycle)
 
   auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("node");
   node->declare_parameter("test_name.regenerate_noises", rclcpp::ParameterValue(false));
-  ParametersHandler handler(node);
+  std::string name = "test";
+ParametersHandler handler(node, name);
 
   generator.initialize(settings, false, "test_name", &handler);
   generator.reset(settings, false);
@@ -50,7 +51,8 @@ TEST(NoiseGeneratorTest, NoiseGeneratorMain)
   // Tests shuts down internal thread cleanly
   auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("node");
   node->declare_parameter("test_name.regenerate_noises", rclcpp::ParameterValue(true));
-  ParametersHandler handler(node);
+  std::string name = "test";
+ParametersHandler handler(node, name);
   NoiseGenerator generator;
   mppi::models::OptimizerSettings settings;
   settings.batch_size = 100;
@@ -138,7 +140,8 @@ TEST(NoiseGeneratorTest, NoiseGeneratorMainNoRegenerate)
   // This time with no regeneration of noises
   auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("node");
   node->declare_parameter("test_name.regenerate_noises", rclcpp::ParameterValue(false));
-  ParametersHandler handler(node);
+  std::string name = "test";
+ParametersHandler handler(node, name);
   NoiseGenerator generator;
   mppi::models::OptimizerSettings settings;
   settings.batch_size = 100;

nav2_mppi_controller/test/optimizer_smoke_test.cpp

@@ -66,7 +66,8 @@ TEST_P(OptimizerSuite, OptimizerTest) {
 
   printInfo(optimizer_settings, path_settings, critics);
   auto node = getDummyNode(optimizer_settings, critics);
-  auto parameters_handler = std::make_unique<mppi::ParametersHandler>(node);
+  std::string name = "test";
+  auto parameters_handler = std::make_unique<mppi::ParametersHandler>(node, name);
   auto optimizer = getDummyOptimizer(node, costmap_ros, parameters_handler.get());
 
   // evalControl args