[JTC] Add additional parameter to enable configuration of interfaces …

…for following controllers in a chain. (#380)

* [JTC] Add additional parameter to enable configuration of interfaces for following controllers in a chain.
* Use command_joint_states correctly in parameters depending on joint states.
* Fixup tests.

joint_trajectory_controller/doc/userdoc.rst

@@ -32,6 +32,26 @@ Other features
 
     Robust to system clock changes: Discontinuous system clock changes do not cause discontinuities in the execution of already queued trajectory segments.
 
+ros2_control interfaces
+------------------------
+
+References
+^^^^^^^^^^^
+(the controller is not yet implemented as chainable controller)
+
+States
+^^^^^^^
+The state interfaces are defined with ``joints`` and ``state_interfaces`` parameters as follows: ``<joint>/<state_interface>``.
+Supported state interfaces are ``position``, ``velocity``, ``acceleration`` and ``effort`` as defined in the [hardware_interface/hardware_interface_type_values.hpp](https://github.com/ros-controls/ros2_control/blob/master/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp).
+Legal combinations of state interfaces are:
+- ``position``
+- ``position`` and ``velocity``
+- ``position``, ``velocity`` and ``acceleration``
+- ``effort``
+
+Commands
+^^^^^^^^^
+
 
 Using Joint Trajectory Controller(s)
 ------------------------------------
@@ -84,8 +104,11 @@ A yaml file for using it could be:
 Details about parameters
 ^^^^^^^^^^^^^^^^^^^^^^^^
 
-joint (list(string)):
-  Joint names to control.
+joints (list(string)):
+  Joint names to control and listen to.
+
+command_joints (list(string)):
+  Joint names to control. This parameters is used if JTC is used in a controller chain where command and state interfaces don't have same names.
 
 command_interface (list(string)):
   Command interfaces provided by the hardware interface for all joints.

joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp

@@ -113,6 +113,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
 
 protected:
   std::vector<std::string> joint_names_;
+  std::vector<std::string> command_joint_names_;
   std::vector<std::string> command_interface_types_;
   std::vector<std::string> state_interface_types_;
 

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -56,6 +56,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_init()
   {
     // with the lifecycle node being initialized, we can declare parameters
     joint_names_ = auto_declare<std::vector<std::string>>("joints", joint_names_);
+    command_joint_names_ =
+      auto_declare<std::vector<std::string>>("command_joints", command_joint_names_);
     command_interface_types_ =
       auto_declare<std::vector<std::string>>("command_interfaces", command_interface_types_);
     state_interface_types_ =
@@ -97,7 +99,7 @@ JointTrajectoryController::command_interface_configuration() const
     std::exit(EXIT_FAILURE);
   }
   conf.names.reserve(dof_ * command_interface_types_.size());
-  for (const auto & joint_name : joint_names_)
+  for (const auto & joint_name : command_joint_names_)
   {
     for (const auto & interface_type : command_interface_types_)
     {
@@ -493,16 +495,33 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
 
   dof_ = joint_names_.size();
 
+  // TODO(destogl): why is this here? Add comment or move
   if (!reset())
   {
     return CallbackReturn::FAILURE;
   }
 
   if (joint_names_.empty())
   {
+    // TODO(destogl): is this correct? Can we really move-on if no joint names are not provided?
     RCLCPP_WARN(logger, "'joints' parameter is empty.");
   }
 
+  command_joint_names_ = get_node()->get_parameter("command_joints").as_string_array();
+
+  if (command_joint_names_.empty())
+  {
+    command_joint_names_ = joint_names_;
+    RCLCPP_INFO(
+      logger, "No specific joint names are used for command interfaces. Using 'joints' parameter.");
+  }
+  else if (command_joint_names_.size() != joint_names_.size())
+  {
+    RCLCPP_ERROR(
+      logger, "'command_joints' parameter has to have the same size as 'joints' parameter.");
+    return CallbackReturn::FAILURE;
+  }
+
   // Specialized, child controllers set interfaces before calling configure function.
   if (command_interface_types_.empty())
   {
@@ -592,12 +611,13 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
     // Init PID gains from ROS parameter server
     for (size_t i = 0; i < pids_.size(); ++i)
     {
-      const std::string prefix = "gains." + joint_names_[i];
+      const std::string prefix = "gains." + command_joint_names_[i];
       const auto k_p = auto_declare<double>(prefix + ".p", 0.0);
       const auto k_i = auto_declare<double>(prefix + ".i", 0.0);
       const auto k_d = auto_declare<double>(prefix + ".d", 0.0);
       const auto i_clamp = auto_declare<double>(prefix + ".i_clamp", 0.0);
-      ff_velocity_scale_[i] = auto_declare<double>("ff_velocity_scale/" + joint_names_[i], 0.0);
+      ff_velocity_scale_[i] =
+        auto_declare<double>("ff_velocity_scale/" + command_joint_names_[i], 0.0);
       // Initialize PID
       pids_[i] = std::make_shared<control_toolbox::Pid>(k_p, k_i, k_d, i_clamp, -i_clamp);
     }
@@ -702,7 +722,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
     get_interface_list(command_interface_types_).c_str(),
     get_interface_list(state_interface_types_).c_str());
 
-  default_tolerances_ = get_segment_tolerances(*get_node(), joint_names_);
+  default_tolerances_ = get_segment_tolerances(*get_node(), command_joint_names_);
 
   // Read parameters customizing controller for special cases
   open_loop_control_ = get_node()->get_parameter("open_loop_control").get_value<bool>();
@@ -738,7 +758,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
   state_publisher_ = std::make_unique<StatePublisher>(publisher_);
 
   state_publisher_->lock();
-  state_publisher_->msg_.joint_names = joint_names_;
+  state_publisher_->msg_.joint_names = command_joint_names_;
   state_publisher_->msg_.desired.positions.resize(dof_);
   state_publisher_->msg_.desired.velocities.resize(dof_);
   state_publisher_->msg_.desired.accelerations.resize(dof_);
@@ -797,7 +817,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate(
       std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface);
     auto index = std::distance(allowed_interface_types_.begin(), it);
     if (!controller_interface::get_ordered_interfaces(
-          command_interfaces_, joint_names_, interface, joint_command_interface_[index]))
+          command_interfaces_, command_joint_names_, interface, joint_command_interface_[index]))
     {
       RCLCPP_ERROR(
         get_node()->get_logger(), "Expected %zu '%s' command interfaces, got %zu.", dof_,

joint_trajectory_controller/test/test_load_joint_trajectory_controller.cpp

@@ -12,9 +12,10 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include <gmock/gmock.h>
 #include <memory>
 
+#include "gmock/gmock.h"
+
 #include "controller_manager/controller_manager.hpp"
 #include "hardware_interface/resource_manager.hpp"
 #include "rclcpp/executor.hpp"

joint_trajectory_controller/test/test_trajectory_controller.cpp

@@ -95,6 +95,87 @@ TEST_P(TrajectoryControllerTestParameterized, configure_state_ignores_commands)
   executor.cancel();
 }
 
+TEST_P(TrajectoryControllerTestParameterized, check_interface_names)
+{
+  SetUpTrajectoryController();
+
+  const auto state = traj_controller_->get_node()->configure();
+  ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE);
+
+  std::vector<std::string> state_interface_names;
+  state_interface_names.reserve(joint_names_.size() * state_interface_types_.size());
+  for (const auto & joint : joint_names_)
+  {
+    for (const auto & interface : state_interface_types_)
+    {
+      state_interface_names.push_back(joint + "/" + interface);
+    }
+  }
+  auto state_interfaces = traj_controller_->state_interface_configuration();
+  EXPECT_EQ(state_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL);
+  EXPECT_EQ(state_interfaces.names.size(), joint_names_.size() * state_interface_types_.size());
+  ASSERT_THAT(state_interfaces.names, testing::UnorderedElementsAreArray(state_interface_names));
+
+  std::vector<std::string> command_interface_names;
+  command_interface_names.reserve(joint_names_.size() * command_interface_types_.size());
+  for (const auto & joint : joint_names_)
+  {
+    for (const auto & interface : command_interface_types_)
+    {
+      command_interface_names.push_back(joint + "/" + interface);
+    }
+  }
+  auto command_interfaces = traj_controller_->command_interface_configuration();
+  EXPECT_EQ(
+    command_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL);
+  EXPECT_EQ(command_interfaces.names.size(), joint_names_.size() * command_interface_types_.size());
+  ASSERT_THAT(
+    command_interfaces.names, testing::UnorderedElementsAreArray(command_interface_names));
+}
+
+TEST_P(TrajectoryControllerTestParameterized, check_interface_names_with_command_joints)
+{
+  SetUpTrajectoryController();
+
+  // set command_joints parameter
+  const rclcpp::Parameter command_joint_names_param("command_joints", command_joint_names_);
+  traj_controller_->get_node()->set_parameter({command_joint_names_param});
+
+  const auto state = traj_controller_->get_node()->configure();
+  ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE);
+
+  std::vector<std::string> state_interface_names;
+  state_interface_names.reserve(joint_names_.size() * state_interface_types_.size());
+  for (const auto & joint : joint_names_)
+  {
+    for (const auto & interface : state_interface_types_)
+    {
+      state_interface_names.push_back(joint + "/" + interface);
+    }
+  }
+  auto state_interfaces = traj_controller_->state_interface_configuration();
+  EXPECT_EQ(state_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL);
+  EXPECT_EQ(state_interfaces.names.size(), joint_names_.size() * state_interface_types_.size());
+  ASSERT_THAT(state_interfaces.names, testing::UnorderedElementsAreArray(state_interface_names));
+
+  std::vector<std::string> command_interface_names;
+  command_interface_names.reserve(command_joint_names_.size() * command_interface_types_.size());
+  for (const auto & joint : command_joint_names_)
+  {
+    for (const auto & interface : command_interface_types_)
+    {
+      command_interface_names.push_back(joint + "/" + interface);
+    }
+  }
+  auto command_interfaces = traj_controller_->command_interface_configuration();
+  EXPECT_EQ(
+    command_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL);
+  EXPECT_EQ(
+    command_interfaces.names.size(), command_joint_names_.size() * command_interface_types_.size());
+  ASSERT_THAT(
+    command_interfaces.names, testing::UnorderedElementsAreArray(command_interface_names));
+}
+
 TEST_P(TrajectoryControllerTestParameterized, activate)
 {
   SetUpTrajectoryController();

joint_trajectory_controller/test/test_trajectory_controller_utils.hpp

@@ -81,6 +81,11 @@ class TestableJointTrajectoryController
 
   void set_joint_names(const std::vector<std::string> & joint_names) { joint_names_ = joint_names; }
 
+  void set_command_joint_names(const std::vector<std::string> & command_joint_names)
+  {
+    command_joint_names_ = command_joint_names;
+  }
+
   void set_command_interfaces(const std::vector<std::string> & command_interfaces)
   {
     command_interface_types_ = command_interfaces;
@@ -115,6 +120,8 @@ class TrajectoryControllerTest : public ::testing::Test
     controller_name_ = "test_joint_trajectory_controller";
 
     joint_names_ = {"joint1", "joint2", "joint3"};
+    command_joint_names_ = {
+      "following_controller/joint1", "following_controller/joint2", "following_controller/joint3"};
     joint_pos_.resize(joint_names_.size(), 0.0);
     joint_state_pos_.resize(joint_names_.size(), 0.0);
     joint_vel_.resize(joint_names_.size(), 0.0);
@@ -396,6 +403,7 @@ class TrajectoryControllerTest : public ::testing::Test
   std::string controller_name_;
 
   std::vector<std::string> joint_names_;
+  std::vector<std::string> command_joint_names_;
   std::vector<std::string> command_interface_types_;
   std::vector<std::string> state_interface_types_;