diff --git a/controllers/easynav_mpc_controller/CMakeLists.txt b/controllers/easynav_mpc_controller/CMakeLists.txt index 2b2df165..ef41bd35 100644 --- a/controllers/easynav_mpc_controller/CMakeLists.txt +++ b/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}") diff --git a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp index e2a32068..9c0f690a 100644 --- a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp +++ b/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(plugin_name + ".horizon_steps", horizon_steps_); - node->declare_parameter(plugin_name + ".dt", dt_); - node->declare_parameter(plugin_name + ".safety_radius", safety_radius_); - node->declare_parameter(plugin_name + ".max_linear_velocity", max_lin_vel_); - node->declare_parameter(plugin_name + ".max_angular_velocity", max_ang_vel_); - node->declare_parameter(plugin_name + ".verbose", verbose_); - - node->declare_parameter(plugin_name + ".fallback_goal_pos_tol", fallback_goal_pos_tol_); - node->declare_parameter(plugin_name + ".fallback_goal_yaw_tol", fallback_goal_yaw_tol_); + if (!node->has_parameter(plugin_name + ".horizon_steps")) { + node->declare_parameter(plugin_name + ".horizon_steps", horizon_steps_); + node->declare_parameter(plugin_name + ".dt", dt_); + node->declare_parameter(plugin_name + ".safety_radius", safety_radius_); + node->declare_parameter(plugin_name + ".max_linear_velocity", max_lin_vel_); + node->declare_parameter(plugin_name + ".max_angular_velocity", max_ang_vel_); + node->declare_parameter(plugin_name + ".verbose", verbose_); + + node->declare_parameter(plugin_name + ".fallback_goal_pos_tol", fallback_goal_pos_tol_); + node->declare_parameter(plugin_name + ".fallback_goal_yaw_tol", fallback_goal_yaw_tol_); + } node->get_parameter(plugin_name + ".horizon_steps", horizon_steps_); node->get_parameter(plugin_name + ".dt", dt_); diff --git a/controllers/easynav_mpc_controller/tests/CMakeLists.txt b/controllers/easynav_mpc_controller/tests/CMakeLists.txt new file mode 100644 index 00000000..c368558d --- /dev/null +++ b/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 +) diff --git a/controllers/easynav_mpc_controller/tests/mpc_controller_reconfigure_tests.cpp b/controllers/easynav_mpc_controller/tests/mpc_controller_reconfigure_tests.cpp new file mode 100644 index 00000000..b7f0350b --- /dev/null +++ b/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(); + ASSERT_NO_THROW(plugin1->initialize(node, "test_controller")); + + auto plugin2 = std::make_shared(); + ASSERT_NO_THROW(plugin2->initialize(node, "test_controller")); +} diff --git a/controllers/easynav_mppi_controller/CMakeLists.txt b/controllers/easynav_mppi_controller/CMakeLists.txt index d3aa0726..7239ae35 100644 --- a/controllers/easynav_mppi_controller/CMakeLists.txt +++ b/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}") diff --git a/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIController.cpp b/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIController.cpp index dbd39b82..dbde0253 100644 --- a/controllers/easynav_mppi_controller/src/easynav_mppi_controller/MPPIController.cpp +++ b/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(plugin_name + ".num_samples", num_samples_); - node->declare_parameter(plugin_name + ".horizon_steps", horizon_steps_); - node->declare_parameter(plugin_name + ".dt", dt_); - node->declare_parameter(plugin_name + ".lambda", lambda_); - node->declare_parameter(plugin_name + ".max_linear_velocity", max_lin_vel_); - node->declare_parameter(plugin_name + ".max_angular_velocity", max_ang_vel_); - node->declare_parameter(plugin_name + ".max_linear_acceleration", max_lin_acc_); - node->declare_parameter(plugin_name + ".max_angular_acceleration", max_ang_acc_); - node->declare_parameter(plugin_name + ".fov", fov_); - node->declare_parameter(plugin_name + ".safety_radius", safety_radius_); + if (!node->has_parameter(plugin_name + ".num_samples")) { + node->declare_parameter(plugin_name + ".num_samples", num_samples_); + node->declare_parameter(plugin_name + ".horizon_steps", horizon_steps_); + node->declare_parameter(plugin_name + ".dt", dt_); + node->declare_parameter(plugin_name + ".lambda", lambda_); + node->declare_parameter(plugin_name + ".max_linear_velocity", max_lin_vel_); + node->declare_parameter(plugin_name + ".max_angular_velocity", max_ang_vel_); + node->declare_parameter(plugin_name + ".max_linear_acceleration", max_lin_acc_); + node->declare_parameter(plugin_name + ".max_angular_acceleration", max_ang_acc_); + node->declare_parameter(plugin_name + ".fov", fov_); + node->declare_parameter(plugin_name + ".safety_radius", safety_radius_); + } node->get_parameter(plugin_name + ".num_samples", num_samples_); node->get_parameter(plugin_name + ".horizon_steps", horizon_steps_); diff --git a/controllers/easynav_mppi_controller/tests/CMakeLists.txt b/controllers/easynav_mppi_controller/tests/CMakeLists.txt new file mode 100644 index 00000000..e69261d6 --- /dev/null +++ b/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 +) diff --git a/controllers/easynav_mppi_controller/tests/mppi_controller_reconfigure_tests.cpp b/controllers/easynav_mppi_controller/tests/mppi_controller_reconfigure_tests.cpp new file mode 100644 index 00000000..90746ec9 --- /dev/null +++ b/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(); + ASSERT_NO_THROW(plugin1->initialize(node, "test_controller")); + + auto plugin2 = std::make_shared(); + ASSERT_NO_THROW(plugin2->initialize(node, "test_controller")); +} diff --git a/controllers/easynav_regulated_pp_controller/src/easynav_regulated_pp_controller/RegulatedPurePursuitController.cpp b/controllers/easynav_regulated_pp_controller/src/easynav_regulated_pp_controller/RegulatedPurePursuitController.cpp index 761dd28b..595185f3 100644 --- a/controllers/easynav_regulated_pp_controller/src/easynav_regulated_pp_controller/RegulatedPurePursuitController.cpp +++ b/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); }; diff --git a/controllers/easynav_regulated_pp_controller/tests/CMakeLists.txt b/controllers/easynav_regulated_pp_controller/tests/CMakeLists.txt index 7c7c867f..cef24eaf 100644 --- a/controllers/easynav_regulated_pp_controller/tests/CMakeLists.txt +++ b/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 +) diff --git a/controllers/easynav_regulated_pp_controller/tests/regulated_pp_controller_reconfigure_tests.cpp b/controllers/easynav_regulated_pp_controller/tests/regulated_pp_controller_reconfigure_tests.cpp new file mode 100644 index 00000000..a9e01970 --- /dev/null +++ b/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(); + ASSERT_NO_THROW(plugin1->initialize(node, "test_controller")); + + auto plugin2 = std::make_shared(); + ASSERT_NO_THROW(plugin2->initialize(node, "test_controller")); +} diff --git a/controllers/easynav_serest_controller/CMakeLists.txt b/controllers/easynav_serest_controller/CMakeLists.txt index c77abe56..a4669a75 100644 --- a/controllers/easynav_serest_controller/CMakeLists.txt +++ b/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}") diff --git a/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp b/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp index 1bf7f3d5..0068dd2a 100644 --- a/controllers/easynav_serest_controller/src/easynav_serest_controller/SerestController.cpp +++ b/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(ns + ".allow_reverse", allow_reverse_); - node->declare_parameter(ns + ".v_progress_min", v_progress_min_); - node->declare_parameter(ns + ".k_s_share_max", k_s_share_max_); - - node->declare_parameter(ns + ".max_linear_speed", max_linear_speed_); - node->declare_parameter(ns + ".max_angular_speed", max_angular_speed_); - node->declare_parameter(ns + ".max_linear_acc", max_linear_acc_); - node->declare_parameter(ns + ".max_angular_acc", max_angular_acc_); - - // Tracking - node->declare_parameter(ns + ".k_s", k_s_); - node->declare_parameter(ns + ".k_theta", k_theta_); - node->declare_parameter(ns + ".k_y", k_y_); - node->declare_parameter(ns + ".ell", ell_); - node->declare_parameter(ns + ".v_ref", v_ref_); - node->declare_parameter(ns + ".eps", eps_); - - // Safety - node->declare_parameter(ns + ".a_acc", a_acc_); - node->declare_parameter(ns + ".a_brake", a_brake_); - node->declare_parameter(ns + ".a_lat_max", a_lat_max_); - node->declare_parameter(ns + ".d0_margin", d0_margin_); - node->declare_parameter(ns + ".tau_latency", tau_latency_); - node->declare_parameter(ns + ".d_hard", d_hard_); - node->declare_parameter(ns + ".t_emerg", t_emerg_); - - // Blend at vertices - node->declare_parameter(ns + ".blend_base", blend_base_); - node->declare_parameter(ns + ".blend_k_per_v", blend_k_per_v_); - node->declare_parameter(ns + ".kappa_max", kappa_max_); - - // For obstacle detection - node->declare_parameter(ns + ".dist_search_radius", dist_search_radius_); - - node->declare_parameter(ns + ".goal_pos_tol", goal_pos_tol_); - node->declare_parameter(ns + ".goal_yaw_tol_deg", goal_yaw_tol_deg_); - node->declare_parameter(ns + ".slow_radius", slow_radius_); - node->declare_parameter(ns + ".slow_min_speed", slow_min_speed_); - node->declare_parameter(ns + ".final_align_k", final_align_k_); - node->declare_parameter(ns + ".final_align_wmax", final_align_wmax_); - - node->declare_parameter(ns + ".corner_guard_enable", corner_guard_enable_); - node->declare_parameter(ns + ".corner_gain_ey", corner_gain_ey_); - node->declare_parameter(ns + ".corner_gain_eth", corner_gain_eth_); - node->declare_parameter(ns + ".corner_gain_kappa", corner_gain_kappa_); - node->declare_parameter(ns + ".corner_min_alpha", corner_min_alpha_); - node->declare_parameter(ns + ".corner_boost_omega", corner_boost_omega_); - node->declare_parameter(ns + ".apex_ey_des", apex_ey_des_); - node->declare_parameter(ns + ".a_lat_soft", a_lat_soft_); - + if (!node->has_parameter(ns + ".allow_reverse")) { + // Maximums and basic limits + node->declare_parameter(ns + ".allow_reverse", allow_reverse_); + node->declare_parameter(ns + ".v_progress_min", v_progress_min_); + node->declare_parameter(ns + ".k_s_share_max", k_s_share_max_); + + node->declare_parameter(ns + ".max_linear_speed", max_linear_speed_); + node->declare_parameter(ns + ".max_angular_speed", max_angular_speed_); + node->declare_parameter(ns + ".max_linear_acc", max_linear_acc_); + node->declare_parameter(ns + ".max_angular_acc", max_angular_acc_); + + // Tracking + node->declare_parameter(ns + ".k_s", k_s_); + node->declare_parameter(ns + ".k_theta", k_theta_); + node->declare_parameter(ns + ".k_y", k_y_); + node->declare_parameter(ns + ".ell", ell_); + node->declare_parameter(ns + ".v_ref", v_ref_); + node->declare_parameter(ns + ".eps", eps_); + + // Safety + node->declare_parameter(ns + ".a_acc", a_acc_); + node->declare_parameter(ns + ".a_brake", a_brake_); + node->declare_parameter(ns + ".a_lat_max", a_lat_max_); + node->declare_parameter(ns + ".d0_margin", d0_margin_); + node->declare_parameter(ns + ".tau_latency", tau_latency_); + node->declare_parameter(ns + ".d_hard", d_hard_); + node->declare_parameter(ns + ".t_emerg", t_emerg_); + + // Blend at vertices + node->declare_parameter(ns + ".blend_base", blend_base_); + node->declare_parameter(ns + ".blend_k_per_v", blend_k_per_v_); + node->declare_parameter(ns + ".kappa_max", kappa_max_); + + // For obstacle detection + node->declare_parameter(ns + ".dist_search_radius", dist_search_radius_); + + node->declare_parameter(ns + ".goal_pos_tol", goal_pos_tol_); + node->declare_parameter(ns + ".goal_yaw_tol_deg", goal_yaw_tol_deg_); + node->declare_parameter(ns + ".slow_radius", slow_radius_); + node->declare_parameter(ns + ".slow_min_speed", slow_min_speed_); + node->declare_parameter(ns + ".final_align_k", final_align_k_); + node->declare_parameter(ns + ".final_align_wmax", final_align_wmax_); + + node->declare_parameter(ns + ".corner_guard_enable", corner_guard_enable_); + node->declare_parameter(ns + ".corner_gain_ey", corner_gain_ey_); + node->declare_parameter(ns + ".corner_gain_eth", corner_gain_eth_); + node->declare_parameter(ns + ".corner_gain_kappa", corner_gain_kappa_); + node->declare_parameter(ns + ".corner_min_alpha", corner_min_alpha_); + node->declare_parameter(ns + ".corner_boost_omega", corner_boost_omega_); + node->declare_parameter(ns + ".apex_ey_des", apex_ey_des_); + node->declare_parameter(ns + ".a_lat_soft", a_lat_soft_); + } // Get node->get_parameter(ns + ".allow_reverse", allow_reverse_); diff --git a/controllers/easynav_serest_controller/tests/CMakeLists.txt b/controllers/easynav_serest_controller/tests/CMakeLists.txt new file mode 100644 index 00000000..ce3b3baf --- /dev/null +++ b/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 +) diff --git a/controllers/easynav_serest_controller/tests/serest_controller_reconfigure_tests.cpp b/controllers/easynav_serest_controller/tests/serest_controller_reconfigure_tests.cpp new file mode 100644 index 00000000..6ee98e69 --- /dev/null +++ b/controllers/easynav_serest_controller/tests/serest_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_serest_controller/SerestController.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +#include "gtest/gtest.h" + +class SerestControllerReconfigureTest : public ::testing::Test +{ +protected: + void SetUp() override + { + if (!rclcpp::ok()) { + rclcpp::init(0, nullptr); + } + } +}; + +TEST_F(SerestControllerReconfigureTest, InitializeTwiceOnSameNodeDoesNotThrow) +{ + auto node = rclcpp_lifecycle::LifecycleNode::make_shared("serest_controller_reconfigure_test"); + + auto plugin1 = std::make_shared(); + ASSERT_NO_THROW(plugin1->initialize(node, "test_controller")); + + auto plugin2 = std::make_shared(); + ASSERT_NO_THROW(plugin2->initialize(node, "test_controller")); +} diff --git a/controllers/easynav_simple_controller/src/easynav_simple_controller/SimpleController.cpp b/controllers/easynav_simple_controller/src/easynav_simple_controller/SimpleController.cpp index 62224033..708cadb8 100644 --- a/controllers/easynav_simple_controller/src/easynav_simple_controller/SimpleController.cpp +++ b/controllers/easynav_simple_controller/src/easynav_simple_controller/SimpleController.cpp @@ -39,21 +39,23 @@ SimpleController::on_initialize() auto node = get_node(); const auto & plugin_name = get_plugin_name(); - node->declare_parameter(plugin_name + ".max_linear_speed", max_linear_speed_); - node->declare_parameter(plugin_name + ".max_angular_speed", max_angular_speed_); - node->declare_parameter(plugin_name + ".max_linear_acc", max_linear_acc_); - node->declare_parameter(plugin_name + ".max_angular_acc", max_angular_acc_); - node->declare_parameter(plugin_name + ".look_ahead_dist", look_ahead_dist_); - node->declare_parameter(plugin_name + ".tolerance_dist", tolerance_dist_); - node->declare_parameter(plugin_name + ".k_rot", k_rot_); - node->declare_parameter(plugin_name + ".final_goal_angle_tolerance", - final_goal_angle_tolerance_); - node->declare_parameter(plugin_name + ".linear_kp", linear_kp_); - node->declare_parameter(plugin_name + ".linear_ki", linear_ki_); - node->declare_parameter(plugin_name + ".linear_kd", linear_kd_); - node->declare_parameter(plugin_name + ".angular_kp", angular_kp_); - node->declare_parameter(plugin_name + ".angular_ki", angular_ki_); - node->declare_parameter(plugin_name + ".angular_kd", angular_kd_); + if (!node->has_parameter(plugin_name + ".max_linear_speed")) { + node->declare_parameter(plugin_name + ".max_linear_speed", max_linear_speed_); + node->declare_parameter(plugin_name + ".max_angular_speed", max_angular_speed_); + node->declare_parameter(plugin_name + ".max_linear_acc", max_linear_acc_); + node->declare_parameter(plugin_name + ".max_angular_acc", max_angular_acc_); + node->declare_parameter(plugin_name + ".look_ahead_dist", look_ahead_dist_); + node->declare_parameter(plugin_name + ".tolerance_dist", tolerance_dist_); + node->declare_parameter(plugin_name + ".k_rot", k_rot_); + node->declare_parameter(plugin_name + ".final_goal_angle_tolerance", + final_goal_angle_tolerance_); + node->declare_parameter(plugin_name + ".linear_kp", linear_kp_); + node->declare_parameter(plugin_name + ".linear_ki", linear_ki_); + node->declare_parameter(plugin_name + ".linear_kd", linear_kd_); + node->declare_parameter(plugin_name + ".angular_kp", angular_kp_); + node->declare_parameter(plugin_name + ".angular_ki", angular_ki_); + node->declare_parameter(plugin_name + ".angular_kd", angular_kd_); + } node->get_parameter(plugin_name + ".max_linear_speed", max_linear_speed_); node->get_parameter(plugin_name + ".max_angular_speed", max_angular_speed_); diff --git a/controllers/easynav_vff_controller/CMakeLists.txt b/controllers/easynav_vff_controller/CMakeLists.txt index dd45fd20..533851ba 100644 --- a/controllers/easynav_vff_controller/CMakeLists.txt +++ b/controllers/easynav_vff_controller/CMakeLists.txt @@ -65,6 +65,9 @@ if(BUILD_TESTING) set(ament_cmake_copyright_FOUND TRUE) set(ament_cmake_cpplint_FOUND TRUE) ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_gtest REQUIRED) + add_subdirectory(tests) endif() ament_export_include_directories(include) diff --git a/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp b/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp index 67cdac8a..f7f344f6 100644 --- a/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp +++ b/controllers/easynav_vff_controller/src/easynav_vff_controller/VffController.cpp @@ -34,16 +34,18 @@ void VffController::on_initialize() auto node = get_node(); const auto & plugin_name = get_plugin_name(); - node->declare_parameter(plugin_name + ".distance_obstacle_detection", 3.0); - node->declare_parameter(plugin_name + ".distance_to_goal", 1.0); - node->declare_parameter(plugin_name + ".obstacle_detection_x_min", 0.5); - node->declare_parameter(plugin_name + ".obstacle_detection_x_max", 10.0); - node->declare_parameter(plugin_name + ".obstacle_detection_y_min", -10.0); - node->declare_parameter(plugin_name + ".obstacle_detection_y_max", 10.0); - node->declare_parameter(plugin_name + ".obstacle_detection_z_min", 0.10); - node->declare_parameter(plugin_name + ".obstacle_detection_z_max", 1.00); - node->declare_parameter(plugin_name + ".max_speed", 0.8); - node->declare_parameter(plugin_name + ".max_angular_speed", 1.5); + if (!node->has_parameter(plugin_name + ".distance_obstacle_detection")) { + node->declare_parameter(plugin_name + ".distance_obstacle_detection", 3.0); + node->declare_parameter(plugin_name + ".distance_to_goal", 1.0); + node->declare_parameter(plugin_name + ".obstacle_detection_x_min", 0.5); + node->declare_parameter(plugin_name + ".obstacle_detection_x_max", 10.0); + node->declare_parameter(plugin_name + ".obstacle_detection_y_min", -10.0); + node->declare_parameter(plugin_name + ".obstacle_detection_y_max", 10.0); + node->declare_parameter(plugin_name + ".obstacle_detection_z_min", 0.10); + node->declare_parameter(plugin_name + ".obstacle_detection_z_max", 1.00); + node->declare_parameter(plugin_name + ".max_speed", 0.8); + node->declare_parameter(plugin_name + ".max_angular_speed", 1.5); + } node->get_parameter(plugin_name + ".distance_obstacle_detection", distance_obstacle_detection_); diff --git a/controllers/easynav_vff_controller/tests/CMakeLists.txt b/controllers/easynav_vff_controller/tests/CMakeLists.txt new file mode 100644 index 00000000..77b49649 --- /dev/null +++ b/controllers/easynav_vff_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(vff_controller_reconfigure_tests vff_controller_reconfigure_tests.cpp) + +target_link_libraries(vff_controller_reconfigure_tests + vff_controller + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle +) diff --git a/controllers/easynav_vff_controller/tests/vff_controller_reconfigure_tests.cpp b/controllers/easynav_vff_controller/tests/vff_controller_reconfigure_tests.cpp new file mode 100644 index 00000000..040768a1 --- /dev/null +++ b/controllers/easynav_vff_controller/tests/vff_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_vff_controller/VffController.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +#include "gtest/gtest.h" + +class VffControllerReconfigureTest : public ::testing::Test +{ +protected: + void SetUp() override + { + if (!rclcpp::ok()) { + rclcpp::init(0, nullptr); + } + } +}; + +TEST_F(VffControllerReconfigureTest, InitializeTwiceOnSameNodeDoesNotThrow) +{ + auto node = rclcpp_lifecycle::LifecycleNode::make_shared("vff_controller_reconfigure_test"); + + auto plugin1 = std::make_shared(); + ASSERT_NO_THROW(plugin1->initialize(node, "test_controller")); + + auto plugin2 = std::make_shared(); + ASSERT_NO_THROW(plugin2->initialize(node, "test_controller")); +} diff --git a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp index b6ad3188..6cdc2040 100644 --- a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp @@ -197,20 +197,22 @@ AMCLLocalizer::on_initialize() double std_dev_yaw = 0.5; double reseed_freq = 1.0; - node->declare_parameter(plugin_name + ".num_particles", num_particles); - node->declare_parameter(plugin_name + ".initial_pose.x", x_init); - node->declare_parameter(plugin_name + ".initial_pose.y", y_init); - node->declare_parameter(plugin_name + ".initial_pose.yaw", yaw_init); - node->declare_parameter(plugin_name + ".initial_pose.std_dev_xy", std_dev_xy); - node->declare_parameter(plugin_name + ".initial_pose.std_dev_yaw", std_dev_yaw); - node->declare_parameter(plugin_name + ".reseed_freq", reseed_freq); - node->declare_parameter(plugin_name + ".noise_translation", noise_translation_); - node->declare_parameter(plugin_name + ".noise_rotation", noise_rotation_); - node->declare_parameter(plugin_name + ".noise_translation_to_rotation", - noise_translation_to_rotation_); - node->declare_parameter(plugin_name + ".min_noise_xy", min_noise_xy_); - node->declare_parameter(plugin_name + ".min_noise_yaw", min_noise_yaw_); - node->declare_parameter(plugin_name + ".compute_odom_from_tf", compute_odom_from_tf_); + if (!node->has_parameter(plugin_name + ".num_particles")) { + node->declare_parameter(plugin_name + ".num_particles", num_particles); + node->declare_parameter(plugin_name + ".initial_pose.x", x_init); + node->declare_parameter(plugin_name + ".initial_pose.y", y_init); + node->declare_parameter(plugin_name + ".initial_pose.yaw", yaw_init); + node->declare_parameter(plugin_name + ".initial_pose.std_dev_xy", std_dev_xy); + node->declare_parameter(plugin_name + ".initial_pose.std_dev_yaw", std_dev_yaw); + node->declare_parameter(plugin_name + ".reseed_freq", reseed_freq); + node->declare_parameter(plugin_name + ".noise_translation", noise_translation_); + node->declare_parameter(plugin_name + ".noise_rotation", noise_rotation_); + node->declare_parameter(plugin_name + ".noise_translation_to_rotation", + noise_translation_to_rotation_); + node->declare_parameter(plugin_name + ".min_noise_xy", min_noise_xy_); + node->declare_parameter(plugin_name + ".min_noise_yaw", min_noise_yaw_); + node->declare_parameter(plugin_name + ".compute_odom_from_tf", compute_odom_from_tf_); + } node->get_parameter(plugin_name + ".num_particles", num_particles); node->get_parameter(plugin_name + ".initial_pose.x", x_init); diff --git a/localizers/easynav_costmap_localizer/tests/CMakeLists.txt b/localizers/easynav_costmap_localizer/tests/CMakeLists.txt index b874c059..a5dc9053 100644 --- a/localizers/easynav_costmap_localizer/tests/CMakeLists.txt +++ b/localizers/easynav_costmap_localizer/tests/CMakeLists.txt @@ -9,3 +9,10 @@ target_link_libraries(costmap_localizer_tests rclcpp_lifecycle::rclcpp_lifecycle ${std_srvs_TARGETS} ) + +ament_add_gtest(costmap_localizer_reconfigure_tests costmap_localizer_reconfigure_tests.cpp) +target_link_libraries(costmap_localizer_reconfigure_tests + ${PROJECT_NAME} + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle +) diff --git a/localizers/easynav_costmap_localizer/tests/costmap_localizer_reconfigure_tests.cpp b/localizers/easynav_costmap_localizer/tests/costmap_localizer_reconfigure_tests.cpp new file mode 100644 index 00000000..7763a27c --- /dev/null +++ b/localizers/easynav_costmap_localizer/tests/costmap_localizer_reconfigure_tests.cpp @@ -0,0 +1,49 @@ +// Copyright 2025 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_costmap_localizer/AMCLLocalizer.hpp" +#include "easynav_localizer/LocalizerNode.hpp" + +#include "rclcpp/rclcpp.hpp" + +#include "gtest/gtest.h" + +class CostmapLocalizerReconfigureTest : public ::testing::Test +{ +protected: + void SetUp() override + { + if (!rclcpp::ok()) { + rclcpp::init(0, nullptr); + } + } +}; + +TEST_F(CostmapLocalizerReconfigureTest, InitializeTwiceOnSameNodeDoesNotThrow) +{ + // AMCLLocalizer::on_initialize() requires a LocalizerNode, not a plain LifecycleNode. + auto node = std::make_shared(); + + auto plugin1 = std::make_shared(); + ASSERT_NO_THROW(plugin1->initialize(node, "test_localizer")); + + auto plugin2 = std::make_shared(); + ASSERT_NO_THROW(plugin2->initialize(node, "test_localizer")); +} diff --git a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp index 8488f319..57505423 100644 --- a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp +++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/FusionLocalizer.cpp @@ -92,17 +92,16 @@ void FusionLocalizer::on_initialize() // GPS-related setup only needed when global filter is active if (has_global_filter_) { - localizer_node->declare_parameter(plugin_name + ".latitude_origin", double(0.0)); + if (!localizer_node->has_parameter(plugin_name + ".latitude_origin")) { + localizer_node->declare_parameter(plugin_name + ".latitude_origin", double(0.0)); + localizer_node->declare_parameter(plugin_name + ".longitude_origin", double(0.0)); + localizer_node->declare_parameter(plugin_name + ".altitude_origin", double(0.0)); + localizer_node->declare_parameter( + plugin_name + ".navsatfix_topic", std::string("gps/filtered")); + } localizer_node->get_parameter(plugin_name + ".latitude_origin", latitude_origin_); - - localizer_node->declare_parameter(plugin_name + ".longitude_origin", double(0.0)); localizer_node->get_parameter(plugin_name + ".longitude_origin", longitude_origin_); - - localizer_node->declare_parameter(plugin_name + ".altitude_origin", double(0.0)); localizer_node->get_parameter(plugin_name + ".altitude_origin", altitude_origin_); - - localizer_node->declare_parameter( - plugin_name + ".navsatfix_topic", std::string("gps/filtered")); localizer_node->get_parameter(plugin_name + ".navsatfix_topic", navsatfix_topic_); navsat_pub_ = localizer_node->create_publisher( navsatfix_topic_, rclcpp::QoS(10)); diff --git a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp index 5ce5bd30..eb815046 100644 --- a/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp +++ b/localizers/easynav_fusion_localizer/src/easynav_fusion_localizer/ukf_wrapper.cpp @@ -825,9 +825,17 @@ void UkfWrapper::loadParams() rclcpp::SubscriptionOptions options; options.callback_group = rt_cbg; - double alpha = parent_node_->declare_parameter(param_prefix + "alpha", 0.001); - double kappa = parent_node_->declare_parameter(param_prefix + "kappa", 0.0); - double beta = parent_node_->declare_parameter(param_prefix + "beta", 2.0); + auto declare_or_get = [this](const std::string & name, auto default_value) { + if (!parent_node_->has_parameter(name)) { + parent_node_->declare_parameter(name, default_value); + } + parent_node_->get_parameter(name, default_value); + return default_value; + }; + + double alpha = declare_or_get(param_prefix + "alpha", 0.001); + double kappa = declare_or_get(param_prefix + "kappa", 0.0); + double beta = declare_or_get(param_prefix + "beta", 2.0); filter_.setConstants(alpha, kappa, beta); /* For diagnostic purposes, collect information about how many different @@ -852,20 +860,20 @@ void UkfWrapper::loadParams() twist_var_counts[StateMemberVyaw] = 0; // Determine if we'll be printing diagnostic information - print_diagnostics_ = parent_node_->declare_parameter(param_prefix + "print_diagnostics", false); + print_diagnostics_ = declare_or_get(param_prefix + "print_diagnostics", false); // Check for custom gravitational acceleration value - gravitational_acceleration_ = parent_node_->declare_parameter( + gravitational_acceleration_ = declare_or_get( param_prefix + "gravitational_acceleration", gravitational_acceleration_); // Grab the debug param. If true, the node will produce a LOT of output. - bool debug = parent_node_->declare_parameter(param_prefix + "debug", false); + bool debug = declare_or_get(param_prefix + "debug", false); std::string debug_out_file = "robot_localization_debug.txt"; if (debug) { try { - debug_out_file = parent_node_->declare_parameter(param_prefix + "debug_out_file", + debug_out_file = declare_or_get(param_prefix + "debug_out_file", debug_out_file); debug_stream_.open(debug_out_file.c_str()); @@ -951,41 +959,41 @@ void UkfWrapper::loadParams() } // Whether we're publshing the world_frame->base_link_frame transform - publish_transform_ = parent_node_->declare_parameter(param_prefix + "publish_tf", true); + publish_transform_ = declare_or_get(param_prefix + "publish_tf", true); // Whether we're publishing the acceleration state transform - publish_acceleration_ = parent_node_->declare_parameter(param_prefix + "publish_acceleration", + publish_acceleration_ = declare_or_get(param_prefix + "publish_acceleration", false); // Whether we'll allow old measurements to cause a re-publication of the updated state - permit_corrected_publication_ = parent_node_->declare_parameter(param_prefix + + permit_corrected_publication_ = declare_or_get(param_prefix + "permit_corrected_publication", false); // Transform future dating - double offset_tmp = parent_node_->declare_parameter(param_prefix + "transform_time_offset", 0.0); + double offset_tmp = declare_or_get(param_prefix + "transform_time_offset", 0.0); tf_time_offset_ = rclcpp::Duration::from_seconds(offset_tmp); // Transform timeout - double timeout_tmp = parent_node_->declare_parameter(param_prefix + "transform_timeout", 0.0); + double timeout_tmp = declare_or_get(param_prefix + "transform_timeout", 0.0); tf_timeout_ = rclcpp::Duration::from_seconds(timeout_tmp); // Update frequency and sensor timeout - frequency_ = parent_node_->declare_parameter(param_prefix + "frequency", 30.0); + frequency_ = declare_or_get(param_prefix + "frequency", 30.0); - predict_to_current_time_ = parent_node_->declare_parameter(param_prefix + + predict_to_current_time_ = declare_or_get(param_prefix + "predict_to_current_time", false); sensor_timeout_ = - rclcpp::Duration::from_seconds(parent_node_->declare_parameter(param_prefix + "sensor_timeout", + rclcpp::Duration::from_seconds(declare_or_get(param_prefix + "sensor_timeout", 1.0 / frequency_)); filter_.setSensorTimeout(sensor_timeout_); // Determine if we're in 2D mode - two_d_mode_ = parent_node_->declare_parameter(param_prefix + "two_d_mode", false); + two_d_mode_ = declare_or_get(param_prefix + "two_d_mode", false); // Smoothing window size - smooth_lagged_data_ = parent_node_->declare_parameter(param_prefix + "smooth_lagged_data", false); - double history_length_double = parent_node_->declare_parameter(param_prefix + "history_length", + smooth_lagged_data_ = declare_or_get(param_prefix + "smooth_lagged_data", false); + double history_length_double = declare_or_get(param_prefix + "history_length", 0.0); if (!smooth_lagged_data_ && std::abs(history_length_double) > 0) { @@ -1005,7 +1013,7 @@ void UkfWrapper::loadParams() history_length_ = rclcpp::Duration::from_seconds(std::abs(history_length_double)); // Whether we reset filter on jump back in time - reset_on_time_jump_ = parent_node_->declare_parameter(param_prefix + "reset_on_time_jump", false); + reset_on_time_jump_ = declare_or_get(param_prefix + "reset_on_time_jump", false); // Determine if we're using a control term double control_timeout = sensor_timeout_.seconds(); @@ -1015,12 +1023,15 @@ void UkfWrapper::loadParams() std::vector deceleration_limits; std::vector deceleration_gains; - use_control_ = parent_node_->declare_parameter(param_prefix + "use_control", false); - stamped_control_ = parent_node_->declare_parameter(param_prefix + "stamped_control", true); - control_timeout = parent_node_->declare_parameter(param_prefix + "control_timeout", 0.0); + use_control_ = declare_or_get(param_prefix + "use_control", false); + stamped_control_ = declare_or_get(param_prefix + "stamped_control", true); + control_timeout = declare_or_get(param_prefix + "control_timeout", 0.0); if (use_control_) { - parent_node_->declare_parameter(param_prefix + "control_config", rclcpp::PARAMETER_BOOL_ARRAY); + if (!parent_node_->has_parameter(param_prefix + "control_config")) { + parent_node_->declare_parameter(param_prefix + "control_config", + rclcpp::PARAMETER_BOOL_ARRAY); + } if (parent_node_->get_parameter(param_prefix + "control_config", control_update_vector)) { if (control_update_vector.size() != TWIST_SIZE) { RCLCPP_ERROR_STREAM( @@ -1038,8 +1049,10 @@ void UkfWrapper::loadParams() use_control_ = false; } - parent_node_->declare_parameter(param_prefix + "acceleration_limits", - rclcpp::PARAMETER_DOUBLE_ARRAY); + if (!parent_node_->has_parameter(param_prefix + "acceleration_limits")) { + parent_node_->declare_parameter(param_prefix + "acceleration_limits", + rclcpp::PARAMETER_DOUBLE_ARRAY); + } if (parent_node_->get_parameter(param_prefix + "acceleration_limits", acceleration_limits)) { if (acceleration_limits.size() != TWIST_SIZE) { RCLCPP_ERROR_STREAM( @@ -1057,8 +1070,10 @@ void UkfWrapper::loadParams() acceleration_limits.resize(TWIST_SIZE, 1.0); } - parent_node_->declare_parameter(param_prefix + "acceleration_gains", - rclcpp::PARAMETER_DOUBLE_ARRAY); + if (!parent_node_->has_parameter(param_prefix + "acceleration_gains")) { + parent_node_->declare_parameter(param_prefix + "acceleration_gains", + rclcpp::PARAMETER_DOUBLE_ARRAY); + } if (parent_node_->get_parameter(param_prefix + "acceleration_gains", acceleration_gains)) { const int size = acceleration_gains.size(); if (size != TWIST_SIZE) { @@ -1073,8 +1088,10 @@ void UkfWrapper::loadParams() } } - parent_node_->declare_parameter(param_prefix + "deceleration_limits", - rclcpp::PARAMETER_DOUBLE_ARRAY); + if (!parent_node_->has_parameter(param_prefix + "deceleration_limits")) { + parent_node_->declare_parameter(param_prefix + "deceleration_limits", + rclcpp::PARAMETER_DOUBLE_ARRAY); + } if (parent_node_->get_parameter(param_prefix + "deceleration_limits", deceleration_limits)) { if (deceleration_limits.size() != TWIST_SIZE) { RCLCPP_ERROR_STREAM( @@ -1091,8 +1108,10 @@ void UkfWrapper::loadParams() deceleration_limits = acceleration_limits; } - parent_node_->declare_parameter(param_prefix + "deceleration_gains", - rclcpp::PARAMETER_DOUBLE_ARRAY); + if (!parent_node_->has_parameter(param_prefix + "deceleration_gains")) { + parent_node_->declare_parameter(param_prefix + "deceleration_gains", + rclcpp::PARAMETER_DOUBLE_ARRAY); + } if (parent_node_->get_parameter(param_prefix + "deceleration_gains", deceleration_gains)) { const int size = deceleration_gains.size(); if (size != TWIST_SIZE) { @@ -1120,13 +1139,15 @@ void UkfWrapper::loadParams() deceleration_gains.resize(TWIST_SIZE, 1.0); } - bool dynamic_process_noise_covariance = parent_node_->declare_parameter(param_prefix + + bool dynamic_process_noise_covariance = declare_or_get(param_prefix + "dynamic_process_noise_covariance", false); filter_.setUseDynamicProcessNoiseCovariance( dynamic_process_noise_covariance); std::vector initial_state; - parent_node_->declare_parameter(param_prefix + "initial_state", rclcpp::PARAMETER_DOUBLE_ARRAY); + if (!parent_node_->has_parameter(param_prefix + "initial_state")) { + parent_node_->declare_parameter(param_prefix + "initial_state", rclcpp::PARAMETER_DOUBLE_ARRAY); + } if (parent_node_->get_parameter(param_prefix + "initial_state", initial_state)) { if (initial_state.size() != STATE_SIZE) { RCLCPP_ERROR_STREAM( @@ -1141,7 +1162,7 @@ void UkfWrapper::loadParams() } // Check if the filter should start or not - disabled_at_startup_ = parent_node_->declare_parameter(param_prefix + "disabled_at_startup", + disabled_at_startup_ = declare_or_get(param_prefix + "disabled_at_startup", false); enabled_ = !disabled_at_startup_; @@ -1234,7 +1255,9 @@ void UkfWrapper::loadParams() ss << "odom" << topic_ind++; std::string odom_topic_name = ss.str(); std::string odom_topic; - parent_node_->declare_parameter(param_prefix + odom_topic_name, rclcpp::PARAMETER_STRING); + if (!parent_node_->has_parameter(param_prefix + odom_topic_name)) { + parent_node_->declare_parameter(param_prefix + odom_topic_name, rclcpp::PARAMETER_STRING); + } rclcpp::Parameter parameter; if (parent_node_->get_parameter(param_prefix + odom_topic_name, parameter)) { @@ -1246,12 +1269,12 @@ void UkfWrapper::loadParams() if (more_params) { // Determine if we want to integrate this sensor differentially - bool differential = parent_node_->declare_parameter(param_prefix + + bool differential = declare_or_get(param_prefix + odom_topic_name + std::string("_differential"), false); // Determine if we want to integrate this sensor relatively - bool relative = parent_node_->declare_parameter(param_prefix + odom_topic_name + + bool relative = declare_or_get(param_prefix + odom_topic_name + std::string("_relative"), false); if (relative && differential) { @@ -1264,23 +1287,23 @@ void UkfWrapper::loadParams() } // Consider odometry transformation from the child_frame_id instead of the base_link_frame_id - bool pose_use_child_frame = parent_node_->declare_parameter(param_prefix + + bool pose_use_child_frame = declare_or_get(param_prefix + odom_topic_name + std::string("_pose_use_child_frame"), false); // Check for pose rejection threshold - double pose_mahalanobis_thresh = parent_node_->declare_parameter(param_prefix + + double pose_mahalanobis_thresh = declare_or_get(param_prefix + odom_topic_name + std::string("_pose_rejection_threshold"), std::numeric_limits::max()); // Check for twist rejection threshold - double twist_mahalanobis_thresh = parent_node_->declare_parameter(param_prefix + + double twist_mahalanobis_thresh = declare_or_get(param_prefix + odom_topic_name + std::string("_twist_rejection_threshold"), std::numeric_limits::max()); // Set optional custom queue size - int queue_size = parent_node_->declare_parameter(param_prefix + + int queue_size = declare_or_get(param_prefix + odom_topic_name + std::string("_queue_size"), 10); @@ -1390,7 +1413,9 @@ void UkfWrapper::loadParams() ss << "pose" << topic_ind++; std::string pose_topic_name = ss.str(); std::string pose_topic; - parent_node_->declare_parameter(param_prefix + pose_topic_name, rclcpp::PARAMETER_STRING); + if (!parent_node_->has_parameter(param_prefix + pose_topic_name)) { + parent_node_->declare_parameter(param_prefix + pose_topic_name, rclcpp::PARAMETER_STRING); + } rclcpp::Parameter parameter; if (parent_node_->get_parameter(param_prefix + pose_topic_name, parameter)) { @@ -1401,12 +1426,12 @@ void UkfWrapper::loadParams() } if (more_params) { - bool differential = parent_node_->declare_parameter(param_prefix + + bool differential = declare_or_get(param_prefix + pose_topic_name + std::string("_differential"), false); // Determine if we want to integrate this sensor relatively - bool relative = parent_node_->declare_parameter(param_prefix + + bool relative = declare_or_get(param_prefix + pose_topic_name + std::string("_relative"), false); @@ -1420,13 +1445,13 @@ void UkfWrapper::loadParams() } // Check for pose rejection threshold - double pose_mahalanobis_thresh = parent_node_->declare_parameter(param_prefix + + double pose_mahalanobis_thresh = declare_or_get(param_prefix + pose_topic_name + std::string("_rejection_threshold"), std::numeric_limits::max()); // Set optional custom queue size - int queue_size = parent_node_->declare_parameter(param_prefix + + int queue_size = declare_or_get(param_prefix + pose_topic_name + std::string("_queue_size"), 10); @@ -1508,7 +1533,9 @@ void UkfWrapper::loadParams() ss << "gps" << topic_ind++; std::string gps_topic_name = ss.str(); std::string gps_topic; - parent_node_->declare_parameter(param_prefix + gps_topic_name, rclcpp::PARAMETER_STRING); + if (!parent_node_->has_parameter(param_prefix + gps_topic_name)) { + parent_node_->declare_parameter(param_prefix + gps_topic_name, rclcpp::PARAMETER_STRING); + } rclcpp::Parameter parameter; if (parent_node_->get_parameter(param_prefix + gps_topic_name, parameter)) { @@ -1519,12 +1546,12 @@ void UkfWrapper::loadParams() } if (more_params) { - bool differential = parent_node_->declare_parameter(param_prefix + + bool differential = declare_or_get(param_prefix + gps_topic_name + std::string("_differential"), false); // Determine if we want to integrate this sensor relatively - bool relative = parent_node_->declare_parameter(param_prefix + + bool relative = declare_or_get(param_prefix + gps_topic_name + std::string("_relative"), false); @@ -1538,7 +1565,7 @@ void UkfWrapper::loadParams() } // Check for gps rejection threshold - double gps_mahalanobis_thresh = parent_node_->declare_parameter(param_prefix + + double gps_mahalanobis_thresh = declare_or_get(param_prefix + gps_topic_name + std::string("_rejection_threshold"), std::numeric_limits::max()); @@ -1630,7 +1657,9 @@ void UkfWrapper::loadParams() ss << "twist" << topic_ind++; std::string twist_topic_name = ss.str(); std::string twist_topic; - parent_node_->declare_parameter(param_prefix + twist_topic_name, rclcpp::PARAMETER_STRING); + if (!parent_node_->has_parameter(param_prefix + twist_topic_name)) { + parent_node_->declare_parameter(param_prefix + twist_topic_name, rclcpp::PARAMETER_STRING); + } rclcpp::Parameter parameter; if (parent_node_->get_parameter(param_prefix + twist_topic_name, parameter)) { @@ -1642,13 +1671,13 @@ void UkfWrapper::loadParams() if (more_params) { // Check for twist rejection threshold - double twist_mahalanobis_thresh = parent_node_->declare_parameter(param_prefix + + double twist_mahalanobis_thresh = declare_or_get(param_prefix + twist_topic_name + std::string("_rejection_threshold"), std::numeric_limits::max()); // Set optional custom queue size - int queue_size = parent_node_->declare_parameter(param_prefix + + int queue_size = declare_or_get(param_prefix + twist_topic_name + std::string("_queue_size"), 10); @@ -1710,7 +1739,9 @@ void UkfWrapper::loadParams() ss << "imu" << topic_ind++; std::string imu_topic_name = ss.str(); std::string imu_topic; - parent_node_->declare_parameter(param_prefix + imu_topic_name, rclcpp::PARAMETER_STRING); + if (!parent_node_->has_parameter(param_prefix + imu_topic_name)) { + parent_node_->declare_parameter(param_prefix + imu_topic_name, rclcpp::PARAMETER_STRING); + } rclcpp::Parameter parameter; if (parent_node_->get_parameter(param_prefix + imu_topic_name, parameter)) { @@ -1721,12 +1752,12 @@ void UkfWrapper::loadParams() } if (more_params) { - bool differential = parent_node_->declare_parameter(param_prefix + + bool differential = declare_or_get(param_prefix + imu_topic_name + std::string("_differential"), false); // Determine if we want to integrate this sensor relatively - bool relative = parent_node_->declare_parameter(param_prefix + imu_topic_name + + bool relative = declare_or_get(param_prefix + imu_topic_name + std::string("_relative"), false); if (relative && differential) { @@ -1739,7 +1770,7 @@ void UkfWrapper::loadParams() } // Check for pose rejection threshold - double pose_mahalanobis_thresh = parent_node_->declare_parameter(param_prefix + + double pose_mahalanobis_thresh = declare_or_get(param_prefix + imu_topic_name + std::string("_pose_rejection_threshold"), std::numeric_limits::max()); @@ -1747,17 +1778,17 @@ void UkfWrapper::loadParams() // Check for angular velocity rejection threshold std::string imu_twist_rejection_name = imu_topic_name + std::string("_twist_rejection_threshold"); - double twist_mahalanobis_thresh = parent_node_->declare_parameter(param_prefix + + double twist_mahalanobis_thresh = declare_or_get(param_prefix + imu_twist_rejection_name, std::numeric_limits::max()); // Check for acceleration rejection threshold - double accel_mahalanobis_thresh = parent_node_->declare_parameter(param_prefix + + double accel_mahalanobis_thresh = declare_or_get(param_prefix + imu_topic_name + std::string("_linear_acceleration_rejection_threshold"), std::numeric_limits::max()); - bool remove_grav_acc = parent_node_->declare_parameter(param_prefix + + bool remove_grav_acc = declare_or_get(param_prefix + imu_topic_name + "_remove_gravitational_acceleration", false); @@ -1765,7 +1796,7 @@ void UkfWrapper::loadParams() remove_grav_acc; // Set optional custom queue size - int queue_size = parent_node_->declare_parameter(param_prefix + + int queue_size = declare_or_get(param_prefix + imu_topic_name + std::string("_queue_size"), 10); @@ -2030,7 +2061,9 @@ void UkfWrapper::loadParams() covariance.setZero(); std::vector covar_flat; - parent_node_->declare_parameter(parameter, rclcpp::PARAMETER_DOUBLE_ARRAY); + if (!parent_node_->has_parameter(parameter)) { + parent_node_->declare_parameter(parameter, rclcpp::PARAMETER_DOUBLE_ARRAY); + } if (parent_node_->get_parameter(parameter, covar_flat)) { if (covar_flat.size() == STATE_SIZE) { RCLCPP_INFO_STREAM( @@ -2840,7 +2873,10 @@ std::vector UkfWrapper::loadUpdateConfig(const std::string & topic_name) std::string prefix = plugin_name_.empty() ? "" : plugin_name_ + "."; const std::string topic_config_name = prefix + topic_name + "_config"; - update_vector = parent_node_->declare_parameter(topic_config_name, update_vector); + if (!parent_node_->has_parameter(topic_config_name)) { + parent_node_->declare_parameter(topic_config_name, update_vector); + } + parent_node_->get_parameter(topic_config_name, update_vector); return update_vector; } diff --git a/localizers/easynav_fusion_localizer/tests/CMakeLists.txt b/localizers/easynav_fusion_localizer/tests/CMakeLists.txt index 8d470925..6c9da3a6 100644 --- a/localizers/easynav_fusion_localizer/tests/CMakeLists.txt +++ b/localizers/easynav_fusion_localizer/tests/CMakeLists.txt @@ -10,3 +10,12 @@ target_link_libraries(fusion_localizer_tests rclcpp_lifecycle::rclcpp_lifecycle easynav_localizer::easynav_localizer ) + +ament_add_gtest(fusion_localizer_reconfigure_tests fusion_localizer_reconfigure_tests.cpp) + +target_link_libraries(fusion_localizer_reconfigure_tests + fusion_localizer + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + easynav_localizer::easynav_localizer +) diff --git a/localizers/easynav_fusion_localizer/tests/fusion_localizer_reconfigure_tests.cpp b/localizers/easynav_fusion_localizer/tests/fusion_localizer_reconfigure_tests.cpp new file mode 100644 index 00000000..d588329e --- /dev/null +++ b/localizers/easynav_fusion_localizer/tests/fusion_localizer_reconfigure_tests.cpp @@ -0,0 +1,54 @@ +// Copyright 2025 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_fusion_localizer/FusionLocalizer.hpp" +#include "easynav_localizer/LocalizerNode.hpp" + +#include "rclcpp/rclcpp.hpp" + +#include "gtest/gtest.h" + +class FusionLocalizerReconfigureTest : public ::testing::Test +{ +protected: + void SetUp() override + { + if (!rclcpp::ok()) { + rclcpp::init(0, nullptr); + } + } +}; + +TEST_F(FusionLocalizerReconfigureTest, InitializeTwiceOnSameNodeDoesNotThrow) +{ + rclcpp::NodeOptions options; + options.parameter_overrides({ + rclcpp::Parameter("test_localizer.global_filter.frequency", 30.0), + }); + + // FusionLocalizer::on_initialize() requires a LocalizerNode, not a plain LifecycleNode. + auto node = std::make_shared(options); + + auto plugin1 = std::make_shared(); + ASSERT_NO_THROW(plugin1->initialize(node, "test_localizer")); + + auto plugin2 = std::make_shared(); + ASSERT_NO_THROW(plugin2->initialize(node, "test_localizer")); +} diff --git a/localizers/easynav_gps_localizer/src/easynav_gps_localizer/GpsLocalizer.cpp b/localizers/easynav_gps_localizer/src/easynav_gps_localizer/GpsLocalizer.cpp index a0ab2cf9..e7576177 100644 --- a/localizers/easynav_gps_localizer/src/easynav_gps_localizer/GpsLocalizer.cpp +++ b/localizers/easynav_gps_localizer/src/easynav_gps_localizer/GpsLocalizer.cpp @@ -56,9 +56,11 @@ void GpsLocalizer::on_initialize() std::bind(&GpsLocalizer::init_pose_callback, this, std::placeholders::_1)); // Optional initial pose from parameters (kept consistent with AMCL parameter names) - node->declare_parameter(plugin_name + ".initial_pose.x", 0.0); - node->declare_parameter(plugin_name + ".initial_pose.y", 0.0); - node->declare_parameter(plugin_name + ".initial_pose.yaw", 0.0); + if (!node->has_parameter(plugin_name + ".initial_pose.x")) { + node->declare_parameter(plugin_name + ".initial_pose.x", 0.0); + node->declare_parameter(plugin_name + ".initial_pose.y", 0.0); + node->declare_parameter(plugin_name + ".initial_pose.yaw", 0.0); + } double init_x = 0.0; double init_y = 0.0; diff --git a/localizers/easynav_gps_localizer/tests/CMakeLists.txt b/localizers/easynav_gps_localizer/tests/CMakeLists.txt index 0e8b0048..be182147 100644 --- a/localizers/easynav_gps_localizer/tests/CMakeLists.txt +++ b/localizers/easynav_gps_localizer/tests/CMakeLists.txt @@ -8,3 +8,11 @@ target_link_libraries(gps_localizer_tests rclcpp::rclcpp rclcpp_lifecycle::rclcpp_lifecycle ) + +ament_add_gtest(gps_localizer_reconfigure_tests gps_localizer_reconfigure_tests.cpp) + +target_link_libraries(gps_localizer_reconfigure_tests + gps_localizer + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle +) diff --git a/localizers/easynav_gps_localizer/tests/gps_localizer_reconfigure_tests.cpp b/localizers/easynav_gps_localizer/tests/gps_localizer_reconfigure_tests.cpp new file mode 100644 index 00000000..836f3ed4 --- /dev/null +++ b/localizers/easynav_gps_localizer/tests/gps_localizer_reconfigure_tests.cpp @@ -0,0 +1,48 @@ +// Copyright 2025 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_gps_localizer/GpsLocalizer.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +#include "gtest/gtest.h" + +class GpsLocalizerReconfigureTest : public ::testing::Test +{ +protected: + void SetUp() override + { + if (!rclcpp::ok()) { + rclcpp::init(0, nullptr); + } + } +}; + +TEST_F(GpsLocalizerReconfigureTest, InitializeTwiceOnSameNodeDoesNotThrow) +{ + auto node = rclcpp_lifecycle::LifecycleNode::make_shared("gps_localizer_reconfigure_test"); + + auto plugin1 = std::make_shared(); + ASSERT_NO_THROW(plugin1->initialize(node, "test_localizer")); + + auto plugin2 = std::make_shared(); + ASSERT_NO_THROW(plugin2->initialize(node, "test_localizer")); +} diff --git a/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp b/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp index fe3b726f..c3f39761 100644 --- a/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_navmap_localizer/src/easynav_navmap_localizer/AMCLLocalizer.cpp @@ -371,25 +371,27 @@ void AMCLLocalizer::on_initialize() double x_init, y_init, yaw_init, std_dev_xy, std_dev_yaw; std::string perception_model; - node->declare_parameter(plugin_name + ".num_particles", 100); - node->declare_parameter(plugin_name + ".initial_pose.x", 0.0); - node->declare_parameter(plugin_name + ".initial_pose.y", 0.0); - node->declare_parameter(plugin_name + ".initial_pose.yaw", 0.0); - node->declare_parameter(plugin_name + ".initial_pose.std_dev_xy", 0.5); - node->declare_parameter(plugin_name + ".initial_pose.std_dev_yaw", 0.5); - node->declare_parameter(plugin_name + ".reseed_freq", 1.0); - node->declare_parameter(plugin_name + ".noise_translation", 0.01); - node->declare_parameter(plugin_name + ".noise_rotation", 0.01); - node->declare_parameter(plugin_name + ".noise_translation_to_rotation", 0.01); - node->declare_parameter(plugin_name + ".min_noise_xy", 0.05); - node->declare_parameter(plugin_name + ".min_noise_yaw", 0.05); - node->declare_parameter(plugin_name + ".compute_odom_from_tf", false); - node->declare_parameter(plugin_name + ".inflation_stddev", 0.05); - node->declare_parameter(plugin_name + ".inflation_prob_min", 0.01); - node->declare_parameter(plugin_name + ".correct_max_points", 1500); - node->declare_parameter(plugin_name + ".weights_tau", 0.7); - node->declare_parameter(plugin_name + ".top_keep_fraction", 0.2); - node->declare_parameter(plugin_name + ".downsampled_cloud_size", 0.05); + if (!node->has_parameter(plugin_name + ".num_particles")) { + node->declare_parameter(plugin_name + ".num_particles", 100); + node->declare_parameter(plugin_name + ".initial_pose.x", 0.0); + node->declare_parameter(plugin_name + ".initial_pose.y", 0.0); + node->declare_parameter(plugin_name + ".initial_pose.yaw", 0.0); + node->declare_parameter(plugin_name + ".initial_pose.std_dev_xy", 0.5); + node->declare_parameter(plugin_name + ".initial_pose.std_dev_yaw", 0.5); + node->declare_parameter(plugin_name + ".reseed_freq", 1.0); + node->declare_parameter(plugin_name + ".noise_translation", 0.01); + node->declare_parameter(plugin_name + ".noise_rotation", 0.01); + node->declare_parameter(plugin_name + ".noise_translation_to_rotation", 0.01); + node->declare_parameter(plugin_name + ".min_noise_xy", 0.05); + node->declare_parameter(plugin_name + ".min_noise_yaw", 0.05); + node->declare_parameter(plugin_name + ".compute_odom_from_tf", false); + node->declare_parameter(plugin_name + ".inflation_stddev", 0.05); + node->declare_parameter(plugin_name + ".inflation_prob_min", 0.01); + node->declare_parameter(plugin_name + ".correct_max_points", 1500); + node->declare_parameter(plugin_name + ".weights_tau", 0.7); + node->declare_parameter(plugin_name + ".top_keep_fraction", 0.2); + node->declare_parameter(plugin_name + ".downsampled_cloud_size", 0.05); + } node->get_parameter(plugin_name + ".num_particles", num_particles); node->get_parameter(plugin_name + ".initial_pose.x", x_init); diff --git a/localizers/easynav_navmap_localizer/tests/CMakeLists.txt b/localizers/easynav_navmap_localizer/tests/CMakeLists.txt index 400b4523..0fc749d3 100644 --- a/localizers/easynav_navmap_localizer/tests/CMakeLists.txt +++ b/localizers/easynav_navmap_localizer/tests/CMakeLists.txt @@ -9,3 +9,10 @@ target_link_libraries(navmap_localizer_tests rclcpp_lifecycle::rclcpp_lifecycle ${std_srvs_TARGETS} ) + +ament_add_gtest(navmap_localizer_reconfigure_tests navmap_localizer_reconfigure_tests.cpp) +target_link_libraries(navmap_localizer_reconfigure_tests + ${PROJECT_NAME} + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle +) diff --git a/localizers/easynav_navmap_localizer/tests/navmap_localizer_reconfigure_tests.cpp b/localizers/easynav_navmap_localizer/tests/navmap_localizer_reconfigure_tests.cpp new file mode 100644 index 00000000..ceb5ccff --- /dev/null +++ b/localizers/easynav_navmap_localizer/tests/navmap_localizer_reconfigure_tests.cpp @@ -0,0 +1,48 @@ +// Copyright 2025 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_navmap_localizer/AMCLLocalizer.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +#include "gtest/gtest.h" + +class NavmapLocalizerReconfigureTest : public ::testing::Test +{ +protected: + void SetUp() override + { + if (!rclcpp::ok()) { + rclcpp::init(0, nullptr); + } + } +}; + +TEST_F(NavmapLocalizerReconfigureTest, InitializeTwiceOnSameNodeDoesNotThrow) +{ + auto node = rclcpp_lifecycle::LifecycleNode::make_shared("navmap_localizer_reconfigure_test"); + + auto plugin1 = std::make_shared(); + ASSERT_NO_THROW(plugin1->initialize(node, "test_localizer")); + + auto plugin2 = std::make_shared(); + ASSERT_NO_THROW(plugin2->initialize(node, "test_localizer")); +} diff --git a/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp b/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp index a5182672..926c2dc4 100644 --- a/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_simple_localizer/src/easynav_simple_localizer/AMCLLocalizer.cpp @@ -195,19 +195,21 @@ AMCLLocalizer::on_initialize() double std_dev_yaw = 0.5; double reseed_freq = 1.0; - node->declare_parameter(plugin_name + ".num_particles", num_particles); - node->declare_parameter(plugin_name + ".initial_pose.x", x_init); - node->declare_parameter(plugin_name + ".initial_pose.y", y_init); - node->declare_parameter(plugin_name + ".initial_pose.yaw", yaw_init); - node->declare_parameter(plugin_name + ".initial_pose.std_dev_xy", std_dev_xy); - node->declare_parameter(plugin_name + ".initial_pose.std_dev_yaw", std_dev_yaw); - node->declare_parameter(plugin_name + ".reseed_freq", reseed_freq); - node->declare_parameter(plugin_name + ".noise_translation", noise_translation_); - node->declare_parameter(plugin_name + ".noise_rotation", noise_rotation_); - node->declare_parameter(plugin_name + ".noise_translation_to_rotation", - noise_translation_to_rotation_); - node->declare_parameter(plugin_name + ".min_noise_xy", min_noise_xy_); - node->declare_parameter(plugin_name + ".min_noise_yaw", min_noise_yaw_); + if (!node->has_parameter(plugin_name + ".num_particles")) { + node->declare_parameter(plugin_name + ".num_particles", num_particles); + node->declare_parameter(plugin_name + ".initial_pose.x", x_init); + node->declare_parameter(plugin_name + ".initial_pose.y", y_init); + node->declare_parameter(plugin_name + ".initial_pose.yaw", yaw_init); + node->declare_parameter(plugin_name + ".initial_pose.std_dev_xy", std_dev_xy); + node->declare_parameter(plugin_name + ".initial_pose.std_dev_yaw", std_dev_yaw); + node->declare_parameter(plugin_name + ".reseed_freq", reseed_freq); + node->declare_parameter(plugin_name + ".noise_translation", noise_translation_); + node->declare_parameter(plugin_name + ".noise_rotation", noise_rotation_); + node->declare_parameter(plugin_name + ".noise_translation_to_rotation", + noise_translation_to_rotation_); + node->declare_parameter(plugin_name + ".min_noise_xy", min_noise_xy_); + node->declare_parameter(plugin_name + ".min_noise_yaw", min_noise_yaw_); + } node->get_parameter(plugin_name + ".num_particles", num_particles); node->get_parameter(plugin_name + ".initial_pose.x", x_init); diff --git a/localizers/easynav_simple_localizer/tests/CMakeLists.txt b/localizers/easynav_simple_localizer/tests/CMakeLists.txt index a33140c8..5f773189 100644 --- a/localizers/easynav_simple_localizer/tests/CMakeLists.txt +++ b/localizers/easynav_simple_localizer/tests/CMakeLists.txt @@ -9,3 +9,10 @@ target_link_libraries(simple_localizer_tests rclcpp_lifecycle::rclcpp_lifecycle ${std_srvs_TARGETS} ) + +ament_add_gtest(simple_localizer_reconfigure_tests simple_localizer_reconfigure_tests.cpp) +target_link_libraries(simple_localizer_reconfigure_tests + ${PROJECT_NAME} + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle +) diff --git a/localizers/easynav_simple_localizer/tests/simple_localizer_reconfigure_tests.cpp b/localizers/easynav_simple_localizer/tests/simple_localizer_reconfigure_tests.cpp new file mode 100644 index 00000000..4da05774 --- /dev/null +++ b/localizers/easynav_simple_localizer/tests/simple_localizer_reconfigure_tests.cpp @@ -0,0 +1,48 @@ +// Copyright 2025 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_simple_localizer/AMCLLocalizer.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +#include "gtest/gtest.h" + +class SimpleLocalizerReconfigureTest : public ::testing::Test +{ +protected: + void SetUp() override + { + if (!rclcpp::ok()) { + rclcpp::init(0, nullptr); + } + } +}; + +TEST_F(SimpleLocalizerReconfigureTest, InitializeTwiceOnSameNodeDoesNotThrow) +{ + auto node = rclcpp_lifecycle::LifecycleNode::make_shared("simple_localizer_reconfigure_test"); + + auto plugin1 = std::make_shared(); + ASSERT_NO_THROW(plugin1->initialize(node, "test_localizer")); + + auto plugin2 = std::make_shared(); + ASSERT_NO_THROW(plugin2->initialize(node, "test_localizer")); +} diff --git a/maps_managers/easynav_bonxai_maps_manager/CMakeLists.txt b/maps_managers/easynav_bonxai_maps_manager/CMakeLists.txt index 942a72d1..b4d55534 100644 --- a/maps_managers/easynav_bonxai_maps_manager/CMakeLists.txt +++ b/maps_managers/easynav_bonxai_maps_manager/CMakeLists.txt @@ -98,6 +98,9 @@ if(BUILD_TESTING) # reformatting upstream code (same rationale as copyright/cpplint above). set(ament_cmake_uncrustify_ADDITIONAL_EXCLUDE include/bonxai/*) ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_gtest REQUIRED) + add_subdirectory(tests) endif() ament_export_include_directories("include/${PROJECT_NAME}") diff --git a/maps_managers/easynav_bonxai_maps_manager/src/easynav_bonxai_maps_manager/BonxaiMapsManager.cpp b/maps_managers/easynav_bonxai_maps_manager/src/easynav_bonxai_maps_manager/BonxaiMapsManager.cpp index 4ad4cfb4..ab421caf 100644 --- a/maps_managers/easynav_bonxai_maps_manager/src/easynav_bonxai_maps_manager/BonxaiMapsManager.cpp +++ b/maps_managers/easynav_bonxai_maps_manager/src/easynav_bonxai_maps_manager/BonxaiMapsManager.cpp @@ -55,10 +55,12 @@ BonxaiMapsManager::on_initialize() const auto & plugin_name = get_plugin_name(); std::string package_name, bonxai_path_file, occmap_path_file; - node->declare_parameter(plugin_name + ".package", package_name); - node->declare_parameter(plugin_name + ".bonxai_path_file", bonxai_path_file); - node->declare_parameter(plugin_name + ".occmap_path_file", occmap_path_file); - node->declare_parameter(plugin_name + ".resolution", resolution_); + if (!node->has_parameter(plugin_name + ".package")) { + node->declare_parameter(plugin_name + ".package", package_name); + node->declare_parameter(plugin_name + ".bonxai_path_file", bonxai_path_file); + node->declare_parameter(plugin_name + ".occmap_path_file", occmap_path_file); + node->declare_parameter(plugin_name + ".resolution", resolution_); + } node->get_parameter(plugin_name + ".package", package_name); node->get_parameter(plugin_name + ".bonxai_path_file", bonxai_path_file); diff --git a/maps_managers/easynav_bonxai_maps_manager/tests/CMakeLists.txt b/maps_managers/easynav_bonxai_maps_manager/tests/CMakeLists.txt new file mode 100644 index 00000000..f00f1331 --- /dev/null +++ b/maps_managers/easynav_bonxai_maps_manager/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(bonxai_maps_manager_reconfigure_tests bonxai_maps_manager_reconfigure_tests.cpp) + +target_link_libraries(bonxai_maps_manager_reconfigure_tests + ${PROJECT_NAME} + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle +) diff --git a/maps_managers/easynav_bonxai_maps_manager/tests/bonxai_maps_manager_reconfigure_tests.cpp b/maps_managers/easynav_bonxai_maps_manager/tests/bonxai_maps_manager_reconfigure_tests.cpp new file mode 100644 index 00000000..0b95a86c --- /dev/null +++ b/maps_managers/easynav_bonxai_maps_manager/tests/bonxai_maps_manager_reconfigure_tests.cpp @@ -0,0 +1,48 @@ +// 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_bonxai_maps_manager/BonxaiMapsManager.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +#include "gtest/gtest.h" + +class BonxaiMapsManagerReconfigureTest : public ::testing::Test +{ +protected: + void SetUp() override + { + if (!rclcpp::ok()) { + rclcpp::init(0, nullptr); + } + } +}; + +TEST_F(BonxaiMapsManagerReconfigureTest, InitializeTwiceOnSameNodeDoesNotThrow) +{ + auto node = rclcpp_lifecycle::LifecycleNode::make_shared( + "bonxai_maps_manager_reconfigure_test"); + + auto plugin1 = std::make_shared(); + ASSERT_NO_THROW(plugin1->initialize(node, "test_maps_manager")); + + auto plugin2 = std::make_shared(); + ASSERT_NO_THROW(plugin2->initialize(node, "test_maps_manager")); +} diff --git a/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/CostmapMapsManager.cpp b/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/CostmapMapsManager.cpp index 7e742a77..58742e03 100644 --- a/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/CostmapMapsManager.cpp +++ b/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/CostmapMapsManager.cpp @@ -55,19 +55,25 @@ CostmapMapsManager::on_initialize() const auto & plugin_name = get_plugin_name(); std::string package_name, map_path_file; - node->declare_parameter(plugin_name + ".package", package_name); - node->declare_parameter(plugin_name + ".map_path_file", map_path_file); + if (!node->has_parameter(plugin_name + ".package")) { + node->declare_parameter(plugin_name + ".package", package_name); + node->declare_parameter(plugin_name + ".map_path_file", map_path_file); + } node->get_parameter(plugin_name + ".package", package_name); node->get_parameter(plugin_name + ".map_path_file", map_path_file); std::vector costmap_filters; - node->declare_parameter(plugin_name + ".filters", costmap_filters); + if (!node->has_parameter(plugin_name + ".filters")) { + node->declare_parameter(plugin_name + ".filters", costmap_filters); + } node->get_parameter(plugin_name + ".filters", costmap_filters); for (const auto & costmap_filter : costmap_filters) { std::string plugin; - node->declare_parameter(plugin_name + "." + costmap_filter + ".plugin", plugin); + if (!node->has_parameter(plugin_name + "." + costmap_filter + ".plugin")) { + node->declare_parameter(plugin_name + "." + costmap_filter + ".plugin", plugin); + } node->get_parameter(plugin_name + "." + costmap_filter + ".plugin", plugin); try { diff --git a/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/InflationFilter.cpp b/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/InflationFilter.cpp index e2a8326c..4e3296ce 100644 --- a/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/InflationFilter.cpp +++ b/maps_managers/easynav_costmap_maps_manager/src/easynav_costmap_maps_manager/filters/InflationFilter.cpp @@ -79,9 +79,11 @@ InflationFilter::on_initialize() inscribed_radius_ = 0.25; cost_scaling_factor_ = 3.0; - node->declare_parameter(plugin_name_ + ".inflation_radius", inflation_radius_); - node->declare_parameter(plugin_name_ + ".inscribed_radius", inscribed_radius_); - node->declare_parameter(plugin_name_ + ".cost_scaling_factor", cost_scaling_factor_); + if (!node->has_parameter(plugin_name_ + ".inflation_radius")) { + node->declare_parameter(plugin_name_ + ".inflation_radius", inflation_radius_); + node->declare_parameter(plugin_name_ + ".inscribed_radius", inscribed_radius_); + node->declare_parameter(plugin_name_ + ".cost_scaling_factor", cost_scaling_factor_); + } node->get_parameter(plugin_name_ + ".inflation_radius", inflation_radius_); node->get_parameter(plugin_name_ + ".inscribed_radius", inscribed_radius_); node->get_parameter(plugin_name_ + ".cost_scaling_factor", cost_scaling_factor_); diff --git a/maps_managers/easynav_costmap_maps_manager/tests/CMakeLists.txt b/maps_managers/easynav_costmap_maps_manager/tests/CMakeLists.txt index 92e107de..1dc9abad 100644 --- a/maps_managers/easynav_costmap_maps_manager/tests/CMakeLists.txt +++ b/maps_managers/easynav_costmap_maps_manager/tests/CMakeLists.txt @@ -1,2 +1,5 @@ ament_add_gtest(costmap_mapsmanager_tests costmap_mapsmanager_tests.cpp) target_link_libraries(costmap_mapsmanager_tests ${PROJECT_NAME}) + +ament_add_gtest(costmap_mapsmanager_reconfigure_tests costmap_mapsmanager_reconfigure_tests.cpp) +target_link_libraries(costmap_mapsmanager_reconfigure_tests ${PROJECT_NAME}) diff --git a/maps_managers/easynav_costmap_maps_manager/tests/costmap_mapsmanager_reconfigure_tests.cpp b/maps_managers/easynav_costmap_maps_manager/tests/costmap_mapsmanager_reconfigure_tests.cpp new file mode 100644 index 00000000..79281bbe --- /dev/null +++ b/maps_managers/easynav_costmap_maps_manager/tests/costmap_mapsmanager_reconfigure_tests.cpp @@ -0,0 +1,49 @@ +// Copyright 2025 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_costmap_maps_manager/CostmapMapsManager.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +#include "gtest/gtest.h" + +class CostmapMapsManagerReconfigureTest : public ::testing::Test +{ +protected: + void SetUp() override + { + if (!rclcpp::ok()) { + rclcpp::init(0, nullptr); + } + } +}; + +TEST_F(CostmapMapsManagerReconfigureTest, InitializeTwiceOnSameNodeDoesNotThrow) +{ + auto node = rclcpp_lifecycle::LifecycleNode::make_shared( + "costmap_maps_manager_reconfigure_test"); + + auto plugin1 = std::make_shared(); + ASSERT_NO_THROW(plugin1->initialize(node, "test_maps_manager")); + + auto plugin2 = std::make_shared(); + ASSERT_NO_THROW(plugin2->initialize(node, "test_maps_manager")); +} diff --git a/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/NavMapMapsManager.cpp b/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/NavMapMapsManager.cpp index 663160a5..c881afd1 100644 --- a/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/NavMapMapsManager.cpp +++ b/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/NavMapMapsManager.cpp @@ -66,21 +66,27 @@ NavMapMapsManager::on_initialize() const auto & plugin_name = get_plugin_name(); std::string package_name, occmap_path_file, navmap_path_file; - node->declare_parameter(plugin_name + ".package", package_name); - node->declare_parameter(plugin_name + ".occmap_path_file", occmap_path_file); - node->declare_parameter(plugin_name + ".navmap_path_file", navmap_path_file); + if (!node->has_parameter(plugin_name + ".package")) { + node->declare_parameter(plugin_name + ".package", package_name); + node->declare_parameter(plugin_name + ".occmap_path_file", occmap_path_file); + node->declare_parameter(plugin_name + ".navmap_path_file", navmap_path_file); + } node->get_parameter(plugin_name + ".package", package_name); node->get_parameter(plugin_name + ".occmap_path_file", occmap_path_file); node->get_parameter(plugin_name + ".navmap_path_file", navmap_path_file); std::vector navmap_filters; - node->declare_parameter(plugin_name + ".filters", navmap_filters); + if (!node->has_parameter(plugin_name + ".filters")) { + node->declare_parameter(plugin_name + ".filters", navmap_filters); + } node->get_parameter(plugin_name + ".filters", navmap_filters); for (const auto & navmap_filter : navmap_filters) { std::string plugin; - node->declare_parameter(plugin_name + "." + navmap_filter + ".plugin", plugin); + if (!node->has_parameter(plugin_name + "." + navmap_filter + ".plugin")) { + node->declare_parameter(plugin_name + "." + navmap_filter + ".plugin", plugin); + } node->get_parameter(plugin_name + "." + navmap_filter + ".plugin", plugin); try { diff --git a/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/InflationFilter.cpp b/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/InflationFilter.cpp index 8a96b347..b9e99f0b 100644 --- a/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/InflationFilter.cpp +++ b/maps_managers/easynav_navmap_maps_manager/src/easynav_navmap_maps_manager/filters/InflationFilter.cpp @@ -205,9 +205,11 @@ InflationFilter::on_initialize() cost_scaling_factor_ = 3.0f; inscribed_radius_ = 0.30f; - node->declare_parameter(plugin_name_ + ".inflation_radius", inflation_radius_); - node->declare_parameter(plugin_name_ + ".cost_scaling_factor", cost_scaling_factor_); - node->declare_parameter(plugin_name_ + ".inscribed_radius", inscribed_radius_); + if (!node->has_parameter(plugin_name_ + ".inflation_radius")) { + node->declare_parameter(plugin_name_ + ".inflation_radius", inflation_radius_); + node->declare_parameter(plugin_name_ + ".cost_scaling_factor", cost_scaling_factor_); + node->declare_parameter(plugin_name_ + ".inscribed_radius", inscribed_radius_); + } node->get_parameter(plugin_name_ + ".inflation_radius", inflation_radius_); node->get_parameter(plugin_name_ + ".cost_scaling_factor", cost_scaling_factor_); diff --git a/maps_managers/easynav_navmap_maps_manager/tests/CMakeLists.txt b/maps_managers/easynav_navmap_maps_manager/tests/CMakeLists.txt index 72c55409..61fb5fa5 100644 --- a/maps_managers/easynav_navmap_maps_manager/tests/CMakeLists.txt +++ b/maps_managers/easynav_navmap_maps_manager/tests/CMakeLists.txt @@ -1,2 +1,5 @@ ament_add_gtest(navmap_mapsmanager_tests navmap_mapsmanager_tests.cpp) target_link_libraries(navmap_mapsmanager_tests ${PROJECT_NAME}) + +ament_add_gtest(navmap_mapsmanager_reconfigure_tests navmap_mapsmanager_reconfigure_tests.cpp) +target_link_libraries(navmap_mapsmanager_reconfigure_tests ${PROJECT_NAME}) diff --git a/maps_managers/easynav_navmap_maps_manager/tests/navmap_mapsmanager_reconfigure_tests.cpp b/maps_managers/easynav_navmap_maps_manager/tests/navmap_mapsmanager_reconfigure_tests.cpp new file mode 100644 index 00000000..0de8f1f8 --- /dev/null +++ b/maps_managers/easynav_navmap_maps_manager/tests/navmap_mapsmanager_reconfigure_tests.cpp @@ -0,0 +1,49 @@ +// Copyright 2025 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_navmap_maps_manager/NavMapMapsManager.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +#include "gtest/gtest.h" + +class NavMapMapsManagerReconfigureTest : public ::testing::Test +{ +protected: + void SetUp() override + { + if (!rclcpp::ok()) { + rclcpp::init(0, nullptr); + } + } +}; + +TEST_F(NavMapMapsManagerReconfigureTest, InitializeTwiceOnSameNodeDoesNotThrow) +{ + auto node = rclcpp_lifecycle::LifecycleNode::make_shared( + "navmap_maps_manager_reconfigure_test"); + + auto plugin1 = std::make_shared(); + ASSERT_NO_THROW(plugin1->initialize(node, "test_maps_manager")); + + auto plugin2 = std::make_shared(); + ASSERT_NO_THROW(plugin2->initialize(node, "test_maps_manager")); +} diff --git a/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/filters/InflationFilter.cpp b/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/filters/InflationFilter.cpp index 898083b9..e85d8b80 100644 --- a/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/filters/InflationFilter.cpp +++ b/maps_managers/easynav_octomap_maps_manager/src/easynav_octomap_maps_manager/filters/InflationFilter.cpp @@ -186,9 +186,11 @@ InflationFilter::on_initialize() cost_scaling_factor_ = 3.0; inscribed_radius_ = 0.3; - node->declare_parameter(plugin_name_ + ".inflation_radius", inflation_radius_); - node->declare_parameter(plugin_name_ + ".cost_scaling_factor", cost_scaling_factor_); - node->declare_parameter(plugin_name_ + ".inscribed_radius", inscribed_radius_); + if (!node->has_parameter(plugin_name_ + ".inflation_radius")) { + node->declare_parameter(plugin_name_ + ".inflation_radius", inflation_radius_); + node->declare_parameter(plugin_name_ + ".cost_scaling_factor", cost_scaling_factor_); + node->declare_parameter(plugin_name_ + ".inscribed_radius", inscribed_radius_); + } node->get_parameter(plugin_name_ + ".inflation_radius", inflation_radius_); node->get_parameter(plugin_name_ + ".cost_scaling_factor", cost_scaling_factor_); node->get_parameter(plugin_name_ + ".inscribed_radius", inscribed_radius_); diff --git a/maps_managers/easynav_routes_maps_manager/tests/CMakeLists.txt b/maps_managers/easynav_routes_maps_manager/tests/CMakeLists.txt index 3638b4d4..3265251d 100644 --- a/maps_managers/easynav_routes_maps_manager/tests/CMakeLists.txt +++ b/maps_managers/easynav_routes_maps_manager/tests/CMakeLists.txt @@ -17,3 +17,11 @@ target_link_libraries(routes_costmap_filter_tests rclcpp::rclcpp rclcpp_lifecycle::rclcpp_lifecycle) +ament_add_gtest(routes_mapsmanager_reconfigure_tests + routes_mapsmanager_reconfigure_tests.cpp) +target_link_libraries(routes_mapsmanager_reconfigure_tests + ${PROJECT_NAME} + easynav_common::easynav_common + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle) + diff --git a/maps_managers/easynav_routes_maps_manager/tests/routes_mapsmanager_reconfigure_tests.cpp b/maps_managers/easynav_routes_maps_manager/tests/routes_mapsmanager_reconfigure_tests.cpp new file mode 100644 index 00000000..abbf2c4f --- /dev/null +++ b/maps_managers/easynav_routes_maps_manager/tests/routes_mapsmanager_reconfigure_tests.cpp @@ -0,0 +1,49 @@ +// Copyright 2025 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_routes_maps_manager/RoutesMapsManager.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +#include "gtest/gtest.h" + +class RoutesMapsManagerReconfigureTest : public ::testing::Test +{ +protected: + void SetUp() override + { + if (!rclcpp::ok()) { + rclcpp::init(0, nullptr); + } + } +}; + +TEST_F(RoutesMapsManagerReconfigureTest, InitializeTwiceOnSameNodeDoesNotThrow) +{ + auto node = rclcpp_lifecycle::LifecycleNode::make_shared( + "routes_maps_manager_reconfigure_test"); + + auto plugin1 = std::make_shared(); + ASSERT_NO_THROW(plugin1->initialize(node, "test_maps_manager")); + + auto plugin2 = std::make_shared(); + ASSERT_NO_THROW(plugin2->initialize(node, "test_maps_manager")); +} diff --git a/maps_managers/easynav_simple_maps_manager/src/easynav_simple_maps_manager/SimpleMapsManager.cpp b/maps_managers/easynav_simple_maps_manager/src/easynav_simple_maps_manager/SimpleMapsManager.cpp index 316019a3..0970a302 100644 --- a/maps_managers/easynav_simple_maps_manager/src/easynav_simple_maps_manager/SimpleMapsManager.cpp +++ b/maps_managers/easynav_simple_maps_manager/src/easynav_simple_maps_manager/SimpleMapsManager.cpp @@ -52,8 +52,10 @@ SimpleMapsManager::on_initialize() const auto & plugin_name = get_plugin_name(); std::string package_name, map_path_file; - node->declare_parameter(plugin_name + ".package", package_name); - node->declare_parameter(plugin_name + ".map_path_file", map_path_file); + if (!node->has_parameter(plugin_name + ".package")) { + node->declare_parameter(plugin_name + ".package", package_name); + node->declare_parameter(plugin_name + ".map_path_file", map_path_file); + } node->get_parameter(plugin_name + ".package", package_name); node->get_parameter(plugin_name + ".map_path_file", map_path_file); diff --git a/maps_managers/easynav_simple_maps_manager/tests/CMakeLists.txt b/maps_managers/easynav_simple_maps_manager/tests/CMakeLists.txt index 003f5ef5..8ff957be 100644 --- a/maps_managers/easynav_simple_maps_manager/tests/CMakeLists.txt +++ b/maps_managers/easynav_simple_maps_manager/tests/CMakeLists.txt @@ -1,2 +1,5 @@ ament_add_gtest(simple_mapsmanager_tests simple_mapsmanager_tests.cpp) target_link_libraries(simple_mapsmanager_tests ${PROJECT_NAME}) + +ament_add_gtest(simple_mapsmanager_reconfigure_tests simple_mapsmanager_reconfigure_tests.cpp) +target_link_libraries(simple_mapsmanager_reconfigure_tests ${PROJECT_NAME}) diff --git a/maps_managers/easynav_simple_maps_manager/tests/simple_mapsmanager_reconfigure_tests.cpp b/maps_managers/easynav_simple_maps_manager/tests/simple_mapsmanager_reconfigure_tests.cpp new file mode 100644 index 00000000..290171f1 --- /dev/null +++ b/maps_managers/easynav_simple_maps_manager/tests/simple_mapsmanager_reconfigure_tests.cpp @@ -0,0 +1,49 @@ +// Copyright 2025 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_simple_maps_manager/SimpleMapsManager.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +#include "gtest/gtest.h" + +class SimpleMapsManagerReconfigureTest : public ::testing::Test +{ +protected: + void SetUp() override + { + if (!rclcpp::ok()) { + rclcpp::init(0, nullptr); + } + } +}; + +TEST_F(SimpleMapsManagerReconfigureTest, InitializeTwiceOnSameNodeDoesNotThrow) +{ + auto node = rclcpp_lifecycle::LifecycleNode::make_shared( + "simple_maps_manager_reconfigure_test"); + + auto plugin1 = std::make_shared(); + ASSERT_NO_THROW(plugin1->initialize(node, "test_maps_manager")); + + auto plugin2 = std::make_shared(); + ASSERT_NO_THROW(plugin2->initialize(node, "test_maps_manager")); +} diff --git a/planners/easynav_costmap_planner/CMakeLists.txt b/planners/easynav_costmap_planner/CMakeLists.txt index 161e69ac..e6baa798 100644 --- a/planners/easynav_costmap_planner/CMakeLists.txt +++ b/planners/easynav_costmap_planner/CMakeLists.txt @@ -52,7 +52,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}") diff --git a/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp b/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp index 842b0864..eb15385f 100644 --- a/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp +++ b/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp @@ -124,10 +124,12 @@ void CostmapPlanner::on_initialize() { auto node = get_node(); const auto & plugin_name = get_plugin_name(); - node->declare_parameter(plugin_name + ".cost_factor", 2.0); - node->declare_parameter(plugin_name + ".inflation_penalty", 5.0); - node->declare_parameter(plugin_name + ".heuristic_scale", 1.0); - node->declare_parameter(plugin_name + ".continuous_replan", true); + if (!node->has_parameter(plugin_name + ".cost_factor")) { + node->declare_parameter(plugin_name + ".cost_factor", 2.0); + node->declare_parameter(plugin_name + ".inflation_penalty", 5.0); + node->declare_parameter(plugin_name + ".heuristic_scale", 1.0); + node->declare_parameter(plugin_name + ".continuous_replan", true); + } node->get_parameter(plugin_name + ".cost_factor", cost_factor_); node->get_parameter(plugin_name + ".inflation_penalty", inflation_penalty_); diff --git a/planners/easynav_costmap_planner/tests/CMakeLists.txt b/planners/easynav_costmap_planner/tests/CMakeLists.txt new file mode 100644 index 00000000..2cb334bd --- /dev/null +++ b/planners/easynav_costmap_planner/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(costmap_planner_reconfigure_tests costmap_planner_reconfigure_tests.cpp) + +target_link_libraries(costmap_planner_reconfigure_tests + ${PROJECT_NAME} + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle +) diff --git a/planners/easynav_costmap_planner/tests/costmap_planner_reconfigure_tests.cpp b/planners/easynav_costmap_planner/tests/costmap_planner_reconfigure_tests.cpp new file mode 100644 index 00000000..54dc3639 --- /dev/null +++ b/planners/easynav_costmap_planner/tests/costmap_planner_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_costmap_planner/CostmapPlanner.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +#include "gtest/gtest.h" + +class CostmapPlannerReconfigureTest : public ::testing::Test +{ +protected: + void SetUp() override + { + if (!rclcpp::ok()) { + rclcpp::init(0, nullptr); + } + } +}; + +TEST_F(CostmapPlannerReconfigureTest, InitializeTwiceOnSameNodeDoesNotThrow) +{ + auto node = rclcpp_lifecycle::LifecycleNode::make_shared("costmap_planner_reconfigure_test"); + + auto plugin1 = std::make_shared(); + ASSERT_NO_THROW(plugin1->initialize(node, "test_planner")); + + auto plugin2 = std::make_shared(); + ASSERT_NO_THROW(plugin2->initialize(node, "test_planner")); +} diff --git a/planners/easynav_navmap_planner/CMakeLists.txt b/planners/easynav_navmap_planner/CMakeLists.txt index b66a00a4..4fbc7b99 100644 --- a/planners/easynav_navmap_planner/CMakeLists.txt +++ b/planners/easynav_navmap_planner/CMakeLists.txt @@ -58,7 +58,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}") diff --git a/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp b/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp index 845e1b54..606138a4 100644 --- a/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp +++ b/planners/easynav_navmap_planner/src/easynav_navmap_planner/AStarPlanner.cpp @@ -64,8 +64,10 @@ void AStarPlanner::on_initialize() auto node = get_node(); const auto & plugin_name = get_plugin_name(); - node->declare_parameter(plugin_name + ".cost_factor", 2.0); - node->declare_parameter(plugin_name + ".continuous_replan", true); + if (!node->has_parameter(plugin_name + ".cost_factor")) { + node->declare_parameter(plugin_name + ".cost_factor", 2.0); + node->declare_parameter(plugin_name + ".continuous_replan", true); + } node->get_parameter(plugin_name + ".cost_factor", cost_factor_); node->get_parameter(plugin_name + ".continuous_replan", continuous_replan_); diff --git a/planners/easynav_navmap_planner/tests/CMakeLists.txt b/planners/easynav_navmap_planner/tests/CMakeLists.txt new file mode 100644 index 00000000..5ebcce7f --- /dev/null +++ b/planners/easynav_navmap_planner/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(navmap_planner_reconfigure_tests navmap_planner_reconfigure_tests.cpp) + +target_link_libraries(navmap_planner_reconfigure_tests + ${PROJECT_NAME} + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle +) diff --git a/planners/easynav_navmap_planner/tests/navmap_planner_reconfigure_tests.cpp b/planners/easynav_navmap_planner/tests/navmap_planner_reconfigure_tests.cpp new file mode 100644 index 00000000..ec84bc0f --- /dev/null +++ b/planners/easynav_navmap_planner/tests/navmap_planner_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_navmap_planner/AStarPlanner.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +#include "gtest/gtest.h" + +class AStarPlannerReconfigureTest : public ::testing::Test +{ +protected: + void SetUp() override + { + if (!rclcpp::ok()) { + rclcpp::init(0, nullptr); + } + } +}; + +TEST_F(AStarPlannerReconfigureTest, InitializeTwiceOnSameNodeDoesNotThrow) +{ + auto node = rclcpp_lifecycle::LifecycleNode::make_shared("navmap_planner_reconfigure_test"); + + auto plugin1 = std::make_shared(); + ASSERT_NO_THROW(plugin1->initialize(node, "test_planner")); + + auto plugin2 = std::make_shared(); + ASSERT_NO_THROW(plugin2->initialize(node, "test_planner")); +} diff --git a/planners/easynav_simple_planner/CMakeLists.txt b/planners/easynav_simple_planner/CMakeLists.txt index 19674fe2..1708ab08 100644 --- a/planners/easynav_simple_planner/CMakeLists.txt +++ b/planners/easynav_simple_planner/CMakeLists.txt @@ -52,7 +52,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}") diff --git a/planners/easynav_simple_planner/src/easynav_simple_planner/SimplePlanner.cpp b/planners/easynav_simple_planner/src/easynav_simple_planner/SimplePlanner.cpp index 30093942..376276ba 100644 --- a/planners/easynav_simple_planner/src/easynav_simple_planner/SimplePlanner.cpp +++ b/planners/easynav_simple_planner/src/easynav_simple_planner/SimplePlanner.cpp @@ -89,8 +89,10 @@ SimplePlanner::on_initialize() auto node = get_node(); const auto & plugin_name = get_plugin_name(); - node->declare_parameter(plugin_name + ".robot_radius", 0.3); - node->declare_parameter(plugin_name + ".clearance_distance", 0.2); + if (!node->has_parameter(plugin_name + ".robot_radius")) { + node->declare_parameter(plugin_name + ".robot_radius", 0.3); + node->declare_parameter(plugin_name + ".clearance_distance", 0.2); + } node->get_parameter(plugin_name + ".robot_radius", robot_radius_); node->get_parameter(plugin_name + ".clearance_distance", clearance_distance_); diff --git a/planners/easynav_simple_planner/tests/CMakeLists.txt b/planners/easynav_simple_planner/tests/CMakeLists.txt new file mode 100644 index 00000000..a395babd --- /dev/null +++ b/planners/easynav_simple_planner/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(simple_planner_reconfigure_tests simple_planner_reconfigure_tests.cpp) + +target_link_libraries(simple_planner_reconfigure_tests + ${PROJECT_NAME} + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle +) diff --git a/planners/easynav_simple_planner/tests/simple_planner_reconfigure_tests.cpp b/planners/easynav_simple_planner/tests/simple_planner_reconfigure_tests.cpp new file mode 100644 index 00000000..8db66091 --- /dev/null +++ b/planners/easynav_simple_planner/tests/simple_planner_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_simple_planner/SimplePlanner.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +#include "gtest/gtest.h" + +class SimplePlannerReconfigureTest : public ::testing::Test +{ +protected: + void SetUp() override + { + if (!rclcpp::ok()) { + rclcpp::init(0, nullptr); + } + } +}; + +TEST_F(SimplePlannerReconfigureTest, InitializeTwiceOnSameNodeDoesNotThrow) +{ + auto node = rclcpp_lifecycle::LifecycleNode::make_shared("simple_planner_reconfigure_test"); + + auto plugin1 = std::make_shared(); + ASSERT_NO_THROW(plugin1->initialize(node, "test_planner")); + + auto plugin2 = std::make_shared(); + ASSERT_NO_THROW(plugin2->initialize(node, "test_planner")); +}