tf_prefix param: fix slashes and add to IMU Broadcaster

diff_drive_controller/src/diff_drive_controller.cpp

@@ -361,26 +361,22 @@ controller_interface::CallbackReturn DiffDriveController::on_configure(
 
   // Append the tf prefix if there is one
   std::string tf_prefix = "";
-  if (params_.tf_frame_prefix_enable)
+  if (!params_.tf_frame_prefix.empty())
   {
-    if (params_.tf_frame_prefix != "")
-    {
-      tf_prefix = params_.tf_frame_prefix;
-    }
-    else
-    {
-      tf_prefix = std::string(get_node()->get_namespace());
-    }
-
-    if (tf_prefix == "/")
-    {
-      tf_prefix = "";
-    }
-    else
+    tf_prefix = params_.tf_frame_prefix;
+  }
+  else
+  {
+    tf_prefix = std::string(get_node()->get_namespace());
+    if (tf_prefix != "/")
     {
-      tf_prefix = 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;

diff_drive_controller/src/diff_drive_controller_parameter.yaml

@@ -40,11 +40,6 @@ diff_drive_controller:
     default_value: 1.0,
     description: "Correction factor when radius of right wheels differs from the nominal value in ``wheel_radius`` parameter.",
   }
-  tf_frame_prefix_enable: {
-    type: bool,
-    default_value: true,
-    description:  "Enables or disables appending tf_prefix to tf frame id's.",
-  }
   tf_frame_prefix: {
     type: string,
     default_value: "",

diff_drive_controller/test/test_diff_drive_controller.cpp

@@ -12,6 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include "test_diff_drive_controller.hpp"
+
 #include <gmock/gmock.h>
 
 #include <array>
@@ -253,169 +255,48 @@ TEST_F(TestDiffDriveController, configure_succeeds_when_wheels_are_specified)
   EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL);
 }
 
-TEST_F(TestDiffDriveController, configure_succeeds_tf_test_prefix_false_no_namespace)
+TEST_F(TestDiffDriveController, TfPrefixNamespaceParams)
 {
-  std::string odom_id = "odom";
-  std::string base_link_id = "base_link";
-  std::string frame_prefix = "test_prefix";
-
-  ASSERT_EQ(
-    InitController(
-      left_wheel_names, right_wheel_names,
-      {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(false)),
-       rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)),
-       rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)),
-       rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}),
-    controller_interface::return_type::OK);
-
-  ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
-
-  auto odometry_message = controller_->get_rt_odom_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(TestDiffDriveController, 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";
-
-  ASSERT_EQ(
-    InitController(
-      left_wheel_names, right_wheel_names,
-      {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true)),
-       rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)),
-       rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)),
-       rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}),
-    controller_interface::return_type::OK);
-
-  ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
-
-  auto odometry_message = controller_->get_rt_odom_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(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_no_namespace)
-{
-  std::string odom_id = "odom";
-  std::string base_link_id = "base_link";
-  std::string frame_prefix = "";
-
-  ASSERT_EQ(
-    InitController(
-      left_wheel_names, right_wheel_names,
-      {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true)),
-       rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)),
-       rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)),
-       rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))}),
-    controller_interface::return_type::OK);
-
-  ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
-
-  auto odometry_message = controller_->get_rt_odom_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(TestDiffDriveController, 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";
-
-  ASSERT_EQ(
-    InitController(
-      left_wheel_names, right_wheel_names,
-      {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(false)),
-       rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)),
-       rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)),
-       rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))},
-      test_namespace),
-    controller_interface::return_type::OK);
-
-  ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
-
-  auto odometry_message = controller_->get_rt_odom_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(TestDiffDriveController, 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";
-
-  ASSERT_EQ(
-    InitController(
-      left_wheel_names, right_wheel_names,
-      {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true)),
-       rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)),
-       rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)),
-       rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))},
-      test_namespace),
-    controller_interface::return_type::OK);
-
-  ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
-
-  auto odometry_message = controller_->get_rt_odom_publisher()->msg_;
-  std::string test_odom_frame_id = odometry_message.header.frame_id;
-  std::string test_base_frame_id = odometry_message.child_frame_id;
+  const std::vector<TestPrefixParams> test_prefix_matrix = {
+    {"", "", ""},
+    {"/", "", ""},
+    {"", "/", ""},
+    {"test_prefix", "", "test_prefix"},
+    {"/test_prefix", "", "test_prefix"},
+    {"", "test_namespace", "test_namespace/"},
+    {"", "/test_namespace", "test_namespace/"},
+    {"test_prefix", "test_namespace", "test_prefix"},
+  };
+
+  for (const auto & params : test_prefix_matrix)
+  {
+    const auto ret = controller_->init(controller_name, params.ns);
+    ASSERT_EQ(ret, controller_interface::return_type::OK);
 
-  /* 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);
-}
+    std::string odom_id = "odom";
+    std::string base_link_id = "base_link";
 
-TEST_F(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_set_namespace)
-{
-  std::string test_namespace = "/test_namespace";
+    controller_->get_node()->set_parameter(
+      rclcpp::Parameter("left_wheel_names", rclcpp::ParameterValue(left_wheel_names)));
+    controller_->get_node()->set_parameter(
+      rclcpp::Parameter("right_wheel_names", rclcpp::ParameterValue(right_wheel_names)));
 
-  std::string odom_id = "odom";
-  std::string base_link_id = "base_link";
-  std::string frame_prefix = "";
+    controller_->get_node()->set_parameter(
+      rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(params.tf_prefix)));
+    controller_->get_node()->set_parameter(
+      rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)));
+    controller_->get_node()->set_parameter(
+      rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id)));
 
-  ASSERT_EQ(
-    InitController(
-      left_wheel_names, right_wheel_names,
-      {rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true)),
-       rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_prefix)),
-       rclcpp::Parameter("odom_frame_id", rclcpp::ParameterValue(odom_id)),
-       rclcpp::Parameter("base_frame_id", rclcpp::ParameterValue(base_link_id))},
-      test_namespace),
-    controller_interface::return_type::OK);
+    ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
 
-  ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
+    auto odometry_message = controller_->get_rt_odom_publisher()->msg_;
+    std::string test_odom_frame_id = odometry_message.header.frame_id;
+    std::string test_base_frame_id = odometry_message.child_frame_id;
 
-  auto odometry_message = controller_->get_rt_odom_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 namespace should be appended to the
-   * frame id's */
-  ASSERT_EQ(test_odom_frame_id, test_namespace + "/" + odom_id);
-  ASSERT_EQ(test_base_frame_id, test_namespace + "/" + base_link_id);
+    ASSERT_EQ(test_odom_frame_id, params.result_prefix + odom_id);
+    ASSERT_EQ(test_base_frame_id, params.result_prefix + base_link_id);
+  }
 }
 
 TEST_F(TestDiffDriveController, activate_fails_without_resources_assigned)

diff_drive_controller/test/test_diff_drive_controller.hpp

@@ -0,0 +1,22 @@
+// Copyright 2020 PAL Robotics SL.
+//
+// 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.
+
+#include <string>
+
+struct TestPrefixParams
+{
+  std::string tf_prefix;
+  std::string ns;
+  std::string result_prefix;
+};

imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp

@@ -62,6 +62,24 @@ controller_interface::CallbackReturn IMUSensorBroadcaster::on_configure(
     return CallbackReturn::ERROR;
   }
 
+  std::string tf_prefix = "";
+  if (!params_.tf_frame_prefix.empty())
+  {
+    tf_prefix = params_.tf_frame_prefix;
+  }
+  else
+  {
+    tf_prefix = std::string(get_node()->get_namespace());
+    if (tf_prefix != "/")
+    {
+      tf_prefix += '/';
+    }
+  }
+  if (tf_prefix.front() == '/')
+  {
+    tf_prefix.erase(0, 1);
+  }
+
   realtime_publisher_->lock();
   realtime_publisher_->msg_.header.frame_id = params_.frame_id;
   // convert double vector to fixed-size array in the message

imu_sensor_broadcaster/src/imu_sensor_broadcaster_parameters.yaml

@@ -41,3 +41,8 @@ imu_sensor_broadcaster:
       fixed_size<>: [9],
     }
   }
+  tf_frame_prefix: {
+    type: string,
+    default_value: "",
+    description:  "(optional) Prefix to be appended to the tf frames, will be added to the sensor's frame_id before publishing. If the parameter is empty, controller's namespace will be used.",
+  }

imu_sensor_broadcaster/test/imu_sensor_broadcaster_params.yaml

@@ -1,5 +1,6 @@
-test_imu_sensor_broadcaster:
-  ros__parameters:
+/**:
+  test_imu_sensor_broadcaster:
+    ros__parameters:
 
-    sensor_name: "imu_sensor"
-    frame_id:  "imu_sensor_frame"
+      sensor_name: "imu_sensor"
+      frame_id:  "imu_sensor_frame"

imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.cpp

@@ -74,7 +74,8 @@ void IMUSensorBroadcasterTest::SetUpIMUBroadcaster()
   imu_broadcaster_->assign_interfaces({}, std::move(state_ifs));
 }
 
-void IMUSensorBroadcasterTest::subscribe_and_get_message(sensor_msgs::msg::Imu & imu_msg)
+void IMUSensorBroadcasterTest::subscribe_and_get_message(
+  sensor_msgs::msg::Imu & imu_msg, const std::string & ns)
 {
   // create a new subscriber
   rclcpp::Node test_subscription_node("test_subscription_node");
@@ -208,6 +209,39 @@ TEST_F(IMUSensorBroadcasterTest, SensorName_Publish_Success)
   }
 }
 
+TEST_F(IMUSensorBroadcasterTest, TfPrefixNamespaceParams)
+{
+  const std::vector<TestPrefixParams> test_prefix_matrix = {
+    {"", "", ""},
+    {"/", "", ""},
+    {"", "/", ""},
+    {"test_prefix", "", "test_prefix"},
+    {"/test_prefix", "", "test_prefix"},
+    {"", "test_namespace", "test_namespace/"},
+    {"", "/test_namespace", "test_namespace/"},
+    {"test_prefix", "test_namespace", "test_prefix"},
+  };
+
+  for (const auto & params : test_prefix_matrix)
+  {
+    const std::string & test_namespace = params.ns;
+
+    SetUpIMUBroadcaster(test_namespace);
+
+    imu_broadcaster_->get_node()->set_parameter({"sensor_name", sensor_name_});
+    imu_broadcaster_->get_node()->set_parameter({"frame_id", frame_id_});
+    imu_broadcaster_->get_node()->set_parameter({"tf_frame_prefix", params.tf_prefix});
+
+    ASSERT_EQ(imu_broadcaster_->on_configure(rclcpp_lifecycle::State()), NODE_SUCCESS);
+    ASSERT_EQ(imu_broadcaster_->on_activate(rclcpp_lifecycle::State()), NODE_SUCCESS);
+
+    sensor_msgs::msg::Imu imu_msg;
+    subscribe_and_get_message(imu_msg, test_namespace);
+
+    EXPECT_EQ(imu_msg.header.frame_id, params.result_prefix + frame_id_);
+  }
+}
+
 int main(int argc, char ** argv)
 {
   ::testing::InitGoogleMock(&argc, argv);

imu_sensor_broadcaster/test/test_imu_sensor_broadcaster.hpp

@@ -81,4 +81,11 @@ class IMUSensorBroadcasterTest : public ::testing::Test
   void subscribe_and_get_message(sensor_msgs::msg::Imu & imu_msg);
 };
 
+struct TestPrefixParams
+{
+  std::string tf_prefix;
+  std::string ns;
+  std::string result_prefix;
+};
+
 #endif  // TEST_IMU_SENSOR_BROADCASTER_HPP_