Restructure command interface exporting

Make passthrough command interfaces part of the joints and fix claiming conflicts

Author: Felix Exner

ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp

@@ -70,51 +70,33 @@
 namespace ur_controllers
 {
 
+/*
+ * 0.0: No trajectory to forward, the controller is idling and ready to receive a new trajectory.
+ * 1.0: The controller has received and accepted a new trajecotry. When the state is 1.0, the controller will write a
+ * point to the hardware interface.
+ * 2.0: The hardware interface will read the point written from the controller. The state will switch between 1.0
+ * and 2.0 until all points have been read by the hardware interface.
+ * 3.0: The hardware interface has read all the points, and will now write all the points to the physical robot
+ * controller.
+ * 4.0: The robot is moving through the trajectory.
+ * 5.0: The robot finished executing the trajectory.
+ */
 const double TRANSFER_STATE_IDLE = 0.0;
 const double TRANSFER_STATE_WAITING_FOR_POINT = 1.0;
 const double TRANSFER_STATE_TRANSFERRING = 2.0;
 const double TRANSFER_STATE_TRANSFER_DONE = 3.0;
 const double TRANSFER_STATE_IN_MOTION = 4.0;
 const double TRANSFER_STATE_DONE = 5.0;
+
 using namespace std::chrono_literals;  // NOLINT
 
 enum CommandInterfaces
 {
-  /* The PASSTHROUGH_TRAJECTORY_TRANSFER_STATE value is used to keep track of which stage the transfer is in.
-  0.0: No trajectory to forward, the controller is idling and ready to receive a new trajectory.
-  1.0: The controller has received and accepted a new trajecotry. When the state is 1.0, the controller will write a
-  point to the hardware interface.
-  2.0: The hardware interface will read the point written from the controller. The state will switch between 1.0
-  and 2.0 until all points have been read by the hardware interface.
-  3.0: The hardware interface has read all the points, and will now write all the points to the physical robot
-  controller.
-  4.0: The robot is moving through the trajectory.
-  5.0: The robot finished executing the trajectory. */
-  PASSTHROUGH_TRAJECTORY_TRANSFER_STATE = 0,
-  /* The PASSTHROUGH_TRAJECTORY_ABORT value is used to indicate whether the trajectory has been cancelled from the
-   * hardware interface.*/
-  PASSTHROUGH_TRAJECTORY_ABORT = 1,
-  /* Arrays to hold the values of a point. */
-  PASSTHROUGH_TRAJECTORY_POSITIONS_ = 2,
-  PASSTHROUGH_TRAJECTORY_VELOCITIES_ = 8,
-  PASSTHROUGH_TRAJECTORY_ACCELERATIONS_ = 14,
-  PASSTHROUGH_TRAJECTORY_TIME_FROM_START = 20
-};
-
-enum StateInterfaces
-{
-  /* State interface 0 - 17 are joint state interfaces*/
-
-  SPEED_SCALING_FACTOR = 18,
-};
-
-// Struct to hold data that has to be transferred between realtime thread and non-realtime threads
-struct AsyncInfo
-{
-  double transfer_state = 0;
-  double abort = 0;
-  double controller_running = 0;
-  bool info_updated = false;
+  // The PASSTHROUGH_TRAJECTORY_TRANSFER_STATE value is used to keep track of which stage the transfer is in.
+  PASSTHROUGH_TRAJECTORY_TRANSFER_STATE = 18,
+  // The PASSTHROUGH_TRAJECTORY_ABORT value is used to indicate whether the trajectory has been cancelled from the
+  // hardware interface./
+  PASSTHROUGH_TRAJECTORY_TIME_FROM_START = 19
 };
 
 class PassthroughTrajectoryController : public controller_interface::ControllerInterface
@@ -199,6 +181,7 @@ class PassthroughTrajectoryController : public controller_interface::ControllerI
   static constexpr double NO_VAL = std::numeric_limits<double>::quiet_NaN();
 
   std::optional<std::reference_wrapper<hardware_interface::LoanedStateInterface>> scaling_state_interface_;
+  std::optional<std::reference_wrapper<hardware_interface::LoanedStateInterface>> abort_state_interface_;
 
   rclcpp::Clock::SharedPtr clock_;
 };

ur_controllers/src/passthrough_trajectory_controller.cpp

@@ -111,6 +111,9 @@ controller_interface::InterfaceConfiguration PassthroughTrajectoryController::st
 
   conf.names.push_back(passthrough_params_.speed_scaling_interface_name);
 
+  const std::string tf_prefix = passthrough_params_.tf_prefix;
+  conf.names.push_back(tf_prefix + "passthrough_controller/passthrough_trajectory_abort");
+
   return conf;
 }
 
@@ -121,23 +124,14 @@ controller_interface::InterfaceConfiguration PassthroughTrajectoryController::co
 
   const std::string tf_prefix = passthrough_params_.tf_prefix;
 
-  config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_trajectory_transfer_state");
-
-  config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_trajectory_abort");
-
-  for (size_t i = 0; i < number_of_joints_; ++i) {
-    config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_trajectory_positions_" +
-                              std::to_string(i));
-  }
-  for (size_t i = 0; i < number_of_joints_; ++i) {
-    config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_trajectory_velocities_" +
-                              std::to_string(i));
-  }
-  for (size_t i = 0; i < number_of_joints_; ++i) {
-    config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_trajectory_accelerations_" +
-                              std::to_string(i));
+  auto joint_names = passthrough_params_.joints;
+  for (auto& joint_name : joint_names) {
+    config.names.emplace_back(joint_name + "/passthrough_position");
+    config.names.emplace_back(joint_name + "/passthrough_velocity");
+    config.names.emplace_back(joint_name + "/passthrough_acceleration");
   }
 
+  config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_trajectory_transfer_state");
   config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_trajectory_time_from_start");
 
   return config;
@@ -175,6 +169,19 @@ controller_interface::CallbackReturn PassthroughTrajectoryController::on_activat
     return controller_interface::CallbackReturn::ERROR;
   }
 
+  {
+    const std::string interface_name = passthrough_params_.tf_prefix + "passthrough_controller/"
+                                                                       "passthrough_trajectory_abort";
+    auto it = std::find_if(state_interfaces_.begin(), state_interfaces_.end(),
+                           [&](auto& interface) { return (interface.get_name() == interface_name); });
+    if (it != state_interfaces_.end()) {
+      abort_state_interface_ = *it;
+    } else {
+      RCLCPP_ERROR(get_node()->get_logger(), "Did not find '%s' in state interfaces.", interface_name.c_str());
+      return controller_interface::CallbackReturn::ERROR;
+    }
+  }
+
   return ControllerInterface::on_activate(state);
 }
 
@@ -190,8 +197,8 @@ controller_interface::return_type PassthroughTrajectoryController::update(const
     if (current_transfer_state != TRANSFER_STATE_IDLE) {
       // Check if the trajectory has been aborted from the hardware interface. E.g. the robot was stopped on the teach
       // pendant.
-      if (command_interfaces_[PASSTHROUGH_TRAJECTORY_ABORT].get_value() == 1.0 && current_index_ > 0) {
-        RCLCPP_INFO(get_node()->get_logger(), "Trajectory aborted hardware, aborting action.");
+      if (abort_state_interface_->get().get_value() == 1.0 && current_index_ > 0) {
+        RCLCPP_INFO(get_node()->get_logger(), "Trajectory aborted by hardware, aborting action.");
         std::shared_ptr<control_msgs::action::FollowJointTrajectory::Result> result =
             std::make_shared<control_msgs::action::FollowJointTrajectory::Result>();
         active_goal->setAborted(result);
@@ -222,23 +229,25 @@ controller_interface::return_type PassthroughTrajectoryController::update(const
 
         // Write the positions for each joint of the robot
         auto joint_names_internal = joint_names_.readFromRT();
+        // We've added the joint interfaces matching the order of the joint names so we can safely access
+        // them by the index.
         for (size_t i = 0; i < number_of_joints_; i++) {
-          command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_POSITIONS_ + i].set_value(
+          command_interfaces_[i * 3].set_value(
               active_joint_traj_.points[current_index_].positions[joint_mapping->at(joint_names_internal->at(i))]);
           // Optionally, also write velocities and accelerations for each joint.
           if (active_joint_traj_.points[current_index_].velocities.size() > 0) {
-            command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_VELOCITIES_ + i].set_value(
+            command_interfaces_[i * 3 + 1].set_value(
                 active_joint_traj_.points[current_index_].velocities[joint_mapping->at(joint_names_internal->at(i))]);
+            if (active_joint_traj_.points[current_index_].accelerations.size() > 0) {
+              command_interfaces_[i * 3 + 2].set_value(
+                  active_joint_traj_.points[current_index_]
+                      .accelerations[joint_mapping->at(joint_names_internal->at(i))]);
+            } else {
+              command_interfaces_[i * 3 + 2].set_value(NO_VAL);
+            }
           } else {
-            command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_VELOCITIES_].set_value(NO_VAL);
-          }
-
-          if (active_joint_traj_.points[current_index_].accelerations.size() > 0) {
-            command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_ACCELERATIONS_ + i].set_value(
-                active_joint_traj_.points[current_index_]
-                    .accelerations[joint_mapping->at(joint_names_internal->at(i))]);
-          } else {
-            command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_ACCELERATIONS_ + i].set_value(NO_VAL);
+            command_interfaces_[i * 3 + 1].set_value(NO_VAL);
+            command_interfaces_[i * 3 + 2].set_value(NO_VAL);
           }
         }
         // Tell hardware interface that this point is ready to be read.

ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp

@@ -249,7 +249,7 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface
 
   urcl::RobotReceiveTimeout receive_timeout_ = urcl::RobotReceiveTimeout::millisec(20);
 
-  const std::string PASSTHROUGH_TRAJECTORY_CONTROLLER = "passthrough_controller/passthrough_trajectory_positions_";
+  const std::string PASSTHROUGH_TRAJECTORY_CONTROLLER = "passthrough";
 };
 }  // namespace ur_robot_driver
 

ur_robot_driver/src/hardware_interface.cpp

@@ -45,7 +45,7 @@
 #include "ur_client_library/exceptions.h"
 #include "ur_client_library/ur/tool_communication.h"
 
-#include "rclcpp/rclcpp.hpp"
+#include <rclcpp/logging.hpp>
 #include "hardware_interface/types/hardware_interface_type_values.hpp"
 #include "ur_robot_driver/hardware_interface.hpp"
 #include "ur_robot_driver/urcl_log_handler.hpp"
@@ -101,9 +101,9 @@ URPositionHardwareInterface::on_init(const hardware_interface::HardwareInfo& sys
   trajectory_joint_accelerations_.clear();
 
   for (const hardware_interface::ComponentInfo& joint : info_.joints) {
-    if (joint.command_interfaces.size() != 2) {
+    if (joint.command_interfaces.size() != 5) {
       RCLCPP_FATAL(rclcpp::get_logger("URPositionHardwareInterface"),
-                   "Joint '%s' has %zu command interfaces found. 2 expected.", joint.name.c_str(),
+                   "Joint '%s' has %zu command interfaces found. 5 expected.", joint.name.c_str(),
                    joint.command_interfaces.size());
       return hardware_interface::CallbackReturn::ERROR;
     }
@@ -236,6 +236,9 @@ std::vector<hardware_interface::StateInterface> URPositionHardwareInterface::exp
   state_interfaces.emplace_back(
       hardware_interface::StateInterface(tf_prefix + "gpio", "program_running", &robot_program_running_copy_));
 
+  state_interfaces.emplace_back(hardware_interface::StateInterface(
+      tf_prefix + "passthrough_controller", "passthrough_trajectory_abort", &passthrough_trajectory_abort_));
+
   return state_interfaces;
 }
 
@@ -248,6 +251,15 @@ std::vector<hardware_interface::CommandInterface> URPositionHardwareInterface::e
 
     command_interfaces.emplace_back(hardware_interface::CommandInterface(
         info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &urcl_velocity_commands_[i]));
+
+    command_interfaces.emplace_back(hardware_interface::CommandInterface(info_.joints[i].name, "passthrough_position",
+                                                                         &passthrough_trajectory_positions_[i]));
+
+    command_interfaces.emplace_back(hardware_interface::CommandInterface(info_.joints[i].name, "passthrough_velocity",
+                                                                         &passthrough_trajectory_velocities_[i]));
+
+    command_interfaces.emplace_back(hardware_interface::CommandInterface(
+        info_.joints[i].name, "passthrough_acceleration", &passthrough_trajectory_accelerations_[i]));
   }
   // Obtain the tf_prefix from the urdf so that we can have the general interface multiple times
   // NOTE using the tf_prefix at this point is some kind of workaround. One should actually go through the list of gpio
@@ -283,6 +295,8 @@ std::vector<hardware_interface::CommandInterface> URPositionHardwareInterface::e
       hardware_interface::CommandInterface(tf_prefix + "payload", "cog.z", &payload_center_of_gravity_[2]));
   command_interfaces.emplace_back(
       hardware_interface::CommandInterface(tf_prefix + "payload", "payload_async_success", &payload_async_success_));
+  command_interfaces.emplace_back(hardware_interface::CommandInterface(
+      tf_prefix + "passthrough_controller", "passthrough_trajectory_abort", &passthrough_trajectory_abort_));
 
   for (size_t i = 0; i < 18; ++i) {
     command_interfaces.emplace_back(hardware_interface::CommandInterface(
@@ -307,27 +321,6 @@ std::vector<hardware_interface::CommandInterface> URPositionHardwareInterface::e
                                                                        "passthrough_trajectory_transfer_state",
                                                                        &passthrough_trajectory_transfer_state_));
 
-  command_interfaces.emplace_back(hardware_interface::CommandInterface(
-      tf_prefix + "passthrough_controller", "passthrough_trajectory_abort", &passthrough_trajectory_abort_));
-
-  for (size_t i = 0; i < 6; ++i) {
-    command_interfaces.emplace_back(hardware_interface::CommandInterface(
-        tf_prefix + "passthrough_controller", "passthrough_trajectory_positions_" + std::to_string(i),
-        &passthrough_trajectory_positions_[i]));
-  }
-
-  for (size_t i = 0; i < 6; ++i) {
-    command_interfaces.emplace_back(hardware_interface::CommandInterface(
-        tf_prefix + "passthrough_controller", "passthrough_trajectory_velocities_" + std::to_string(i),
-        &passthrough_trajectory_velocities_[i]));
-  }
-
-  for (size_t i = 0; i < 6; ++i) {
-    command_interfaces.emplace_back(hardware_interface::CommandInterface(
-        tf_prefix + "passthrough_controller", "passthrough_trajectory_accelerations_" + std::to_string(i),
-        &passthrough_trajectory_accelerations_[i]));
-  }
-
   command_interfaces.emplace_back(hardware_interface::CommandInterface(tf_prefix + "passthrough_controller",
                                                                        "passthrough_trajectory_time_from_start",
                                                                        &passthrough_trajectory_time_from_start_));
@@ -853,20 +846,11 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod
       if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY) {
         start_modes_.push_back(hardware_interface::HW_IF_VELOCITY);
       }
-      if (key == tf_prefix + PASSTHROUGH_TRAJECTORY_CONTROLLER + std::to_string(i)) {
+      if (key == info_.joints[i].name + "/" + PASSTHROUGH_TRAJECTORY_CONTROLLER + "_position") {
         start_modes_.push_back(PASSTHROUGH_TRAJECTORY_CONTROLLER);
       }
     }
   }
-  // set new mode to all interfaces at the same time
-  if (start_modes_.size() != 0 && start_modes_.size() != 6) {
-    ret_val = hardware_interface::return_type::ERROR;
-  }
-
-  // all start interfaces must be the same - can't mix position and velocity control
-  if (start_modes_.size() != 0 && !std::equal(start_modes_.begin() + 1, start_modes_.end(), start_modes_.begin())) {
-    ret_val = hardware_interface::return_type::ERROR;
-  }
 
   // Stopping interfaces
   // add stop interface per joint in tmp var for later check
@@ -878,14 +862,61 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod
       if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY) {
         stop_modes_.push_back(StoppingInterface::STOP_VELOCITY);
       }
-      if (key == PASSTHROUGH_TRAJECTORY_CONTROLLER + std::to_string(i)) {
+      if (key == info_.joints[i].name + "/" + PASSTHROUGH_TRAJECTORY_CONTROLLER + "_position") {
         stop_modes_.push_back(StoppingInterface::STOP_PASSTHROUGH);
       }
     }
   }
+
+  // set new mode to all interfaces at the same time
+  if (start_modes_.size() != 0 && start_modes_.size() != 6) {
+    RCLCPP_ERROR(get_logger(), "Either none or all joint interfaces must be started at once.");
+    ret_val = hardware_interface::return_type::ERROR;
+  }
+
+  if (position_controller_running_ &&
+      std::none_of(stop_modes_.begin(), stop_modes_.end(),
+                   [](auto item) { return item == StoppingInterface::STOP_POSITION; }) &&
+      std::any_of(start_modes_.begin(), start_modes_.end(), [this](auto& item) {
+        return (item == hardware_interface::HW_IF_VELOCITY || item == PASSTHROUGH_TRAJECTORY_CONTROLLER);
+      })) {
+    RCLCPP_ERROR(get_logger(), "Start of velocity or passthrough interface requested while there is the position "
+                               "interface running.");
+    ret_val = hardware_interface::return_type::ERROR;
+  }
+
+  if (velocity_controller_running_ &&
+      std::none_of(stop_modes_.begin(), stop_modes_.end(),
+                   [](auto item) { return item == StoppingInterface::STOP_VELOCITY; }) &&
+      std::any_of(start_modes_.begin(), start_modes_.end(), [this](auto& item) {
+        return (item == hardware_interface::HW_IF_POSITION || item == PASSTHROUGH_TRAJECTORY_CONTROLLER);
+      })) {
+    RCLCPP_ERROR(get_logger(), "Start of position or passthrough interface requested while there is the velocity "
+                               "interface running.");
+    ret_val = hardware_interface::return_type::ERROR;
+  }
+
+  if (passthrough_trajectory_controller_running_ &&
+      std::none_of(stop_modes_.begin(), stop_modes_.end(),
+                   [](auto item) { return item == StoppingInterface::STOP_PASSTHROUGH; }) &&
+      std::any_of(start_modes_.begin(), start_modes_.end(), [](auto& item) {
+        return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION);
+      })) {
+    RCLCPP_ERROR(get_logger(), "Start of position or velocity interface requested while there is the passthrough "
+                               "interface running.");
+    ret_val = hardware_interface::return_type::ERROR;
+  }
+
+  // all start interfaces must be the same - can't mix position and velocity control
+  if (start_modes_.size() != 0 && !std::equal(start_modes_.begin() + 1, start_modes_.end(), start_modes_.begin())) {
+    RCLCPP_ERROR(get_logger(), "Can't mix different control domains for joint control.");
+    ret_val = hardware_interface::return_type::ERROR;
+  }
+
   // stop all interfaces at the same time
   if (stop_modes_.size() != 0 &&
       (stop_modes_.size() != 6 || !std::equal(stop_modes_.begin() + 1, stop_modes_.end(), stop_modes_.begin()))) {
+    RCLCPP_ERROR(get_logger(), "Either none or all joint interfaces must be stopped at once.");
     ret_val = hardware_interface::return_type::ERROR;
   }
 

ur_robot_driver/urdf/ur.ros2_control.xacro

@@ -227,6 +227,14 @@
       <gpio name="${tf_prefix}system_interface">
         <state_interface name="initialized"/>
       </gpio>
+
+      <gpio name="${tf_prefix}passthrough_controller">
+        <command_interface name="passthrough_trajectory_transfer_state"/>
+        <command_interface name="passthrough_trajectory_time_from_start"/>
+        <state_interface name="passthrough_trajectory_abort"/>
+      </gpio>
+
+
     </ros2_control>
   </xacro:macro>
 </robot>