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

…for following controllers in a chain.

joint_trajectory_controller/CMakeLists.txt

@@ -51,15 +51,15 @@ install(TARGETS ${PROJECT_NAME}
 )
 
 if(BUILD_TESTING)
-  find_package(ament_cmake_gtest REQUIRED)
+  find_package(ament_cmake_gmock REQUIRED)
   find_package(controller_manager REQUIRED)
   find_package(ros2_control_test_assets REQUIRED)
 
-  ament_add_gtest(test_trajectory test/test_trajectory.cpp)
+  ament_add_gmock(test_trajectory test/test_trajectory.cpp)
   target_include_directories(test_trajectory PRIVATE include)
   target_link_libraries(test_trajectory ${PROJECT_NAME})
 
-  ament_add_gtest(test_trajectory_controller
+  ament_add_gmock(test_trajectory_controller
     test/test_trajectory_controller.cpp
     ENV config_file=${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_joint_trajectory_controller.yaml)
   set_tests_properties(test_trajectory_controller PROPERTIES TIMEOUT 220)
@@ -71,7 +71,7 @@ if(BUILD_TESTING)
     ${THIS_PACKAGE_INCLUDE_DEPENDS}
   )
 
-  ament_add_gtest(
+  ament_add_gmock(
     test_load_joint_trajectory_controller
     test/test_load_joint_trajectory_controller.cpp
   )
@@ -84,7 +84,7 @@ if(BUILD_TESTING)
   )
 
   # TODO(andyz): Disabled due to flakiness
-  # ament_add_gtest(
+  # ament_add_gmock(
   #   test_trajectory_actions
   #   test/test_trajectory_actions.cpp
   # )

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/package.xml

@@ -27,7 +27,7 @@
   <depend>rclcpp_lifecycle</depend>
   <depend>trajectory_msgs</depend>
 
-  <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>ament_cmake_gmock</test_depend>
   <test_depend>controller_manager</test_depend>
   <test_depend>ros2_control_test_assets</test_depend>
 

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_interface_types_);
     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_)
     {
@@ -487,16 +489,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())
   {
@@ -790,7 +809,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 <gtest/gtest.h>
 #include <memory>
 
+#include "gtest/gtest.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

@@ -25,7 +25,7 @@
 #include <thread>
 #include <vector>
 
-#include "gtest/gtest.h"
+#include "gmock/gmock.h"
 
 #include "builtin_interfaces/msg/duration.hpp"
 #include "builtin_interfaces/msg/time.hpp"
@@ -91,6 +91,87 @@ TEST_P(TrajectoryControllerTestParameterized, configure)
   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 : 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);
@@ -394,6 +401,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_;