Prepare for State transitions

controllers/easynav_mpc_controller/CMakeLists.txt

@@ -89,7 +89,7 @@ if(BUILD_TESTING)
   ament_lint_auto_find_test_dependencies()
 
   find_package(ament_cmake_gtest REQUIRED)
-  # add_subdirectory(tests)
+  add_subdirectory(tests)
 endif()
 
 ament_export_include_directories("include/${PROJECT_NAME}")

controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp

@@ -34,15 +34,17 @@ MPCController::on_initialize()
   auto node = get_node();
   const auto & plugin_name = get_plugin_name();
 
-  node->declare_parameter<int>(plugin_name + ".horizon_steps", horizon_steps_);
-  node->declare_parameter<double>(plugin_name + ".dt", dt_);
-  node->declare_parameter<double>(plugin_name + ".safety_radius", safety_radius_);
-  node->declare_parameter<double>(plugin_name + ".max_linear_velocity", max_lin_vel_);
-  node->declare_parameter<double>(plugin_name + ".max_angular_velocity", max_ang_vel_);
-  node->declare_parameter<bool>(plugin_name + ".verbose", verbose_);
-
-  node->declare_parameter<double>(plugin_name + ".fallback_goal_pos_tol", fallback_goal_pos_tol_);
-  node->declare_parameter<double>(plugin_name + ".fallback_goal_yaw_tol", fallback_goal_yaw_tol_);
+  if (!node->has_parameter(plugin_name + ".horizon_steps")) {
+    node->declare_parameter<int>(plugin_name + ".horizon_steps", horizon_steps_);
+    node->declare_parameter<double>(plugin_name + ".dt", dt_);
+    node->declare_parameter<double>(plugin_name + ".safety_radius", safety_radius_);
+    node->declare_parameter<double>(plugin_name + ".max_linear_velocity", max_lin_vel_);
+    node->declare_parameter<double>(plugin_name + ".max_angular_velocity", max_ang_vel_);
+    node->declare_parameter<bool>(plugin_name + ".verbose", verbose_);
+
+    node->declare_parameter<double>(plugin_name + ".fallback_goal_pos_tol", fallback_goal_pos_tol_);
+    node->declare_parameter<double>(plugin_name + ".fallback_goal_yaw_tol", fallback_goal_yaw_tol_);
+  }
 
   node->get_parameter<int>(plugin_name + ".horizon_steps", horizon_steps_);
   node->get_parameter<double>(plugin_name + ".dt", dt_);

controllers/easynav_mpc_controller/tests/CMakeLists.txt

@@ -0,0 +1,11 @@
+find_package(ament_cmake_gtest REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
+
+ament_add_gtest(mpc_controller_reconfigure_tests mpc_controller_reconfigure_tests.cpp)
+
+target_link_libraries(mpc_controller_reconfigure_tests
+  ${PROJECT_NAME}
+  rclcpp::rclcpp
+  rclcpp_lifecycle::rclcpp_lifecycle
+)

controllers/easynav_mpc_controller/tests/mpc_controller_reconfigure_tests.cpp

@@ -0,0 +1,47 @@
+// Copyright 2025 Intelligent Robotics Lab
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/// \file
+/// \brief Regression test: a plugin must tolerate initialize() being
+/// called twice on the same node (as happens across a cleanup/reconfigure
+/// cycle) without throwing.
+
+#include "easynav_mpc_controller/MPCController.hpp"
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
+
+#include "gtest/gtest.h"
+
+class MPCControllerReconfigureTest : public ::testing::Test
+{
+protected:
+  void SetUp() override
+  {
+    if (!rclcpp::ok()) {
+      rclcpp::init(0, nullptr);
+    }
+  }
+};
+
+TEST_F(MPCControllerReconfigureTest, InitializeTwiceOnSameNodeDoesNotThrow)
+{
+  auto node = rclcpp_lifecycle::LifecycleNode::make_shared("mpc_controller_reconfigure_test");
+
+  auto plugin1 = std::make_shared<easynav::MPCController>();
+  ASSERT_NO_THROW(plugin1->initialize(node, "test_controller"));
+
+  auto plugin2 = std::make_shared<easynav::MPCController>();
+  ASSERT_NO_THROW(plugin2->initialize(node, "test_controller"));
+}

controllers/easynav_mppi_controller/CMakeLists.txt

@@ -66,7 +66,7 @@ if(BUILD_TESTING)
   ament_lint_auto_find_test_dependencies()
 
   find_package(ament_cmake_gtest REQUIRED)
-  # add_subdirectory(tests)
+  add_subdirectory(tests)
 endif()
 
 ament_export_include_directories("include/${PROJECT_NAME}")

controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIController.cpp

@@ -37,16 +37,18 @@ MPPIController::on_initialize()
   auto node = get_node();
   const auto & plugin_name = get_plugin_name();
 
-  node->declare_parameter<int>(plugin_name + ".num_samples", num_samples_);
-  node->declare_parameter<int>(plugin_name + ".horizon_steps", horizon_steps_);
-  node->declare_parameter<double>(plugin_name + ".dt", dt_);
-  node->declare_parameter<double>(plugin_name + ".lambda", lambda_);
-  node->declare_parameter<double>(plugin_name + ".max_linear_velocity", max_lin_vel_);
-  node->declare_parameter<double>(plugin_name + ".max_angular_velocity", max_ang_vel_);
-  node->declare_parameter<double>(plugin_name + ".max_linear_acceleration", max_lin_acc_);
-  node->declare_parameter<double>(plugin_name + ".max_angular_acceleration", max_ang_acc_);
-  node->declare_parameter<double>(plugin_name + ".fov", fov_);
-  node->declare_parameter<double>(plugin_name + ".safety_radius", safety_radius_);
+  if (!node->has_parameter(plugin_name + ".num_samples")) {
+    node->declare_parameter<int>(plugin_name + ".num_samples", num_samples_);
+    node->declare_parameter<int>(plugin_name + ".horizon_steps", horizon_steps_);
+    node->declare_parameter<double>(plugin_name + ".dt", dt_);
+    node->declare_parameter<double>(plugin_name + ".lambda", lambda_);
+    node->declare_parameter<double>(plugin_name + ".max_linear_velocity", max_lin_vel_);
+    node->declare_parameter<double>(plugin_name + ".max_angular_velocity", max_ang_vel_);
+    node->declare_parameter<double>(plugin_name + ".max_linear_acceleration", max_lin_acc_);
+    node->declare_parameter<double>(plugin_name + ".max_angular_acceleration", max_ang_acc_);
+    node->declare_parameter<double>(plugin_name + ".fov", fov_);
+    node->declare_parameter<double>(plugin_name + ".safety_radius", safety_radius_);
+  }
 
   node->get_parameter<int>(plugin_name + ".num_samples", num_samples_);
   node->get_parameter<int>(plugin_name + ".horizon_steps", horizon_steps_);

controllers/easynav_mppi_controller/tests/CMakeLists.txt

@@ -0,0 +1,11 @@
+find_package(ament_cmake_gtest REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
+
+ament_add_gtest(mppi_controller_reconfigure_tests mppi_controller_reconfigure_tests.cpp)
+
+target_link_libraries(mppi_controller_reconfigure_tests
+  ${PROJECT_NAME}
+  rclcpp::rclcpp
+  rclcpp_lifecycle::rclcpp_lifecycle
+)

controllers/easynav_mppi_controller/tests/mppi_controller_reconfigure_tests.cpp

@@ -0,0 +1,47 @@
+// Copyright 2025 Intelligent Robotics Lab
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/// \file
+/// \brief Regression test: a plugin must tolerate initialize() being
+/// called twice on the same node (as happens across a cleanup/reconfigure
+/// cycle) without throwing.
+
+#include "easynav_mppi_controller/MPPIController.hpp"
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
+
+#include "gtest/gtest.h"
+
+class MPPIControllerReconfigureTest : public ::testing::Test
+{
+protected:
+  void SetUp() override
+  {
+    if (!rclcpp::ok()) {
+      rclcpp::init(0, nullptr);
+    }
+  }
+};
+
+TEST_F(MPPIControllerReconfigureTest, InitializeTwiceOnSameNodeDoesNotThrow)
+{
+  auto node = rclcpp_lifecycle::LifecycleNode::make_shared("mppi_controller_reconfigure_test");
+
+  auto plugin1 = std::make_shared<easynav::MPPIController>();
+  ASSERT_NO_THROW(plugin1->initialize(node, "test_controller"));
+
+  auto plugin2 = std::make_shared<easynav::MPPIController>();
+  ASSERT_NO_THROW(plugin2->initialize(node, "test_controller"));
+}

controllers/easynav_regulated_pp_controller/src/easynav_regulated_pp_controller/RegulatedPurePursuitController.cpp

@@ -59,7 +59,9 @@ RegulatedPurePursuitController::on_initialize()
   const auto & plugin_name = get_plugin_name();
 
   auto declare_and_get = [&node, &plugin_name](const std::string & name, auto & value) {
-      node->declare_parameter(plugin_name + "." + name, value);
+      if (!node->has_parameter(plugin_name + "." + name)) {
+        node->declare_parameter(plugin_name + "." + name, value);
+      }
       node->get_parameter(plugin_name + "." + name, value);
     };
 

controllers/easynav_regulated_pp_controller/tests/CMakeLists.txt

@@ -2,3 +2,14 @@ ament_add_gtest(regulated_pp_controller_tests regulated_pp_controller_tests.cpp)
 target_link_libraries(regulated_pp_controller_tests
   ${PROJECT_NAME}
 )
+
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
+
+ament_add_gtest(regulated_pp_controller_reconfigure_tests
+  regulated_pp_controller_reconfigure_tests.cpp)
+target_link_libraries(regulated_pp_controller_reconfigure_tests
+  ${PROJECT_NAME}
+  rclcpp::rclcpp
+  rclcpp_lifecycle::rclcpp_lifecycle
+)

controllers/easynav_regulated_pp_controller/tests/regulated_pp_controller_reconfigure_tests.cpp

@@ -0,0 +1,49 @@
+// Copyright 2026 Intelligent Robotics Lab
+//
+// This file is part of the project Easy Navigation (EasyNav in short)
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/// \file
+/// \brief Regression test: a plugin must tolerate initialize() being
+/// called twice on the same node (as happens across a cleanup/reconfigure
+/// cycle) without throwing.
+
+#include "easynav_regulated_pp_controller/RegulatedPurePursuitController.hpp"
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
+
+#include "gtest/gtest.h"
+
+class RegulatedPurePursuitControllerReconfigureTest : public ::testing::Test
+{
+protected:
+  void SetUp() override
+  {
+    if (!rclcpp::ok()) {
+      rclcpp::init(0, nullptr);
+    }
+  }
+};
+
+TEST_F(RegulatedPurePursuitControllerReconfigureTest, InitializeTwiceOnSameNodeDoesNotThrow)
+{
+  auto node = rclcpp_lifecycle::LifecycleNode::make_shared(
+    "regulated_pp_controller_reconfigure_test");
+
+  auto plugin1 = std::make_shared<easynav::RegulatedPurePursuitController>();
+  ASSERT_NO_THROW(plugin1->initialize(node, "test_controller"));
+
+  auto plugin2 = std::make_shared<easynav::RegulatedPurePursuitController>();
+  ASSERT_NO_THROW(plugin2->initialize(node, "test_controller"));
+}

controllers/easynav_serest_controller/CMakeLists.txt

@@ -58,6 +58,7 @@ if(BUILD_TESTING)
   ament_lint_auto_find_test_dependencies()
 
   find_package(ament_cmake_gtest REQUIRED)
+  add_subdirectory(tests)
 endif()
 
 ament_export_include_directories("include/${PROJECT_NAME}")

controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp

@@ -42,57 +42,58 @@ SerestController::on_initialize()
   auto node = get_node();
   const auto & ns = get_plugin_name();
 
-  // Maximums and basic limits
-  node->declare_parameter<bool>(ns + ".allow_reverse", allow_reverse_);
-  node->declare_parameter<double>(ns + ".v_progress_min", v_progress_min_);
-  node->declare_parameter<double>(ns + ".k_s_share_max", k_s_share_max_);
-
-  node->declare_parameter<double>(ns + ".max_linear_speed", max_linear_speed_);
-  node->declare_parameter<double>(ns + ".max_angular_speed", max_angular_speed_);
-  node->declare_parameter<double>(ns + ".max_linear_acc", max_linear_acc_);
-  node->declare_parameter<double>(ns + ".max_angular_acc", max_angular_acc_);
-
-  // Tracking
-  node->declare_parameter<double>(ns + ".k_s", k_s_);
-  node->declare_parameter<double>(ns + ".k_theta", k_theta_);
-  node->declare_parameter<double>(ns + ".k_y", k_y_);
-  node->declare_parameter<double>(ns + ".ell", ell_);
-  node->declare_parameter<double>(ns + ".v_ref", v_ref_);
-  node->declare_parameter<double>(ns + ".eps", eps_);
-
-  // Safety
-  node->declare_parameter<double>(ns + ".a_acc", a_acc_);
-  node->declare_parameter<double>(ns + ".a_brake", a_brake_);
-  node->declare_parameter<double>(ns + ".a_lat_max", a_lat_max_);
-  node->declare_parameter<double>(ns + ".d0_margin", d0_margin_);
-  node->declare_parameter<double>(ns + ".tau_latency", tau_latency_);
-  node->declare_parameter<double>(ns + ".d_hard", d_hard_);
-  node->declare_parameter<double>(ns + ".t_emerg", t_emerg_);
-
-  // Blend at vertices
-  node->declare_parameter<double>(ns + ".blend_base", blend_base_);
-  node->declare_parameter<double>(ns + ".blend_k_per_v", blend_k_per_v_);
-  node->declare_parameter<double>(ns + ".kappa_max", kappa_max_);
-
-  // For obstacle detection
-  node->declare_parameter<double>(ns + ".dist_search_radius", dist_search_radius_);
-
-  node->declare_parameter<double>(ns + ".goal_pos_tol", goal_pos_tol_);
-  node->declare_parameter<double>(ns + ".goal_yaw_tol_deg", goal_yaw_tol_deg_);
-  node->declare_parameter<double>(ns + ".slow_radius", slow_radius_);
-  node->declare_parameter<double>(ns + ".slow_min_speed", slow_min_speed_);
-  node->declare_parameter<double>(ns + ".final_align_k", final_align_k_);
-  node->declare_parameter<double>(ns + ".final_align_wmax", final_align_wmax_);
-
-  node->declare_parameter<bool>(ns + ".corner_guard_enable", corner_guard_enable_);
-  node->declare_parameter<double>(ns + ".corner_gain_ey", corner_gain_ey_);
-  node->declare_parameter<double>(ns + ".corner_gain_eth", corner_gain_eth_);
-  node->declare_parameter<double>(ns + ".corner_gain_kappa", corner_gain_kappa_);
-  node->declare_parameter<double>(ns + ".corner_min_alpha", corner_min_alpha_);
-  node->declare_parameter<double>(ns + ".corner_boost_omega", corner_boost_omega_);
-  node->declare_parameter<double>(ns + ".apex_ey_des", apex_ey_des_);
-  node->declare_parameter<double>(ns + ".a_lat_soft", a_lat_soft_);
-
+  if (!node->has_parameter(ns + ".allow_reverse")) {
+    // Maximums and basic limits
+    node->declare_parameter<bool>(ns + ".allow_reverse", allow_reverse_);
+    node->declare_parameter<double>(ns + ".v_progress_min", v_progress_min_);
+    node->declare_parameter<double>(ns + ".k_s_share_max", k_s_share_max_);
+
+    node->declare_parameter<double>(ns + ".max_linear_speed", max_linear_speed_);
+    node->declare_parameter<double>(ns + ".max_angular_speed", max_angular_speed_);
+    node->declare_parameter<double>(ns + ".max_linear_acc", max_linear_acc_);
+    node->declare_parameter<double>(ns + ".max_angular_acc", max_angular_acc_);
+
+    // Tracking
+    node->declare_parameter<double>(ns + ".k_s", k_s_);
+    node->declare_parameter<double>(ns + ".k_theta", k_theta_);
+    node->declare_parameter<double>(ns + ".k_y", k_y_);
+    node->declare_parameter<double>(ns + ".ell", ell_);
+    node->declare_parameter<double>(ns + ".v_ref", v_ref_);
+    node->declare_parameter<double>(ns + ".eps", eps_);
+
+    // Safety
+    node->declare_parameter<double>(ns + ".a_acc", a_acc_);
+    node->declare_parameter<double>(ns + ".a_brake", a_brake_);
+    node->declare_parameter<double>(ns + ".a_lat_max", a_lat_max_);
+    node->declare_parameter<double>(ns + ".d0_margin", d0_margin_);
+    node->declare_parameter<double>(ns + ".tau_latency", tau_latency_);
+    node->declare_parameter<double>(ns + ".d_hard", d_hard_);
+    node->declare_parameter<double>(ns + ".t_emerg", t_emerg_);
+
+    // Blend at vertices
+    node->declare_parameter<double>(ns + ".blend_base", blend_base_);
+    node->declare_parameter<double>(ns + ".blend_k_per_v", blend_k_per_v_);
+    node->declare_parameter<double>(ns + ".kappa_max", kappa_max_);
+
+    // For obstacle detection
+    node->declare_parameter<double>(ns + ".dist_search_radius", dist_search_radius_);
+
+    node->declare_parameter<double>(ns + ".goal_pos_tol", goal_pos_tol_);
+    node->declare_parameter<double>(ns + ".goal_yaw_tol_deg", goal_yaw_tol_deg_);
+    node->declare_parameter<double>(ns + ".slow_radius", slow_radius_);
+    node->declare_parameter<double>(ns + ".slow_min_speed", slow_min_speed_);
+    node->declare_parameter<double>(ns + ".final_align_k", final_align_k_);
+    node->declare_parameter<double>(ns + ".final_align_wmax", final_align_wmax_);
+
+    node->declare_parameter<bool>(ns + ".corner_guard_enable", corner_guard_enable_);
+    node->declare_parameter<double>(ns + ".corner_gain_ey", corner_gain_ey_);
+    node->declare_parameter<double>(ns + ".corner_gain_eth", corner_gain_eth_);
+    node->declare_parameter<double>(ns + ".corner_gain_kappa", corner_gain_kappa_);
+    node->declare_parameter<double>(ns + ".corner_min_alpha", corner_min_alpha_);
+    node->declare_parameter<double>(ns + ".corner_boost_omega", corner_boost_omega_);
+    node->declare_parameter<double>(ns + ".apex_ey_des", apex_ey_des_);
+    node->declare_parameter<double>(ns + ".a_lat_soft", a_lat_soft_);
+  }
 
   // Get
   node->get_parameter<bool>(ns + ".allow_reverse", allow_reverse_);

controllers/easynav_serest_controller/tests/CMakeLists.txt

@@ -0,0 +1,11 @@
+find_package(ament_cmake_gtest REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
+
+ament_add_gtest(serest_controller_reconfigure_tests serest_controller_reconfigure_tests.cpp)
+
+target_link_libraries(serest_controller_reconfigure_tests
+  ${PROJECT_NAME}
+  rclcpp::rclcpp
+  rclcpp_lifecycle::rclcpp_lifecycle
+)