Merge branch 'master' into open_loop_control_param

ackermann_steering_controller/test/test_ackermann_steering_controller.cpp

@@ -12,12 +12,13 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "test_ackermann_steering_controller.hpp"
-
 #include <memory>
 #include <string>
 #include <vector>
 
+#include "hardware_interface/types/hardware_interface_type_values.hpp"
+#include "test_ackermann_steering_controller.hpp"
+
 class AckermannSteeringControllerTest
 : public AckermannSteeringControllerFixture<TestableAckermannSteeringController>
 {
@@ -87,11 +88,13 @@ TEST_F(AckermannSteeringControllerTest, 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_name(),
+      ref_itf_prefix_name + "/" + hardware_interface::HW_IF_VELOCITY);
+    EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), ref_itf_prefix_name);
+    EXPECT_EQ(reference_interfaces[i]->get_interface_name(), hardware_interface::HW_IF_VELOCITY);
   }
 }
 

ackermann_steering_controller/test/test_ackermann_steering_controller.hpp

@@ -295,8 +295,7 @@ class AckermannSteeringControllerFixture : 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_ = "";

ackermann_steering_controller/test/test_ackermann_steering_controller_preceeding.cpp

@@ -12,12 +12,13 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "test_ackermann_steering_controller.hpp"
-
 #include <memory>
 #include <string>
 #include <vector>
 
+#include "hardware_interface/types/hardware_interface_type_values.hpp"
+#include "test_ackermann_steering_controller.hpp"
+
 class AckermannSteeringControllerTest
 : public AckermannSteeringControllerFixture<TestableAckermannSteeringController>
 {
@@ -89,11 +90,13 @@ TEST_F(AckermannSteeringControllerTest, 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_name(),
+      ref_itf_prefix_name + "/" + hardware_interface::HW_IF_VELOCITY);
+    EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), ref_itf_prefix_name);
+    EXPECT_EQ(reference_interfaces[i]->get_interface_name(), hardware_interface::HW_IF_VELOCITY);
   }
 }
 

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)

bicycle_steering_controller/test/test_bicycle_steering_controller.cpp

@@ -12,12 +12,13 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "test_bicycle_steering_controller.hpp"
-
 #include <memory>
 #include <string>
 #include <vector>
 
+#include "hardware_interface/types/hardware_interface_type_values.hpp"
+#include "test_bicycle_steering_controller.hpp"
+
 class BicycleSteeringControllerTest
 : public BicycleSteeringControllerFixture<TestableBicycleSteeringController>
 {
@@ -71,11 +72,13 @@ TEST_F(BicycleSteeringControllerTest, 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_name(),
+      ref_itf_prefix_name + "/" + hardware_interface::HW_IF_VELOCITY);
+    EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), ref_itf_prefix_name);
+    EXPECT_EQ(reference_interfaces[i]->get_interface_name(), hardware_interface::HW_IF_VELOCITY);
   }
 }
 

bicycle_steering_controller/test/test_bicycle_steering_controller.hpp

@@ -263,8 +263,7 @@ class BicycleSteeringControllerFixture : public ::testing::Test
 
   std::array<double, 2> joint_state_values_ = {{3.3, 0.5}};
   std::array<double, 2> joint_command_values_ = {{1.1, 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

bicycle_steering_controller/test/test_bicycle_steering_controller_preceeding.cpp

@@ -12,12 +12,13 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "test_bicycle_steering_controller.hpp"
-
 #include <memory>
 #include <string>
 #include <vector>
 
+#include "hardware_interface/types/hardware_interface_type_values.hpp"
+#include "test_bicycle_steering_controller.hpp"
+
 class BicycleSteeringControllerTest
 : public BicycleSteeringControllerFixture<TestableBicycleSteeringController>
 {
@@ -75,11 +76,13 @@ TEST_F(BicycleSteeringControllerTest, 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_name(),
+      ref_itf_prefix_name + "/" + hardware_interface::HW_IF_VELOCITY);
+    EXPECT_EQ(reference_interfaces[i]->get_prefix_name(), ref_itf_prefix_name);
+    EXPECT_EQ(reference_interfaces[i]->get_interface_name(), hardware_interface::HW_IF_VELOCITY);
   }
 }
 

diff_drive_controller/include/diff_drive_controller/diff_drive_controller.hpp

@@ -141,8 +141,6 @@ class DiffDriveController : public controller_interface::ChainableControllerInte
   rclcpp::Duration publish_period_ = rclcpp::Duration::from_nanoseconds(0);
   rclcpp::Time previous_publish_timestamp_{0, 0, RCL_CLOCK_UNINITIALIZED};
 
-  bool is_halted = false;
-
   bool reset();
   void halt();
 

diff_drive_controller/src/diff_drive_controller.cpp

@@ -101,15 +101,6 @@ controller_interface::return_type DiffDriveController::update_reference_from_sub
   const rclcpp::Time & time, const rclcpp::Duration & /*period*/)
 {
   auto logger = get_node()->get_logger();
-  if (get_lifecycle_state().id() == State::PRIMARY_STATE_INACTIVE)
-  {
-    if (!is_halted)
-    {
-      halt();
-      is_halted = true;
-    }
-    return controller_interface::return_type::OK;
-  }
 
   const std::shared_ptr<TwistStamped> command_msg_ptr = *(received_velocity_msg_ptr_.readFromRT());
 
@@ -149,15 +140,6 @@ controller_interface::return_type DiffDriveController::update_and_write_commands
   const rclcpp::Time & time, const rclcpp::Duration & period)
 {
   auto logger = get_node()->get_logger();
-  if (get_lifecycle_state().id() == State::PRIMARY_STATE_INACTIVE)
-  {
-    if (!is_halted)
-    {
-      halt();
-      is_halted = true;
-    }
-    return controller_interface::return_type::OK;
-  }
 
   // command may be limited further by SpeedLimit,
   // without affecting the stored twist command
@@ -562,7 +544,6 @@ controller_interface::CallbackReturn DiffDriveController::on_activate(
     return controller_interface::CallbackReturn::ERROR;
   }
 
-  is_halted = false;
   subscriber_is_active_ = true;
 
   RCLCPP_DEBUG(get_node()->get_logger(), "Subscriber and publisher are now active.");
@@ -573,11 +554,7 @@ controller_interface::CallbackReturn DiffDriveController::on_deactivate(
   const rclcpp_lifecycle::State &)
 {
   subscriber_is_active_ = false;
-  if (!is_halted)
-  {
-    halt();
-    is_halted = true;
-  }
+  halt();
   reset_buffers();
   registered_left_wheel_handles_.clear();
   registered_right_wheel_handles_.clear();
@@ -616,7 +593,6 @@ bool DiffDriveController::reset()
   subscriber_is_active_ = false;
   velocity_command_subscriber_.reset();
 
-  is_halted = false;
   return true;
 }
 
@@ -724,11 +700,11 @@ DiffDriveController::on_export_reference_interfaces()
   reference_interfaces.reserve(reference_interfaces_.size());
 
   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

@@ -719,11 +719,16 @@ TEST_F(TestDiffDriveController, cleanup)
     controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)),
     controller_interface::return_type::OK);
 
+  // should be moving
+  EXPECT_LT(0.0, left_wheel_vel_cmd_.get_value());
+  EXPECT_LT(0.0, right_wheel_vel_cmd_.get_value());
+
   state = controller_->get_node()->deactivate();
   ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id());
-  ASSERT_EQ(
-    controller_->update(rclcpp::Time(0, 0, RCL_ROS_TIME), rclcpp::Duration::from_seconds(0.01)),
-    controller_interface::return_type::OK);
+
+  // should be stopped
+  EXPECT_EQ(0.0, left_wheel_vel_cmd_.get_value()) << "Wheels should be halted on deactivate()";
+  EXPECT_EQ(0.0, right_wheel_vel_cmd_.get_value()) << "Wheels should be halted on deactivate()";
 
   state = controller_->get_node()->cleanup();
   ASSERT_EQ(State::PRIMARY_STATE_UNCONFIGURED, state.id());
@@ -971,23 +976,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;
 }