Started with GripperCommand Controller

src/robotiq_2f/robotiq_2f_controllers/controller_plugin.xml

@@ -1,7 +1,12 @@
 <library path="robotiq_2f_controllers">
-    <class name="robotiq_2f_controllers/RobotiqForwardController" type="robotiq_2f_controllers::RobotiqForwardController" base_class_type="controller_interface::ControllerInterface">
+    <class name="robotiq_2f_controllers/ForwardController" type="robotiq_2f_controllers::RobotiqForwardController" base_class_type="controller_interface::ControllerInterface">
         <description>
             Forward controller for Robotiq 2F Gripper.
         </description>
     </class>
+        <class name="robotiq_2f_controllers/GripperCommand" type="robotiq_2f_controllers::RobotiqGripperCommandController" base_class_type="controller_interface::ControllerInterface">
+        <description>
+            Gripper Command controller for Robotiq 2F Gripper.
+        </description>
+    </class>
 </library>

src/robotiq_2f/robotiq_2f_controllers/include/robotiq_2f_controllers/robotiq_gripper_command_controller.hpp

@@ -0,0 +1,113 @@
+
+
+#ifndef ROBOTIQ_GRIPPER_COMMAND_CONTROLLER_HPP
+#define ROBOTIQ_GRIPPER_COMMAND_CONTROLLER_HPP
+
+#include <stddef.h>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp/qos.hpp>
+#include <rclcpp_lifecycle/lifecycle_node.hpp>
+#include <rclcpp_action/rclcpp_action.hpp>
+
+#include <controller_interface/controller_interface.hpp>
+#include <hardware_interface/loaned_command_interface.hpp>
+#include <hardware_interface/types/hardware_interface_type_values.hpp>
+
+#include "realtime_tools/realtime_buffer.h"
+
+#include "robotiq_2f_msgs/action/gripper_command.hpp"
+
+namespace robotiq_2f_controllers
+{
+class RobotiqGripperCommandController : public controller_interface::ControllerInterface
+{
+public:
+    CONTROLLER_INTERFACE_PUBLIC
+    RobotiqForwardController();
+
+    CONTROLLER_INTERFACE_PUBLIC
+    controller_interface::InterfaceConfiguration command_interface_configuration() const override;
+
+    CONTROLLER_INTERFACE_PUBLIC
+    controller_interface::InterfaceConfiguration state_interface_configuration() const override;
+
+    CONTROLLER_INTERFACE_PUBLIC
+    controller_interface::return_type update(
+    const rclcpp::Time & time, const rclcpp::Duration & period) override;
+
+    CONTROLLER_INTERFACE_PUBLIC
+    controller_interface::CallbackReturn on_init() override;
+
+    CONTROLLER_INTERFACE_PUBLIC
+    controller_interface::CallbackReturn on_configure(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+    CONTROLLER_INTERFACE_PUBLIC
+    controller_interface::CallbackReturn on_activate(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+    CONTROLLER_INTERFACE_PUBLIC
+    controller_interface::CallbackReturn on_deactivate(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+    CONTROLLER_INTERFACE_PUBLIC
+    controller_interface::CallbackReturn on_cleanup(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+    CONTROLLER_INTERFACE_PUBLIC
+    controller_interface::CallbackReturn on_error(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+    CONTROLLER_INTERFACE_PUBLIC
+    controller_interface::CallbackReturn on_shutdown(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+protected:
+    std::string joint_name_;
+    std::vector<std::string> command_interface_types_;
+    std::vector<std::string> state_interface_types_;
+
+    //Checker Methods
+    bool check_if_stalled();
+
+    // communication interface
+    bool new_action_ = false;
+    rclcpp::Subscription<robotiq_2f_msgs::action::GripperCommand>::SharedPtr command_action_server_;
+    std::shared_ptr<robotiq_2f_msgs::action::GripperCommand> command_action_;
+    realtime_tools::RealtimeBuffer<std::shared_ptr<robotiq_2f_msgs::action::GripperCommand>>
+    action_external_ptr_;
+
+    // Command Interfaces
+    std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
+    joint_position_command_interface_;
+    std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
+    max_velocity_command_interface_;
+    std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
+    max_effort_command_interface_;
+
+    std::unordered_map<std::string, std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> *>
+    command_interface_map_ = {
+      {"position", &joint_position_command_interface_},
+      {"max_velocity", &max_velocity_command_interface_},
+      {"max_effort", &max_effort_command_interface_}};
+
+    // State Interfaces
+    std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
+    joint_position_state_interface_;
+
+    std::unordered_map<std::string, std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> *>
+    state_interface_map_ = {
+      {"position", &joint_position_state_interface_}};
+
+    double desired_position_;
+    double desired_max_velocity_;
+    double desired_max_effort_;
+
+    double current_position_;
+};
+} // end namespace robotiq_2f_controllers
+#endif

src/robotiq_2f/robotiq_2f_controllers/src/robotiq_forward_controller.cpp

@@ -46,7 +46,7 @@ controller_interface::InterfaceConfiguration RobotiqForwardController::command_i
 
 controller_interface::InterfaceConfiguration RobotiqForwardController::state_interface_configuration() const {
     controller_interface::InterfaceConfiguration config;
-    config.type = controller_interface::interface_configuration_type::NONE;  // Or whatever is appropriate
+    config.type = controller_interface::interface_configuration_type::NONE;
     return config;
 }
 

src/robotiq_2f/robotiq_2f_controllers/src/robotiq_gripper_command_controller.cpp

@@ -0,0 +1,145 @@
+#include "robotiq_2f_controllers/robotiq_gripper_command_controller.hpp"
+
+
+using config_type = controller_interface::interface_configuration_type;
+
+namespace robotiq_2f_controllers
+{
+RobotiqForwardController::RobotiqGripperCommandController() : controller_interface::ControllerInterface() {}
+
+controller_interface::CallbackReturn RobotiqGripperCommandController::on_init()
+{
+    joint_name_ = auto_declare<std::string>("joint", joint_name_);
+    command_interface_types_ =
+    auto_declare<std::vector<std::string>>("command_interfaces", command_interface_types_);
+    state_interface_types_ =
+    auto_declare<std::vector<std::string>>("state_interfaces", state_interface_types_);
+
+
+    if (joint_name_.empty()) {
+        RCLCPP_ERROR(get_node()->get_logger(), "Missing 'joint_name' parameter.");
+        return CallbackReturn::ERROR;
+    }
+    else{
+        RCLCPP_INFO(get_node()->get_logger(), "Joint name: %s.", joint_name_.c_str());
+    }
+
+    return CallbackReturn::SUCCESS;
+}
+
+controller_interface::InterfaceConfiguration RobotiqGripperCommandController::command_interface_configuration() const
+{
+    RCLCPP_DEBUG(get_node()->get_logger(), "Starting configuration of command interfaces.");
+
+    controller_interface::InterfaceConfiguration config;
+    config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+
+    for (const auto& interface_map_entry : command_interface_map_) {
+        const std::string& interface_type = interface_map_entry.first;
+        config.names.push_back(joint_name_ + "/" + interface_type);
+    }
+    RCLCPP_DEBUG(get_node()->get_logger(), "Completed configuration of command interfaces.");
+    return config;
+}
+
+
+controller_interface::InterfaceConfiguration RobotiqGripperCommandController::state_interface_configuration() const
+{
+    RCLCPP_DEBUG(get_node()->get_logger(), "Starting configuration of state interfaces.");
+
+    controller_interface::InterfaceConfiguration config;
+    config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+
+    for (const auto& interface_map_entry : state_interface_map_) {
+        const std::string& interface_type = interface_map_entry.first;
+        config.names.push_back(joint_name_ + "/" + interface_type);
+    }
+
+    RCLCPP_DEBUG(get_node()->get_logger(), "Completed configuration of state interfaces.");
+    return config;
+}
+
+controller_interface::CallbackReturn RobotiqGripperCommandController::on_configure(const rclcpp_lifecycle::State &)
+{
+    this->action_server_ = rclcpp_action::create_server<robotiq_2f_msgs::action::GripperCommand>(
+        get_node(),
+        "~/gripper_command",
+        std::bind(&RobotiqGripperCommandController::handle_goal, this, std::placeholders::_1, std::placeholders::_2),
+        std::bind(&RobotiqGripperCommandController::handle_cancel, this, std::placeholders::_1),
+        std::bind(&RobotiqGripperCommandController::handle_accepted, this, std::placeholders::_1)
+    );
+    return CallbackReturn::SUCCESS;
+}
+
+controller_interface::CallbackReturn RobotiqGripperCommandController::on_activate(const rclcpp_lifecycle::State &)
+{
+    RCLCPP_INFO(get_node()->get_logger(), "Available command interfaces:");
+    for (const auto& interface : command_interfaces_) {
+        RCLCPP_INFO(get_node()->get_logger(), "Interface Name: %s, Interface Type: %s", interface.get_name().c_str(), interface.get_interface_name().c_str());
+    }
+
+    // clear out vectors in case of restart
+    joint_position_command_interface_.clear();
+    max_velocity_command_interface_.clear();
+    max_effort_command_interface_.clear();
+
+    // assign command interfaces
+    for (auto & interface : command_interfaces_)
+    {
+        command_interface_map_[interface.get_interface_name()]->push_back(interface);
+    }
+
+    return CallbackReturn::SUCCESS;
+}
+
+controller_interface::return_type RobotiqGripperCommandController::update(const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/)
+{
+    /*
+    if (new_msg_)
+    {
+        command_msg_ = *msg_external_ptr_.readFromRT();
+        desired_position_ = command_msg_->position;
+        desired_max_velocity_ = command_msg_->max_velocity;
+        desired_max_effort_ = command_msg_->max_effort;
+    }
+    // Set the hardware interface commands
+    if (joint_position_command_interface_.empty()) {
+        RCLCPP_FATAL(get_node()->get_logger(), "Command interface is not available.");
+        return controller_interface::return_type::ERROR;
+    }
+
+    joint_position_command_interface_[0].get().set_value(desired_position_);
+    max_velocity_command_interface_[0].get().set_value(desired_max_velocity_);
+    max_effort_command_interface_[0].get().set_value(desired_max_effort_);
+
+    return controller_interface::return_type::OK;
+    */
+}
+
+
+controller_interface::CallbackReturn RobotiqGripperCommandController::on_deactivate(const rclcpp_lifecycle::State &)
+{
+  release_interfaces();
+
+  return CallbackReturn::SUCCESS;
+}
+
+controller_interface::CallbackReturn RobotiqGripperCommandController::on_cleanup(const rclcpp_lifecycle::State &)
+{
+  return CallbackReturn::SUCCESS;
+}
+
+controller_interface::CallbackReturn RobotiqGripperCommandController::on_error(const rclcpp_lifecycle::State &)
+{
+  return CallbackReturn::SUCCESS;
+}
+
+controller_interface::CallbackReturn RobotiqGripperCommandController::on_shutdown(const rclcpp_lifecycle::State &)
+{
+  return CallbackReturn::SUCCESS;
+}
+}
+
+#include "pluginlib/class_list_macros.hpp"
+PLUGINLIB_EXPORT_CLASS(robotiq_2f_controllers::RobotiqGripperCommandController, controller_interface::ControllerInterface)
+

src/robotiq_2f/robotiq_2f_description/config/robotiq_controllers.yaml

@@ -4,7 +4,7 @@ controller_manager:
     joint_state_broadcaster:
       type: joint_state_broadcaster/JointStateBroadcaster
     forward_gripper_position_controller:
-      type: robotiq_2f_controllers/RobotiqForwardController
+      type: robotiq_2f_controllers/ForwardController
     robotiq_activation_controller:
       type: robotiq_controllers/RobotiqActivationController
 

src/robotiq_2f/robotiq_2f_msgs/CMakeLists.txt

@@ -9,12 +9,14 @@ endif()
 find_package(ament_cmake REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
 find_package(builtin_interfaces REQUIRED)
+find_package(action_msgs REQUIRED)
 
 find_package(rosidl_default_generators REQUIRED)
 rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/ForwardCommand.msg"
+  "action/GripperCommand.action"
   # Add more messages here
-  DEPENDENCIES builtin_interfaces
+  DEPENDENCIES builtin_interfaces action_msgs
 )
 
 ament_export_dependencies(rosidl_default_runtime)

src/robotiq_2f/robotiq_2f_msgs/action/GripperCommand.action

@@ -0,0 +1,12 @@
+# Goal definition
+float64 desired_position    # in m
+float64 max_velocity        # in m/s
+float64 max_effort          # in Newtons
+---
+# Result definition
+float64 final_position      # The final gripper gap size (in meters)
+bool stalled                # True if the gripper is exerting max effort and not moving
+bool reached_goal           # True if the gripper position has reached the commanded setpoint
+---
+# Feedback definition
+float64 current_progress    # in %

src/robotiq_2f/robotiq_2f_msgs/package.xml

@@ -11,6 +11,7 @@
 
   <depend>std_msgs</depend>
   <depend>builtin_interfaces</depend>
+  <depend>action_msgs</depend>
 
   <build_depend>rosidl_default_generators</build_depend>
   <exec_depend>rosidl_default_runtime</exec_depend>