Fix all bugs in chained-controllers mode.

forward_command_controller/include/forward_command_controller/forward_controllers_base.hpp

@@ -59,7 +59,9 @@ class ForwardControllersBase
   CallbackReturn execute_init(const std::shared_ptr<rclcpp_lifecycle::LifecycleNode> & node);
 
   FORWARD_COMMAND_CONTROLLER_PUBLIC
-  CallbackReturn execute_configure(const rclcpp_lifecycle::State & previous_state);
+  CallbackReturn execute_configure(
+    const rclcpp_lifecycle::State & previous_state,
+    std::vector<hardware_interface::LoanedCommandInterface> & command_interfaces);
 
   FORWARD_COMMAND_CONTROLLER_PUBLIC
   CallbackReturn execute_activate(

forward_command_controller/src/chainable_forward_controller.cpp

@@ -37,7 +37,8 @@ CallbackReturn ChainableForwardController::on_init() { return execute_init(get_n
 CallbackReturn ChainableForwardController::on_configure(
   const rclcpp_lifecycle::State & previous_state)
 {
-  auto ret = execute_configure(previous_state);
+  auto ret = execute_configure(previous_state, command_interfaces_);
+  RCLCPP_ERROR(get_node()->get_logger(), "Configure is successful, now tryting configure ref_itfs");
   if (ret != CallbackReturn::SUCCESS)
   {
     return ret;
@@ -47,7 +48,12 @@ CallbackReturn ChainableForwardController::on_configure(
   reference_interface_names_ = command_interface_names_;
   // for any case make reference interfaces size of command interfaces
   reference_interfaces_.resize(
-    command_interfaces_.size(), std::numeric_limits<double>::quiet_NaN());
+    reference_interface_names_.size(), std::numeric_limits<double>::quiet_NaN());
+
+  for (const auto & value : reference_interfaces_)
+  {
+    RCLCPP_ERROR(get_node()->get_logger(), "Reference interface value is %f", value);
+  }
 
   return CallbackReturn::SUCCESS;
 }
@@ -93,8 +99,9 @@ CallbackReturn ChainableForwardController::on_activate(
     return ret;
   }
 
-  reference_interfaces_.resize(
-    command_interfaces_.size(), std::numeric_limits<double>::quiet_NaN());
+  std::fill(
+    reference_interfaces_.begin(), reference_interfaces_.end(),
+    std::numeric_limits<double>::quiet_NaN());
 
   return CallbackReturn::SUCCESS;
 }
@@ -117,14 +124,15 @@ controller_interface::return_type ChainableForwardController::update(
       reference_interfaces_ = (*joint_commands)->data;
     }
   }
-  RCLCPP_ERROR_THROTTLE(
-    get_node()->get_logger(), *(get_node()->get_clock()), 2000, "Reference interface value is %f",
-    reference_interfaces_[0]);
 
   for (size_t i = 0; i < command_interfaces_.size(); ++i)
   {
     if (!std::isnan(reference_interfaces_[i]))
     {
+      RCLCPP_ERROR(
+        get_node()->get_logger(), "Reference interface value is %f; Command interface name is '%s'",
+        reference_interfaces_[i], command_interfaces_[i].get_full_name().c_str());
+
       command_interfaces_[i].set_value(reference_interfaces_[i]);
     }
   }

forward_command_controller/src/forward_controller.cpp

@@ -39,7 +39,7 @@ CallbackReturn ForwardController::on_init() { return execute_init(get_node()); }
 
 CallbackReturn ForwardController::on_configure(const rclcpp_lifecycle::State & previous_state)
 {
-  return execute_configure(previous_state);
+  return execute_configure(previous_state, command_interfaces_);
 }
 
 CallbackReturn ForwardController::on_activate(const rclcpp_lifecycle::State & previous_state)

forward_command_controller/src/forward_controllers_base.cpp

@@ -51,12 +51,13 @@ CallbackReturn ForwardControllersBase::execute_init(
 }
 
 CallbackReturn ForwardControllersBase::execute_configure(
-  const rclcpp_lifecycle::State & /*previous_state*/)
+  const rclcpp_lifecycle::State & /*previous_state*/,
+  std::vector<hardware_interface::LoanedCommandInterface> & command_interfaces)
 {
   auto ret = this->read_parameters();
   if (ret != CallbackReturn::SUCCESS)
   {
-    RCLCPP_ERROR(get_node()->get_logger(), "Error when reading parameters.");
+    RCLCPP_ERROR(node_->get_logger(), "Error when reading parameters.");
     return ret;
   }
 
@@ -69,6 +70,9 @@ CallbackReturn ForwardControllersBase::execute_configure(
       }
     });
 
+  // pre-reserve command interfaces
+  command_interfaces.reserve(command_interface_names_.size());
+
   RCLCPP_INFO(node_->get_logger(), "configure successful");
   return CallbackReturn::SUCCESS;
 }

joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp

@@ -108,6 +108,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,7 @@ CallbackReturn JointTrajectoryController::on_init()
   {
     // with the lifecycle node being initialized, we can declare parameters
     auto_declare<std::vector<std::string>>("joints", joint_names_);
+    auto_declare<std::vector<std::string>>("command_joints", command_joint_names_);
     auto_declare<std::vector<std::string>>("command_interfaces", command_interface_types_);
     auto_declare<std::vector<std::string>>("state_interfaces", state_interface_types_);
     auto_declare<double>("state_publish_rate", 50.0);
@@ -79,8 +80,8 @@ JointTrajectoryController::command_interface_configuration() const
 {
   controller_interface::InterfaceConfiguration conf;
   conf.type = controller_interface::interface_configuration_type::INDIVIDUAL;
-  conf.names.reserve(joint_names_.size() * command_interface_types_.size());
-  for (const auto & joint_name : joint_names_)
+  conf.names.reserve(command_joint_names_.size() * command_interface_types_.size());
+  for (const auto & joint_name : command_joint_names_)
   {
     for (const auto & interface_type : command_interface_types_)
     {
@@ -436,16 +437,33 @@ CallbackReturn JointTrajectoryController::on_configure(const rclcpp_lifecycle::S
   // update parameters
   joint_names_ = node_->get_parameter("joints").as_string_array();
 
+  // 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())
   {
@@ -731,11 +749,11 @@ CallbackReturn JointTrajectoryController::on_activate(const rclcpp_lifecycle::St
       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(
-        node_->get_logger(), "Expected %zu '%s' command interfaces, got %zu.", joint_names_.size(),
-        interface.c_str(), joint_command_interface_[index].size());
+        node_->get_logger(), "Expected %zu '%s' command interfaces, got %zu.",
+        command_joint_names_.size(), interface.c_str(), joint_command_interface_[index].size());
       return CallbackReturn::ERROR;
     }
   }