[Humble Backport] tf_prefix param: fix slashes and add to IMU Broadcaster

diff_drive_controller/src/diff_drive_controller.cpp

@@ -394,26 +394,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

@@ -39,11 +39,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>
@@ -255,214 +257,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)
-{
-  const auto ret = controller_->init(controller_name);
-  ASSERT_EQ(ret, controller_interface::return_type::OK);
-
-  std::string odom_id = "odom";
-  std::string base_link_id = "base_link";
-  std::string frame_prefix = "test_prefix";
-
-  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)));
-
-  controller_->get_node()->set_parameter(
-    rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(false)));
-  controller_->get_node()->set_parameter(
-    rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_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(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)
-{
-  const auto ret = controller_->init(controller_name);
-  ASSERT_EQ(ret, controller_interface::return_type::OK);
-
-  std::string odom_id = "odom";
-  std::string base_link_id = "base_link";
-  std::string frame_prefix = "test_prefix";
-
-  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)));
-
-  controller_->get_node()->set_parameter(
-    rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true)));
-  controller_->get_node()->set_parameter(
-    rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_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(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)
-{
-  const auto ret = controller_->init(controller_name);
-  ASSERT_EQ(ret, controller_interface::return_type::OK);
-
-  std::string odom_id = "odom";
-  std::string base_link_id = "base_link";
-  std::string frame_prefix = "";
-
-  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)));
-
-  controller_->get_node()->set_parameter(
-    rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true)));
-  controller_->get_node()->set_parameter(
-    rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_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(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";
-
-  const auto ret = controller_->init(controller_name, test_namespace);
-  ASSERT_EQ(ret, controller_interface::return_type::OK);
-
-  std::string odom_id = "odom";
-  std::string base_link_id = "base_link";
-  std::string frame_prefix = "test_prefix";
-
-  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)));
-
-  controller_->get_node()->set_parameter(
-    rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(false)));
-  controller_->get_node()->set_parameter(
-    rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_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(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)
+TEST_F(TestDiffDriveController, TfPrefixNamespaceParams)
 {
-  std::string test_namespace = "/test_namespace";
-
-  const auto ret = controller_->init(controller_name, test_namespace);
-  ASSERT_EQ(ret, controller_interface::return_type::OK);
-
-  std::string odom_id = "odom";
-  std::string base_link_id = "base_link";
-  std::string frame_prefix = "test_prefix";
-
-  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)));
-
-  controller_->get_node()->set_parameter(
-    rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true)));
-  controller_->get_node()->set_parameter(
-    rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_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(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 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(TestDiffDriveController, configure_succeeds_tf_blank_prefix_true_set_namespace)
-{
-  std::string test_namespace = "/test_namespace";
+  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);
 
-  const auto ret = controller_->init(controller_name, test_namespace);
-  ASSERT_EQ(ret, controller_interface::return_type::OK);
+    std::string odom_id = "odom";
+    std::string base_link_id = "base_link";
 
-  std::string odom_id = "odom";
-  std::string base_link_id = "base_link";
-  std::string frame_prefix = "";
+    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)));
 
-  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)));
+    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)));
 
-  controller_->get_node()->set_parameter(
-    rclcpp::Parameter("tf_frame_prefix_enable", rclcpp::ParameterValue(true)));
-  controller_->get_node()->set_parameter(
-    rclcpp::Parameter("tf_frame_prefix", rclcpp::ParameterValue(frame_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(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,8 @@
+#include <string>
+
+struct TestPrefixParams
+{
+  std::string tf_prefix;
+  std::string ns;
+  std::string result_prefix;
+};

imu_sensor_broadcaster/src/imu_sensor_broadcaster.cpp

@@ -45,8 +45,24 @@ controller_interface::CallbackReturn IMUSensorBroadcaster::on_configure(
 {
   params_ = param_listener_->get_params();
 
+  std::string namespace_for_sensor_name = "";
+  if (params_.use_namespace_as_sensor_name_prefix)
+  {
+    namespace_for_sensor_name = std::string(get_node()->get_namespace());
+    ;
+    namespace_for_sensor_name.erase(0, 1);
+
+    if (*std::end(namespace_for_sensor_name) != '/' and namespace_for_sensor_name.size())
+    {
+      namespace_for_sensor_name = namespace_for_sensor_name + "/";
+    }
+  }
+
+  RCLCPP_INFO_STREAM(
+    get_node()->get_logger(), "Sensor name: " << namespace_for_sensor_name + params_.sensor_name);
+
   imu_sensor_ = std::make_unique<semantic_components::IMUSensor>(
-    semantic_components::IMUSensor(params_.sensor_name));
+    semantic_components::IMUSensor(namespace_for_sensor_name + params_.sensor_name));
   try
   {
     // register ft sensor data publisher
@@ -62,8 +78,26 @@ 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;
+  realtime_publisher_->msg_.header.frame_id = tf_prefix + params_.frame_id;
   // convert double vector to fixed-size array in the message
   for (size_t i = 0; i < 9; ++i)
   {

imu_sensor_broadcaster/src/imu_sensor_broadcaster_parameters.yaml

@@ -41,3 +41,13 @@ 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.",
+  }
+  use_namespace_as_sensor_name_prefix: {
+    type: bool,
+    default_value: false,
+    description: "If true the '/namespace/' is added to the sensor name which causes changes in interfaces names e. g. /namespace/sensor_name/orientation.x",
+  }

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"