Fix the exported interface naming in the chainable controllers of ROS…

…2 Controllers

admittance_controller/src/admittance_controller.cpp

@@ -147,9 +147,9 @@ AdmittanceController::on_export_reference_interfaces()
       {
         velocity_reference_.emplace_back(reference_interfaces_[index]);
       }
-      const auto full_name = joint + "/" + interface;
+      const auto exported_prefix = std::string(get_node()->get_name()) + "/" + joint;
       chainable_command_interfaces.emplace_back(hardware_interface::CommandInterface(
-        std::string(get_node()->get_name()), full_name, reference_interfaces_.data() + index));
+        exported_prefix, interface, reference_interfaces_.data() + index));
 
       index++;
     }

admittance_controller/test/test_admittance_controller.cpp

@@ -168,6 +168,27 @@ TEST_F(AdmittanceControllerTest, check_interfaces)
   ASSERT_EQ(
     controller_->state_interfaces_.size(),
     state_interface_types_.size() * joint_names_.size() + fts_state_values_.size());
+
+  const auto reference_interfaces = controller_->ordered_exported_reference_interfaces_;
+  ASSERT_EQ(reference_interfaces.size(), 2 * joint_names_.size());
+  for (auto i = 0ul; i < joint_names_.size(); i++)
+  {
+    const std::string ref_itf_prefix_name =
+      std::string(controller_->get_node()->get_name()) + "/" + joint_names_[i];
+    EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), ref_itf_prefix_name);
+    EXPECT_EQ(
+      reference_interfaces[i]->get_name(),
+      ref_itf_prefix_name + "/" + hardware_interface::HW_IF_POSITION);
+    EXPECT_EQ(reference_interfaces[i]->get_interface_name(), hardware_interface::HW_IF_POSITION);
+    EXPECT_EQ(
+      reference_interfaces[i + joint_names_.size()]->get_prefix_name(), ref_itf_prefix_name);
+    EXPECT_EQ(
+      reference_interfaces[i + joint_names_.size()]->get_name(),
+      ref_itf_prefix_name + "/" + hardware_interface::HW_IF_VELOCITY);
+    EXPECT_EQ(
+      reference_interfaces[i + joint_names_.size()]->get_interface_name(),
+      hardware_interface::HW_IF_VELOCITY);
+  }
 }
 
 TEST_F(AdmittanceControllerTest, activate_success)

diff_drive_controller/src/diff_drive_controller.cpp

@@ -720,11 +720,11 @@ DiffDriveController::on_export_reference_interfaces()
   reference_interfaces.reserve(nr_ref_itfs);
 
   reference_interfaces.push_back(hardware_interface::CommandInterface(
-    get_node()->get_name(), std::string("linear/") + hardware_interface::HW_IF_VELOCITY,
+    get_node()->get_name() + std::string("/linear"), hardware_interface::HW_IF_VELOCITY,
     &reference_interfaces_[0]));
 
   reference_interfaces.push_back(hardware_interface::CommandInterface(
-    get_node()->get_name(), std::string("angular/") + hardware_interface::HW_IF_VELOCITY,
+    get_node()->get_name() + std::string("/angular"), hardware_interface::HW_IF_VELOCITY,
     &reference_interfaces_[1]));
 
   return reference_interfaces;

diff_drive_controller/test/test_diff_drive_controller.cpp

@@ -971,23 +971,22 @@ TEST_F(TestDiffDriveController, reference_interfaces_are_properly_exported)
   ASSERT_EQ(reference_interfaces.size(), 2)
     << "Expected exactly 2 reference interfaces: linear and angular";
 
-  const std::string expected_prefix_name = std::string(controller_->get_node()->get_name());
-  const std::string expected_linear_interface_name =
-    std::string("linear/") + hardware_interface::HW_IF_VELOCITY;
-  const std::string expected_angular_interface_name =
-    std::string("angular/") + hardware_interface::HW_IF_VELOCITY;
+  const std::string expected_linear_prefix_name =
+    std::string(controller_->get_node()->get_name()) + std::string("/linear");
+  const std::string expected_angular_prefix_name =
+    std::string(controller_->get_node()->get_name()) + std::string("/angular");
   const std::string expected_linear_name =
-    expected_prefix_name + std::string("/") + expected_linear_interface_name;
+    expected_linear_prefix_name + std::string("/") + hardware_interface::HW_IF_VELOCITY;
   const std::string expected_angular_name =
-    expected_prefix_name + std::string("/") + expected_angular_interface_name;
+    expected_angular_prefix_name + std::string("/") + hardware_interface::HW_IF_VELOCITY;
 
   ASSERT_EQ(reference_interfaces[0]->get_name(), expected_linear_name);
   ASSERT_EQ(reference_interfaces[1]->get_name(), expected_angular_name);
 
-  EXPECT_EQ(reference_interfaces[0]->get_prefix_name(), expected_prefix_name);
-  EXPECT_EQ(reference_interfaces[0]->get_interface_name(), expected_linear_interface_name);
-  EXPECT_EQ(reference_interfaces[1]->get_prefix_name(), expected_prefix_name);
-  EXPECT_EQ(reference_interfaces[1]->get_interface_name(), expected_angular_interface_name);
+  EXPECT_EQ(reference_interfaces[0]->get_prefix_name(), expected_linear_prefix_name);
+  EXPECT_EQ(reference_interfaces[0]->get_interface_name(), hardware_interface::HW_IF_VELOCITY);
+  EXPECT_EQ(reference_interfaces[1]->get_prefix_name(), expected_angular_prefix_name);
+  EXPECT_EQ(reference_interfaces[1]->get_interface_name(), hardware_interface::HW_IF_VELOCITY);
 }
 
 // Make sure that the controller is properly reset when deactivated

force_torque_sensor_broadcaster/src/force_torque_sensor_broadcaster.cpp

@@ -176,44 +176,55 @@ ForceTorqueSensorBroadcaster::on_export_state_interfaces()
   std::vector<std::string> torque_names(
     {params_.interface_names.torque.x, params_.interface_names.torque.y,
      params_.interface_names.torque.z});
+  std::string export_prefix = get_node()->get_name();
   if (!params_.sensor_name.empty())
   {
     const auto semantic_comp_itf_names = force_torque_sensor_->get_state_interface_names();
     std::copy(
       semantic_comp_itf_names.begin(), semantic_comp_itf_names.begin() + 3, force_names.begin());
     std::copy(
       semantic_comp_itf_names.begin() + 3, semantic_comp_itf_names.end(), torque_names.begin());
+
+    // Update the prefix and get the proper force and torque names
+    export_prefix = export_prefix + "/" + params_.sensor_name;
+    // strip "/" and get the second part of the information
+    // e.g. /ft_sensor/force.x -> force.x
+    std::for_each(
+      force_names.begin(), force_names.end(),
+      [](std::string & name) { name = name.substr(name.find_last_of("/") + 1); });
+    std::for_each(
+      torque_names.begin(), torque_names.end(),
+      [](std::string & name) { name = name.substr(name.find_last_of("/") + 1); });
   }
-  const std::string controller_name = get_node()->get_name();
   if (!force_names[0].empty())
   {
     exported_state_interfaces.emplace_back(hardware_interface::StateInterface(
-      controller_name, force_names[0], &realtime_publisher_->msg_.wrench.force.x));
+      export_prefix, force_names[0], &realtime_publisher_->msg_.wrench.force.x));
   }
   if (!force_names[1].empty())
   {
     exported_state_interfaces.emplace_back(hardware_interface::StateInterface(
-      controller_name, force_names[1], &realtime_publisher_->msg_.wrench.force.y));
+      export_prefix, force_names[1], &realtime_publisher_->msg_.wrench.force.y));
   }
   if (!force_names[2].empty())
   {
     exported_state_interfaces.emplace_back(hardware_interface::StateInterface(
-      controller_name, force_names[2], &realtime_publisher_->msg_.wrench.force.z));
+      export_prefix, force_names[2], &realtime_publisher_->msg_.wrench.force.z));
   }
   if (!torque_names[0].empty())
   {
     exported_state_interfaces.emplace_back(hardware_interface::StateInterface(
-      controller_name, torque_names[0], &realtime_publisher_->msg_.wrench.torque.x));
+      export_prefix, torque_names[0], &realtime_publisher_->msg_.wrench.torque.x));
   }
   if (!torque_names[1].empty())
   {
     exported_state_interfaces.emplace_back(hardware_interface::StateInterface(
-      controller_name, torque_names[1], &realtime_publisher_->msg_.wrench.torque.y));
+      export_prefix, torque_names[1], &realtime_publisher_->msg_.wrench.torque.y));
   }
   if (!torque_names[2].empty())
   {
     exported_state_interfaces.emplace_back(hardware_interface::StateInterface(
-      controller_name, torque_names[2], &realtime_publisher_->msg_.wrench.torque.z));
+      export_prefix, torque_names[2], &realtime_publisher_->msg_.wrench.torque.z));
   }
   return exported_state_interfaces;
 }

force_torque_sensor_broadcaster/test/test_force_torque_sensor_broadcaster.cpp

@@ -324,8 +324,16 @@ TEST_F(ForceTorqueSensorBroadcasterTest, SensorName_Publish_Success_with_Offsets
     exported_state_interfaces[4]->get_name(), controller_name + "/" + sensor_name_ + "/torque.y");
   ASSERT_EQ(
     exported_state_interfaces[5]->get_name(), controller_name + "/" + sensor_name_ + "/torque.z");
+  ASSERT_EQ(exported_state_interfaces[0]->get_interface_name(), "force.x");
+  ASSERT_EQ(exported_state_interfaces[1]->get_interface_name(), "force.y");
+  ASSERT_EQ(exported_state_interfaces[2]->get_interface_name(), "force.z");
+  ASSERT_EQ(exported_state_interfaces[3]->get_interface_name(), "torque.x");
+  ASSERT_EQ(exported_state_interfaces[4]->get_interface_name(), "torque.y");
+  ASSERT_EQ(exported_state_interfaces[5]->get_interface_name(), "torque.z");
   for (size_t i = 0; i < 6; ++i)
   {
+    ASSERT_EQ(
+      exported_state_interfaces[i]->get_prefix_name(), controller_name + "/" + sensor_name_);
     ASSERT_EQ(
       exported_state_interfaces[i]->get_value(),
       sensor_values_[i] + (i < 3 ? force_offsets[i] : torque_offsets[i - 3]));
@@ -361,6 +369,10 @@ TEST_F(ForceTorqueSensorBroadcasterTest, InterfaceNames_Publish_Success)
   const std::string controller_name = fts_broadcaster_->get_node()->get_name();
   ASSERT_EQ(exported_state_interfaces[0]->get_name(), controller_name + "/fts_sensor/force.x");
   ASSERT_EQ(exported_state_interfaces[1]->get_name(), controller_name + "/fts_sensor/torque.z");
+  ASSERT_EQ(exported_state_interfaces[0]->get_prefix_name(), controller_name);
+  ASSERT_EQ(exported_state_interfaces[1]->get_prefix_name(), controller_name);
+  ASSERT_EQ(exported_state_interfaces[0]->get_interface_name(), "fts_sensor/force.x");
+  ASSERT_EQ(exported_state_interfaces[1]->get_interface_name(), "fts_sensor/torque.z");
   ASSERT_EQ(exported_state_interfaces[0]->get_value(), sensor_values_[0]);
   ASSERT_EQ(exported_state_interfaces[1]->get_value(), sensor_values_[5]);
 }
@@ -402,8 +414,15 @@ TEST_F(ForceTorqueSensorBroadcasterTest, All_InterfaceNames_Publish_Success)
   ASSERT_EQ(exported_state_interfaces[3]->get_name(), controller_name + "/fts_sensor/torque.x");
   ASSERT_EQ(exported_state_interfaces[4]->get_name(), controller_name + "/fts_sensor/torque.y");
   ASSERT_EQ(exported_state_interfaces[5]->get_name(), controller_name + "/fts_sensor/torque.z");
+  ASSERT_EQ(exported_state_interfaces[0]->get_interface_name(), "fts_sensor/force.x");
+  ASSERT_EQ(exported_state_interfaces[1]->get_interface_name(), "fts_sensor/force.y");
+  ASSERT_EQ(exported_state_interfaces[2]->get_interface_name(), "fts_sensor/force.z");
+  ASSERT_EQ(exported_state_interfaces[3]->get_interface_name(), "fts_sensor/torque.x");
+  ASSERT_EQ(exported_state_interfaces[4]->get_interface_name(), "fts_sensor/torque.y");
+  ASSERT_EQ(exported_state_interfaces[5]->get_interface_name(), "fts_sensor/torque.z");
   for (size_t i = 0; i < 6; ++i)
   {
+    ASSERT_EQ(exported_state_interfaces[0]->get_prefix_name(), controller_name);
     ASSERT_EQ(exported_state_interfaces[i]->get_value(), sensor_values_[i]);
   }
 }

mecanum_drive_controller/src/mecanum_drive_controller.cpp

@@ -279,13 +279,13 @@ MecanumDriveController::on_export_reference_interfaces()
 
   reference_interfaces.reserve(reference_interfaces_.size());
 
-  std::vector<std::string> reference_interface_names = {
-    "linear/x/velocity", "linear/y/velocity", "angular/z/velocity"};
+  std::vector<std::string> reference_interface_names = {"/linear/x", "/linear/y", "/angular/z"};
 
   for (size_t i = 0; i < reference_interfaces_.size(); ++i)
   {
     reference_interfaces.push_back(hardware_interface::CommandInterface(
-      get_node()->get_name(), reference_interface_names[i], &reference_interfaces_[i]));
+      get_node()->get_name() + reference_interface_names[i], hardware_interface::HW_IF_VELOCITY,
+      &reference_interfaces_[i]));
   }
 
   return reference_interfaces;

mecanum_drive_controller/test/test_mecanum_drive_controller.cpp

@@ -108,11 +108,13 @@ TEST_F(MecanumDriveControllerTest, when_controller_configured_expect_properly_ex
 
   for (size_t i = 0; i < reference_interface_names.size(); ++i)
   {
-    const std::string ref_itf_name = std::string(controller_->get_node()->get_name()) +
-                                     std::string("/") + reference_interface_names[i];
-    EXPECT_EQ(reference_interfaces[i]->get_name(), ref_itf_name);
-    EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), controller_->get_node()->get_name());
-    EXPECT_EQ(reference_interfaces[i]->get_interface_name(), reference_interface_names[i]);
+    const std::string ref_itf_prefix_name =
+      std::string(controller_->get_node()->get_name()) + "/" + reference_interface_names[i];
+    EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), ref_itf_prefix_name);
+    EXPECT_EQ(
+      reference_interfaces[i]->get_name(),
+      ref_itf_prefix_name + "/" + hardware_interface::HW_IF_VELOCITY);
+    EXPECT_EQ(reference_interfaces[i]->get_interface_name(), hardware_interface::HW_IF_VELOCITY);
   }
 }
 

mecanum_drive_controller/test/test_mecanum_drive_controller.hpp

@@ -269,8 +269,7 @@ class MecanumDriveControllerFixture : public ::testing::Test
   }
 
 protected:
-  std::vector<std::string> reference_interface_names = {
-    "linear/x/velocity", "linear/y/velocity", "angular/z/velocity"};
+  std::vector<std::string> reference_interface_names = {"linear/x", "linear/y", "angular/z"};
 
   static constexpr char TEST_FRONT_LEFT_CMD_JOINT_NAME[] = "front_left_wheel_joint";
   static constexpr char TEST_FRONT_RIGHT_CMD_JOINT_NAME[] = "front_right_wheel_joint";

pid_controller/src/pid_controller.cpp

@@ -392,7 +392,8 @@ std::vector<hardware_interface::CommandInterface> PidController::on_export_refer
     for (const auto & dof_name : reference_and_state_dof_names_)
     {
       reference_interfaces.push_back(hardware_interface::CommandInterface(
-        get_node()->get_name(), dof_name + "/" + interface, &reference_interfaces_[index]));
+        std::string(get_node()->get_name()) + "/" + dof_name, interface,
+        &reference_interfaces_[index]));
       ++index;
     }
   }

pid_controller/test/test_pid_controller.cpp

@@ -98,8 +98,10 @@ TEST_F(PidControllerTest, check_exported_interfaces)
       const std::string ref_itf_name =
         std::string(controller_->get_node()->get_name()) + "/" + dof_name + "/" + interface;
       EXPECT_EQ(ref_if_conf[ri_index]->get_name(), ref_itf_name);
-      EXPECT_EQ(ref_if_conf[ri_index]->get_prefix_name(), controller_->get_node()->get_name());
-      EXPECT_EQ(ref_if_conf[ri_index]->get_interface_name(), dof_name + "/" + interface);
+      EXPECT_EQ(
+        ref_if_conf[ri_index]->get_prefix_name(),
+        std::string(controller_->get_node()->get_name()) + "/" + dof_name);
+      EXPECT_EQ(ref_if_conf[ri_index]->get_interface_name(), interface);
       ++ri_index;
     }
   }

pid_controller/test/test_pid_controller_preceding.cpp

@@ -85,8 +85,10 @@ TEST_F(PidControllerTest, check_exported_interfaces)
       const std::string ref_itf_name =
         std::string(controller_->get_node()->get_name()) + "/" + dof_name + "/" + interface;
       EXPECT_EQ(ref_if_conf[ri_index]->get_name(), ref_itf_name);
-      EXPECT_EQ(ref_if_conf[ri_index]->get_prefix_name(), controller_->get_node()->get_name());
-      EXPECT_EQ(ref_if_conf[ri_index]->get_interface_name(), dof_name + "/" + interface);
+      EXPECT_EQ(
+        ref_if_conf[ri_index]->get_prefix_name(),
+        std::string(controller_->get_node()->get_name()) + "/" + dof_name);
+      EXPECT_EQ(ref_if_conf[ri_index]->get_interface_name(), interface);
       ++ri_index;
     }
   }

steering_controllers_library/src/steering_controllers_library.cpp

@@ -317,11 +317,11 @@ SteeringControllersLibrary::on_export_reference_interfaces()
   reference_interfaces.reserve(nr_ref_itfs_);
 
   reference_interfaces.push_back(hardware_interface::CommandInterface(
-    get_node()->get_name(), std::string("linear/") + hardware_interface::HW_IF_VELOCITY,
+    get_node()->get_name() + std::string("/linear"), hardware_interface::HW_IF_VELOCITY,
     &reference_interfaces_[0]));
 
   reference_interfaces.push_back(hardware_interface::CommandInterface(
-    get_node()->get_name(), std::string("angular/") + hardware_interface::HW_IF_VELOCITY,
+    get_node()->get_name() + std::string("/angular"), hardware_interface::HW_IF_VELOCITY,
     &reference_interfaces_[1]));
 
   return reference_interfaces;

steering_controllers_library/test/test_steering_controllers_library.cpp

@@ -12,13 +12,14 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "test_steering_controllers_library.hpp"
-
 #include <limits>
 #include <memory>
 #include <string>
 #include <vector>
 
+#include "hardware_interface/types/hardware_interface_type_values.hpp"
+#include "test_steering_controllers_library.hpp"
+
 class SteeringControllersLibraryTest
 : public SteeringControllersLibraryFixture<TestableSteeringControllersLibrary>
 {
@@ -68,11 +69,13 @@ TEST_F(SteeringControllersLibraryTest, check_exported_interfaces)
   ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size());
   for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i)
   {
-    const std::string ref_itf_name =
+    const std::string ref_itf_prefix_name =
       std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i];
-    EXPECT_EQ(reference_interfaces[i]->get_name(), ref_itf_name);
-    EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), controller_->get_node()->get_name());
-    EXPECT_EQ(reference_interfaces[i]->get_interface_name(), joint_reference_interfaces_[i]);
+    EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), ref_itf_prefix_name);
+    EXPECT_EQ(
+      reference_interfaces[i]->get_name(),
+      ref_itf_prefix_name + "/" + hardware_interface::HW_IF_VELOCITY);
+    EXPECT_EQ(reference_interfaces[i]->get_interface_name(), hardware_interface::HW_IF_VELOCITY);
   }
 }
 

steering_controllers_library/test/test_steering_controllers_library.hpp

@@ -317,8 +317,7 @@ class SteeringControllersLibraryFixture : public ::testing::Test
   std::array<double, 4> joint_state_values_ = {{0.5, 0.5, 0.0, 0.0}};
   std::array<double, 4> joint_command_values_ = {{1.1, 3.3, 2.2, 4.4}};
 
-  std::array<std::string, 2> joint_reference_interfaces_ = {
-    {"linear/velocity", "angular/velocity"}};
+  std::array<std::string, 2> joint_reference_interfaces_ = {{"linear", "angular"}};
   std::string steering_interface_name_ = "position";
   // defined in setup
   std::string traction_interface_name_ = "";

tricycle_steering_controller/test/test_tricycle_steering_controller.cpp

@@ -12,12 +12,13 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "test_tricycle_steering_controller.hpp"
-
 #include <memory>
 #include <string>
 #include <vector>
 
+#include "hardware_interface/types/hardware_interface_type_values.hpp"
+#include "test_tricycle_steering_controller.hpp"
+
 class TricycleSteeringControllerTest
 : public TricycleSteeringControllerFixture<TestableTricycleSteeringController>
 {
@@ -75,15 +76,17 @@ TEST_F(TricycleSteeringControllerTest, check_exported_interfaces)
   EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL);
 
   // check ref itfs
-  auto ref_if_conf = controller_->export_reference_interfaces();
-  ASSERT_EQ(ref_if_conf.size(), joint_reference_interfaces_.size());
+  auto reference_interfaces = controller_->export_reference_interfaces();
+  ASSERT_EQ(reference_interfaces.size(), joint_reference_interfaces_.size());
   for (size_t i = 0; i < joint_reference_interfaces_.size(); ++i)
   {
-    const std::string ref_itf_name =
+    const std::string ref_itf_prefix_name =
       std::string(controller_->get_node()->get_name()) + "/" + joint_reference_interfaces_[i];
-    EXPECT_EQ(ref_if_conf[i]->get_name(), ref_itf_name);
-    EXPECT_EQ(ref_if_conf[i]->get_prefix_name(), controller_->get_node()->get_name());
-    EXPECT_EQ(ref_if_conf[i]->get_interface_name(), joint_reference_interfaces_[i]);
+    EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), ref_itf_prefix_name);
+    EXPECT_EQ(
+      reference_interfaces[i]->get_name(),
+      ref_itf_prefix_name + "/" + hardware_interface::HW_IF_VELOCITY);
+    EXPECT_EQ(reference_interfaces[i]->get_interface_name(), hardware_interface::HW_IF_VELOCITY);
   }
 }
 

tricycle_steering_controller/test/test_tricycle_steering_controller.hpp

@@ -280,7 +280,7 @@ class TricycleSteeringControllerFixture : public ::testing::Test
 
   std::array<double, 3> joint_state_values_{{0.5, 0.5, 0.0}};
   std::array<double, 3> joint_command_values_{{1.1, 3.3, 2.2}};
-  std::array<std::string, 2> joint_reference_interfaces_{{"linear/velocity", "angular/velocity"}};
+  std::array<std::string, 2> joint_reference_interfaces_{{"linear", "angular"}};
   std::string steering_interface_name_ = "position";
   // defined in setup
   std::string traction_interface_name_ = "";