[costmap2d] make robot_base_frame param dynamic (#2782)

* make costmap2d robot_base_frame_param dynamic

* add test

* add rejecting logic and test

Author: doisyg

nav2_costmap_2d/src/costmap_2d_ros.cpp

@@ -650,6 +650,26 @@ Costmap2DROS::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameter
         if (makeFootprintFromString(footprint_, new_footprint)) {
           setRobotFootprint(new_footprint);
         }
+      } else if (name == "robot_base_frame") {
+        // First, make sure that the transform between the robot base frame
+        // and the global frame is available
+        std::string tf_error;
+        RCLCPP_INFO(get_logger(), "Checking transform");
+        if (!tf_buffer_->canTransform(
+            global_frame_, parameter.as_string(), tf2::TimePointZero,
+            tf2::durationFromSec(1.0), &tf_error))
+        {
+          RCLCPP_WARN(
+            get_logger(), "Timed out waiting for transform from %s to %s"
+            " to become available, tf error: %s",
+            parameter.as_string().c_str(), global_frame_.c_str(), tf_error.c_str());
+          RCLCPP_WARN(
+            get_logger(), "Rejecting robot_base_frame change to %s , leaving it to its original"
+            " value of %s", parameter.as_string().c_str(), robot_base_frame_.c_str());
+          result.successful = false;
+          return result;
+        }
+        robot_base_frame_ = parameter.as_string();
       }
     }
   }

nav2_costmap_2d/test/integration/dyn_params_tests.cpp

@@ -50,14 +50,17 @@ TEST(DynParamTestNode, testDynParamsSet)
   t.header.frame_id = "map";
   t.child_frame_id = "base_link";
   tf_broadcaster_->sendTransform(t);
+  t.header.frame_id = "map";
+  t.child_frame_id = "test_frame";
+  tf_broadcaster_->sendTransform(t);
 
   costmap->on_activate(rclcpp_lifecycle::State());
 
   auto parameter_client = std::make_shared<rclcpp::AsyncParametersClient>(
     node->shared_from_this(),
     "/test_costmap/test_costmap",
     rmw_qos_profile_parameters);
-  auto results = parameter_client->set_parameters_atomically(
+  auto results1 = parameter_client->set_parameters_atomically(
   {
     rclcpp::Parameter("robot_radius", 1.234),
     rclcpp::Parameter("footprint_padding", 2.345),
@@ -71,6 +74,13 @@ TEST(DynParamTestNode, testDynParamsSet)
     rclcpp::Parameter(
       "footprint",
       "[[-0.325, -0.325], [-0.325, 0.325], [0.325, 0.325], [0.46, 0.0], [0.325, -0.325]]"),
+    rclcpp::Parameter("robot_base_frame", "test_frame"),
+  });
+
+  // Try setting robot_base_frame to an invalid frame, should be rejected
+  auto results2 = parameter_client->set_parameters_atomically(
+  {
+    rclcpp::Parameter("robot_base_frame", "wrong_test_frame"),
   });
 
   rclcpp::spin_some(costmap->get_node_base_interface());
@@ -87,6 +97,7 @@ TEST(DynParamTestNode, testDynParamsSet)
   EXPECT_EQ(
     costmap->get_parameter("footprint").as_string(),
     "[[-0.325, -0.325], [-0.325, 0.325], [0.325, 0.325], [0.46, 0.0], [0.325, -0.325]]");
+  EXPECT_EQ(costmap->get_parameter("robot_base_frame").as_string(), "test_frame");
 
   costmap->on_deactivate(rclcpp_lifecycle::State());
   costmap->on_cleanup(rclcpp_lifecycle::State());