allowing costmap to change with new costmap resolutions (#4223)

* allowing costmap to change with new costmap resolutions

* add test

* fix test

Author: SteveMacenski

nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp

@@ -133,6 +133,8 @@ class SmacPlannerHybrid : public nav2_core::GlobalPlanner
 
   // Dynamic parameters handler
   rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr _dyn_params_handler;
+  std::shared_ptr<rclcpp::ParameterEventHandler> _remote_param_subscriber;
+  std::shared_ptr<rclcpp::ParameterCallbackHandle> _remote_resolution_handler;
 };
 
 }  // namespace nav2_smac_planner

nav2_smac_planner/src/smac_planner_hybrid.cpp

@@ -286,6 +286,16 @@ void SmacPlannerHybrid::activate()
   // Add callback for dynamic parameters
   _dyn_params_handler = node->add_on_set_parameters_callback(
     std::bind(&SmacPlannerHybrid::dynamicParametersCallback, this, _1));
+
+  // Special case handling to obtain resolution changes in global costmap
+  auto resolution_remote_cb = [this](const rclcpp::Parameter & p) {
+      auto node = _node.lock();
+      dynamicParametersCallback(
+        {rclcpp::Parameter("resolution", rclcpp::ParameterValue(p.as_double()))});
+    };
+  _remote_param_subscriber = std::make_shared<rclcpp::ParameterEventHandler>(_node.lock());
+  _remote_resolution_handler = _remote_param_subscriber->add_parameter_callback(
+    "resolution", resolution_remote_cb, "global_costmap/global_costmap");
 }
 
 void SmacPlannerHybrid::deactivate()
@@ -560,6 +570,14 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector<rclcpp::Parameter> para
       } else if (name == _name + ".analytic_expansion_max_cost") {
         reinit_a_star = true;
         _search_info.analytic_expansion_max_cost = static_cast<float>(parameter.as_double());
+      } else if (name == "resolution") {
+        // Special case: When the costmap's resolution changes, need to reinitialize
+        // the controller to have new resolution information
+        RCLCPP_INFO(_logger, "Costmap resolution changed. Reinitializing SmacPlannerHybrid.");
+        reinit_collision_checker = true;
+        reinit_a_star = true;
+        reinit_downsampler = true;
+        reinit_smoother = true;
       }
     } else if (type == ParameterType::PARAMETER_BOOL) {
       if (name == _name + ".downsample_costmap") {

nav2_smac_planner/test/test_smac_hybrid.cpp

@@ -98,6 +98,8 @@ TEST(SmacTest, test_smac_se2_reconfigure)
   planner->configure(nodeSE2, "test", nullptr, costmap_ros);
   planner->activate();
 
+  nodeSE2->declare_parameter("resolution", 0.05);
+
   auto rec_param = std::make_shared<rclcpp::AsyncParametersClient>(
     nodeSE2->get_node_base_interface(), nodeSE2->get_node_topics_interface(),
     nodeSE2->get_node_graph_interface(),
@@ -153,4 +155,11 @@ TEST(SmacTest, test_smac_se2_reconfigure)
   EXPECT_EQ(
     nodeSE2->get_parameter("test.motion_model_for_search").as_string(),
     std::string("REEDS_SHEPP"));
+
+  auto results2 = rec_param->set_parameters_atomically(
+    {rclcpp::Parameter("resolution", 0.2)});
+  rclcpp::spin_until_future_complete(
+    nodeSE2->get_node_base_interface(),
+    results2);
+  EXPECT_EQ(nodeSE2->get_parameter("resolution").as_double(), 0.2);
 }