Merge branch 'master' into add-multi_omni_wheel_drive_controller

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/src/diff_drive_controller.cpp

@@ -700,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

@@ -976,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

doc/release_notes.rst

@@ -64,6 +64,7 @@ multi_omni_wheel_drive_controller
 pid_controller
 ************************
 * 🚀 The PID controller was added 🎉 (`#434 <https://github.com/ros-controls/ros2_controllers/pull/434>`_).
+* Add ``save_i_term`` parameter to control retention of integral state after re-activation (`#1507 <https://github.com/ros-controls/ros2_controllers/pull/1507>`_).
 
 steering_controllers_library
 ********************************

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]);
   }
 }

gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp

@@ -165,8 +165,7 @@ class HardwareInterfaceAdapter<hardware_interface::HW_IF_EFFORT>
     // Time since the last call to update
     const auto period = std::chrono::steady_clock::now() - last_update_time_;
     // Update PIDs
-    double command =
-      pid_->computeCommand(error_position, error_velocity, static_cast<uint64_t>(period.count()));
+    double command = pid_->compute_command(error_position, error_velocity, period);
     command = std::min<double>(
       fabs(max_allowed_effort), std::max<double>(-fabs(max_allowed_effort), command));
     joint_handle_->get().set_value(command);

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -290,9 +290,8 @@ controller_interface::return_type JointTrajectoryController::update(
           for (auto i = 0ul; i < dof_; ++i)
           {
             tmp_command_[i] = (command_next_.velocities[i] * ff_velocity_scale_[i]) +
-                              pids_[i]->computeCommand(
-                                state_error_.positions[i], state_error_.velocities[i],
-                                (uint64_t)period.nanoseconds());
+                              pids_[i]->compute_command(
+                                state_error_.positions[i], state_error_.velocities[i], period);
           }
         }
 
@@ -1582,7 +1581,7 @@ void JointTrajectoryController::update_pids()
     if (pids_[i])
     {
       // update PIDs with gains from ROS parameters
-      pids_[i]->setGains(gains.p, gains.i, gains.d, gains.i_clamp, -gains.i_clamp);
+      pids_[i]->set_gains(gains.p, gains.i, gains.d, gains.i_clamp, -gains.i_clamp);
     }
     else
     {