Add tf_frame_prefix parameters to mecanum_drive_controller (backport #1680) (#1810)

Co-authored-by: Dawid Kmak <73443304+KmakD@users.noreply.github.com>

Author: mergify[bot]

mecanum_drive_controller/src/mecanum_drive_controller.cpp

@@ -148,10 +148,31 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure(
     return controller_interface::CallbackReturn::ERROR;
   }
 
+  // Append the tf prefix if there is one
+  std::string tf_prefix = "";
+  if (params_.tf_frame_prefix_enable)
+  {
+    tf_prefix = params_.tf_frame_prefix != "" ? params_.tf_frame_prefix
+                                              : std::string(get_node()->get_namespace());
+
+    // Make sure prefix does not start with '/' and always ends with '/'
+    if (tf_prefix.back() != '/')
+    {
+      tf_prefix = tf_prefix + "/";
+    }
+    if (tf_prefix.front() == '/')
+    {
+      tf_prefix.erase(0, 1);
+    }
+  }
+
+  const auto odom_frame_id = tf_prefix + params_.odom_frame_id;
+  const auto base_frame_id = tf_prefix + params_.base_frame_id;
+
   rt_odom_state_publisher_->lock();
   rt_odom_state_publisher_->msg_.header.stamp = get_node()->now();
-  rt_odom_state_publisher_->msg_.header.frame_id = params_.odom_frame_id;
-  rt_odom_state_publisher_->msg_.child_frame_id = params_.base_frame_id;
+  rt_odom_state_publisher_->msg_.header.frame_id = odom_frame_id;
+  rt_odom_state_publisher_->msg_.child_frame_id = base_frame_id;
   rt_odom_state_publisher_->msg_.pose.pose.position.z = 0;
 
   auto & covariance = rt_odom_state_publisher_->msg_.twist.covariance;
@@ -182,8 +203,8 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure(
   rt_tf_odom_state_publisher_->lock();
   rt_tf_odom_state_publisher_->msg_.transforms.resize(1);
   rt_tf_odom_state_publisher_->msg_.transforms[0].header.stamp = get_node()->now();
-  rt_tf_odom_state_publisher_->msg_.transforms[0].header.frame_id = params_.odom_frame_id;
-  rt_tf_odom_state_publisher_->msg_.transforms[0].child_frame_id = params_.base_frame_id;
+  rt_tf_odom_state_publisher_->msg_.transforms[0].header.frame_id = odom_frame_id;
+  rt_tf_odom_state_publisher_->msg_.transforms[0].child_frame_id = base_frame_id;
   rt_tf_odom_state_publisher_->msg_.transforms[0].transform.translation.z = 0.0;
   rt_tf_odom_state_publisher_->unlock();
 
@@ -207,7 +228,7 @@ controller_interface::CallbackReturn MecanumDriveController::on_configure(
 
   controller_state_publisher_->lock();
   controller_state_publisher_->msg_.header.stamp = get_node()->now();
-  controller_state_publisher_->msg_.header.frame_id = params_.odom_frame_id;
+  controller_state_publisher_->msg_.header.frame_id = odom_frame_id;
   controller_state_publisher_->unlock();
 
   RCLCPP_INFO(get_node()->get_logger(), "MecanumDriveController configured successfully");

mecanum_drive_controller/src/mecanum_drive_controller.yaml

@@ -113,12 +113,21 @@ mecanum_drive_controller:
       read_only: false,
     }
 
+  tf_frame_prefix_enable: {
+    type: bool,
+    default_value: true,
+    description:  "Enables or disables appending ``tf_frame_prefix`` to tf frame id's. See its parameter description for more information.",
+  }
+  tf_frame_prefix: {
+    type: string,
+    default_value: "",
+    description:  "(optional) Prefix to be appended to the tf frames, will be added to odom_frame_id and base_frame_id before publishing. If the parameter is empty, controller's namespace will be used.",
+  }
   base_frame_id: {
     type: string,
     default_value: "base_link",
     description: "Base frame_id set to value of base_frame_id.",
     read_only: false,
-
   }
   odom_frame_id: {
     type: string,

mecanum_drive_controller/test/mecanum_drive_controller_params.yaml

@@ -1,40 +1,41 @@
-test_mecanum_drive_controller:
-  ros__parameters:
-    reference_timeout: 0.9
-
-    front_left_wheel_command_joint_name: "front_left_wheel_joint"
-    front_right_wheel_command_joint_name: "front_right_wheel_joint"
-    rear_right_wheel_command_joint_name: "back_right_wheel_joint"
-    rear_left_wheel_command_joint_name: "back_left_wheel_joint"
-
-    kinematics:
-      base_frame_offset: { x: 0.0, y: 0.0, theta: 0.0 }
-      wheels_radius: 0.5
-      sum_of_robot_center_projection_on_X_Y_axis: 1.0
-
-    base_frame_id: "base_link"
-    odom_frame_id: "odom"
-    enable_odom_tf: true
-    twist_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0]
-    pose_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0]
-
-
-test_mecanum_drive_controller_with_rotation:
-  ros__parameters:
-    reference_timeout: 5.0
-
-    front_left_wheel_command_joint_name: "front_left_wheel_joint"
-    front_right_wheel_command_joint_name: "front_right_wheel_joint"
-    rear_right_wheel_command_joint_name: "rear_right_wheel_joint"
-    rear_left_wheel_command_joint_name: "rear_left_wheel_joint"
-
-    kinematics:
-      base_frame_offset: { x: 1.0, y: 2.0, theta: 3.0 }
-      wheels_radius: 0.05
-      sum_of_robot_center_projection_on_X_Y_axis: 0.2925
-
-    base_frame_id: "base_link"
-    odom_frame_id: "odom"
-    enable_odom_tf: true
-    pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.001]
-    twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.001]
+/**:
+  test_mecanum_drive_controller:
+    ros__parameters:
+      reference_timeout: 0.9
+
+      front_left_wheel_command_joint_name: "front_left_wheel_joint"
+      front_right_wheel_command_joint_name: "front_right_wheel_joint"
+      rear_right_wheel_command_joint_name: "back_right_wheel_joint"
+      rear_left_wheel_command_joint_name: "back_left_wheel_joint"
+
+      kinematics:
+        base_frame_offset: { x: 0.0, y: 0.0, theta: 0.0 }
+        wheels_radius: 0.5
+        sum_of_robot_center_projection_on_X_Y_axis: 1.0
+
+      base_frame_id: "base_link"
+      odom_frame_id: "odom"
+      enable_odom_tf: true
+      twist_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0]
+      pose_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0]
+
+
+  test_mecanum_drive_controller_with_rotation:
+    ros__parameters:
+      reference_timeout: 5.0
+
+      front_left_wheel_command_joint_name: "front_left_wheel_joint"
+      front_right_wheel_command_joint_name: "front_right_wheel_joint"
+      rear_right_wheel_command_joint_name: "rear_right_wheel_joint"
+      rear_left_wheel_command_joint_name: "rear_left_wheel_joint"
+
+      kinematics:
+        base_frame_offset: { x: 1.0, y: 2.0, theta: 3.0 }
+        wheels_radius: 0.05
+        sum_of_robot_center_projection_on_X_Y_axis: 0.2925
+
+      base_frame_id: "base_link"
+      odom_frame_id: "odom"
+      enable_odom_tf: true
+      pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.001]
+      twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.001]

mecanum_drive_controller/test/mecanum_drive_controller_preceding_params.yaml

@@ -1,24 +1,25 @@
-test_mecanum_drive_controller:
-  ros__parameters:
-    reference_timeout: 0.1
+/**:
+  test_mecanum_drive_controller:
+    ros__parameters:
+      reference_timeout: 0.1
 
-    front_left_wheel_command_joint_name: "front_left_wheel_joint"
-    front_right_wheel_command_joint_name: "front_right_wheel_joint"
-    rear_right_wheel_command_joint_name: "back_right_wheel_joint"
-    rear_left_wheel_command_joint_name: "back_left_wheel_joint"
+      front_left_wheel_command_joint_name: "front_left_wheel_joint"
+      front_right_wheel_command_joint_name: "front_right_wheel_joint"
+      rear_right_wheel_command_joint_name: "back_right_wheel_joint"
+      rear_left_wheel_command_joint_name: "back_left_wheel_joint"
 
-    front_left_wheel_state_joint_name: "state_front_left_wheel_joint"
-    front_right_wheel_state_joint_name: "state_front_right_wheel_joint"
-    rear_right_wheel_state_joint_name: "state_back_right_wheel_joint"
-    rear_left_wheel_state_joint_name: "state_back_left_wheel_joint"
+      front_left_wheel_state_joint_name: "state_front_left_wheel_joint"
+      front_right_wheel_state_joint_name: "state_front_right_wheel_joint"
+      rear_right_wheel_state_joint_name: "state_back_right_wheel_joint"
+      rear_left_wheel_state_joint_name: "state_back_left_wheel_joint"
 
-    kinematics:
-      base_frame_offset: { x: 0.0, y: 0.0, theta: 0.0 }
-      wheels_radius: 0.5
-      sum_of_robot_center_projection_on_X_Y_axis: 1.0
+      kinematics:
+        base_frame_offset: { x: 0.0, y: 0.0, theta: 0.0 }
+        wheels_radius: 0.5
+        sum_of_robot_center_projection_on_X_Y_axis: 1.0
 
-    base_frame_id: "base_link"
-    odom_frame_id: "odom"
-    enable_odom_tf: true
-    twist_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0]
-    pose_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0]
+      base_frame_id: "base_link"
+      odom_frame_id: "odom"
+      enable_odom_tf: true
+      twist_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0]
+      pose_covariance_diagonal: [0.0, 7.0, 14.0, 21.0, 28.0, 35.0]

mecanum_drive_controller/test/test_mecanum_drive_controller.cpp

@@ -118,6 +118,166 @@ TEST_F(MecanumDriveControllerTest, when_controller_configured_expect_properly_ex
   }
 }
 
+TEST_F(MecanumDriveControllerTest, configure_succeeds_tf_test_prefix_false_no_namespace)
+{
+  std::string odom_id = "odom";
+  std::string base_link_id = "base_link";
+  std::string frame_prefix = "test_prefix";
+
+  auto node_options = controller_->define_custom_node_options();
+  node_options.append_parameter_override("tf_frame_prefix_enable", rclcpp::ParameterValue(false));
+  node_options.append_parameter_override("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix));
+  node_options.append_parameter_override("odom_frame_id", rclcpp::ParameterValue(odom_id));
+  node_options.append_parameter_override("base_frame_id", rclcpp::ParameterValue(base_link_id));
+
+  SetUpController("test_mecanum_drive_controller", node_options);
+
+  ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
+
+  auto odometry_message = controller_->get_rt_odom_state_publisher_msg();
+  std::string test_odom_frame_id = odometry_message.header.frame_id;
+  std::string test_base_frame_id = odometry_message.child_frame_id;
+  /* tf_frame_prefix_enable is false so no modifications to the frame id's */
+  ASSERT_EQ(test_odom_frame_id, odom_id);
+  ASSERT_EQ(test_base_frame_id, base_link_id);
+}
+
+TEST_F(MecanumDriveControllerTest, configure_succeeds_tf_test_prefix_true_no_namespace)
+{
+  std::string odom_id = "odom";
+  std::string base_link_id = "base_link";
+  std::string frame_prefix = "test_prefix";
+
+  auto node_options = controller_->define_custom_node_options();
+  node_options.append_parameter_override("tf_frame_prefix_enable", rclcpp::ParameterValue(true));
+  node_options.append_parameter_override("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix));
+  node_options.append_parameter_override("odom_frame_id", rclcpp::ParameterValue(odom_id));
+  node_options.append_parameter_override("base_frame_id", rclcpp::ParameterValue(base_link_id));
+
+  SetUpController("test_mecanum_drive_controller", node_options);
+
+  ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
+
+  auto odometry_message = controller_->get_rt_odom_state_publisher_msg();
+  std::string test_odom_frame_id = odometry_message.header.frame_id;
+  std::string test_base_frame_id = odometry_message.child_frame_id;
+
+  /* tf_frame_prefix_enable is true and frame_prefix is not blank so should be appended to the
+  frame
+   * id's */
+  ASSERT_EQ(test_odom_frame_id, frame_prefix + "/" + odom_id);
+  ASSERT_EQ(test_base_frame_id, frame_prefix + "/" + base_link_id);
+}
+
+TEST_F(MecanumDriveControllerTest, configure_succeeds_tf_blank_prefix_true_no_namespace)
+{
+  std::string odom_id = "odom";
+  std::string base_link_id = "base_link";
+  std::string frame_prefix = "";
+
+  auto node_options = controller_->define_custom_node_options();
+  node_options.append_parameter_override("tf_frame_prefix_enable", rclcpp::ParameterValue(true));
+  node_options.append_parameter_override("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix));
+  node_options.append_parameter_override("odom_frame_id", rclcpp::ParameterValue(odom_id));
+  node_options.append_parameter_override("base_frame_id", rclcpp::ParameterValue(base_link_id));
+
+  SetUpController("test_mecanum_drive_controller", node_options);
+
+  ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
+
+  auto odometry_message = controller_->get_rt_odom_state_publisher_msg();
+  std::string test_odom_frame_id = odometry_message.header.frame_id;
+  std::string test_base_frame_id = odometry_message.child_frame_id;
+  /* tf_frame_prefix_enable is true but frame_prefix is blank so should not be appended to the
+  frame
+   * id's */
+  ASSERT_EQ(test_odom_frame_id, odom_id);
+  ASSERT_EQ(test_base_frame_id, base_link_id);
+}
+
+TEST_F(MecanumDriveControllerTest, configure_succeeds_tf_test_prefix_false_set_namespace)
+{
+  std::string test_namespace = "/test_namespace";
+
+  std::string odom_id = "odom";
+  std::string base_link_id = "base_link";
+  std::string frame_prefix = "test_prefix";
+
+  auto node_options = controller_->define_custom_node_options();
+  node_options.append_parameter_override("tf_frame_prefix_enable", rclcpp::ParameterValue(false));
+  node_options.append_parameter_override("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix));
+  node_options.append_parameter_override("odom_frame_id", rclcpp::ParameterValue(odom_id));
+  node_options.append_parameter_override("base_frame_id", rclcpp::ParameterValue(base_link_id));
+
+  SetUpController("test_mecanum_drive_controller", node_options, test_namespace);
+
+  ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
+
+  auto odometry_message = controller_->get_rt_odom_state_publisher_msg();
+  std::string test_odom_frame_id = odometry_message.header.frame_id;
+  std::string test_base_frame_id = odometry_message.child_frame_id;
+  /* tf_frame_prefix_enable is false so no modifications to the frame id's */
+  ASSERT_EQ(test_odom_frame_id, odom_id);
+  ASSERT_EQ(test_base_frame_id, base_link_id);
+}
+
+TEST_F(MecanumDriveControllerTest, configure_succeeds_tf_test_prefix_true_set_namespace)
+{
+  std::string test_namespace = "/test_namespace";
+
+  std::string odom_id = "odom";
+  std::string base_link_id = "base_link";
+  std::string frame_prefix = "test_prefix";
+
+  auto node_options = controller_->define_custom_node_options();
+  node_options.append_parameter_override("tf_frame_prefix_enable", rclcpp::ParameterValue(true));
+  node_options.append_parameter_override("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix));
+  node_options.append_parameter_override("odom_frame_id", rclcpp::ParameterValue(odom_id));
+  node_options.append_parameter_override("base_frame_id", rclcpp::ParameterValue(base_link_id));
+
+  SetUpController("test_mecanum_drive_controller", node_options, test_namespace);
+
+  ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
+
+  auto odometry_message = controller_->get_rt_odom_state_publisher_msg();
+  std::string test_odom_frame_id = odometry_message.header.frame_id;
+  std::string test_base_frame_id = odometry_message.child_frame_id;
+
+  /* tf_frame_prefix_enable is true and frame_prefix is not blank so should be appended to the
+  frame
+   * id's instead of the namespace*/
+  ASSERT_EQ(test_odom_frame_id, frame_prefix + "/" + odom_id);
+  ASSERT_EQ(test_base_frame_id, frame_prefix + "/" + base_link_id);
+}
+
+TEST_F(MecanumDriveControllerTest, configure_succeeds_tf_blank_prefix_true_set_namespace)
+{
+  std::string test_namespace = "/test_namespace";
+  std::string odom_id = "odom";
+  std::string base_link_id = "base_link";
+  std::string frame_prefix = "";
+
+  auto node_options = controller_->define_custom_node_options();
+  node_options.append_parameter_override("tf_frame_prefix_enable", rclcpp::ParameterValue(true));
+  node_options.append_parameter_override("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix));
+  node_options.append_parameter_override("odom_frame_id", rclcpp::ParameterValue(odom_id));
+  node_options.append_parameter_override("base_frame_id", rclcpp::ParameterValue(base_link_id));
+
+  SetUpController("test_mecanum_drive_controller", node_options, test_namespace);
+
+  ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
+
+  auto odometry_message = controller_->get_rt_odom_state_publisher_msg();
+  std::string test_odom_frame_id = odometry_message.header.frame_id;
+  std::string test_base_frame_id = odometry_message.child_frame_id;
+  std::string ns_prefix = test_namespace.erase(0, 1) + "/";
+  /* tf_frame_prefix_enable is true but frame_prefix is blank so namespace should be appended to
+  the
+   * frame id's */
+  ASSERT_EQ(test_odom_frame_id, ns_prefix + odom_id);
+  ASSERT_EQ(test_base_frame_id, ns_prefix + base_link_id);
+}
+
 TEST_F(MecanumDriveControllerTest, when_controller_is_activated_expect_reference_reset)
 {
   SetUpController();