robotiq_gripper_command_controller.cpp build complete

src/robotiq_2f/robotiq_2f_controllers/CMakeLists.txt

@@ -17,6 +17,7 @@ include_directories(include)
 
 add_library(robotiq_2f_controllers SHARED
   src/robotiq_forward_controller.cpp
+  src/robotiq_gripper_command_controller.cpp
 )
 ament_target_dependencies(${PROJECT_NAME}
   rclcpp

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

@@ -7,11 +7,14 @@
 #include <memory>
 #include <string>
 #include <vector>
+#include <queue>
+#include <mutex>
 
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp/qos.hpp>
 #include <rclcpp_lifecycle/lifecycle_node.hpp>
 #include <rclcpp_action/rclcpp_action.hpp>
+#include <rclcpp/callback_group.hpp>
 
 #include <controller_interface/controller_interface.hpp>
 #include <hardware_interface/loaned_command_interface.hpp>
@@ -21,13 +24,15 @@
 
 #include "robotiq_2f_msgs/action/gripper_command.hpp"
 
+using GripperGoalHandle = rclcpp_action::ServerGoalHandle<robotiq_2f_msgs::action::GripperCommand>;
+
 namespace robotiq_2f_controllers
 {
 class RobotiqGripperCommandController : public controller_interface::ControllerInterface
 {
 public:
     CONTROLLER_INTERFACE_PUBLIC
-    RobotiqForwardController();
+    RobotiqGripperCommandController();
 
     CONTROLLER_INTERFACE_PUBLIC
     controller_interface::InterfaceConfiguration command_interface_configuration() const override;
@@ -67,19 +72,29 @@ public:
     const rclcpp_lifecycle::State & previous_state) override;
 
 protected:
+    std::mutex mutex_;
     std::string joint_name_;
     std::vector<std::string> command_interface_types_;
     std::vector<std::string> state_interface_types_;
 
-    //Checker Methods
-    bool check_if_stalled();
+    // Checker Methods
+    bool is_stalled(double current_position);
+    bool is_goal_valid(std::shared_ptr<const robotiq_2f_msgs::action::GripperCommand::Goal> goal);
+    bool is_stopped();
+    // Action Interface
+    std::queue<std::shared_ptr<GripperGoalHandle>> goal_queue_;
+    std::shared_ptr<GripperGoalHandle> current_goal_handle_;
+    rclcpp_action::Server<robotiq_2f_msgs::action::GripperCommand>::SharedPtr action_server_;
 
-    // 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_;
+    rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID & goal_uuid,
+         std::shared_ptr<const robotiq_2f_msgs::action::GripperCommand::Goal> goal);
+    rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr<GripperGoalHandle> goal_handle);
+    void handle_accepted(const std::shared_ptr<GripperGoalHandle> goal_handle);
+    void process_next_goal();
+    void execute_goal(const std::shared_ptr<GripperGoalHandle> goal_handle);
+
+    // Error Related
+    bool stop_motion(double current_position);
 
     // Command Interfaces
     std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
@@ -103,11 +118,18 @@ protected:
     state_interface_map_ = {
       {"position", &joint_position_state_interface_}};
 
-    double desired_position_;
-    double desired_max_velocity_;
-    double desired_max_effort_;
+    // Addtional State variables
+    double target_position_ = -1.0;
+    double last_position_ = -1.0;
+    int stall_counter_ = 0;
+    bool cancel_requested_ = false;
+
+    // Const
+    const int MAX_STALL_COUNT = 5;
+    const double POSITION_TOLERANCE = 0.01;
+    const double MIN_SPEED = 0.0;
+    const double MIN_EFFORT = 0.0;
 
-    double current_position_;
 };
 } // end namespace robotiq_2f_controllers
 #endif

src/robotiq_2f/robotiq_2f_controllers/src/robotiq_gripper_command_controller.cpp

@@ -5,7 +5,7 @@ using config_type = controller_interface::interface_configuration_type;
 
 namespace robotiq_2f_controllers
 {
-RobotiqForwardController::RobotiqGripperCommandController() : controller_interface::ControllerInterface() {}
+RobotiqGripperCommandController::RobotiqGripperCommandController() : controller_interface::ControllerInterface() {}
 
 controller_interface::CallbackReturn RobotiqGripperCommandController::on_init()
 {
@@ -62,16 +62,23 @@ controller_interface::InterfaceConfiguration RobotiqGripperCommandController::st
 controller_interface::CallbackReturn RobotiqGripperCommandController::on_configure(const rclcpp_lifecycle::State &)
 {
     this->action_server_ = rclcpp_action::create_server<robotiq_2f_msgs::action::GripperCommand>(
-        get_node(),
+        get_node()->get_node_base_interface(),
+        get_node()->get_node_clock_interface(),
+        get_node()->get_node_logging_interface(),
+        get_node()->get_node_waitables_interface(),
         "~/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)
+        [this](const std::array<unsigned char, 16>& uuid, std::shared_ptr<const robotiq_2f_msgs::action::GripperCommand::Goal> goal) {
+            return this->handle_goal(uuid, goal);
+        },
+        [this](std::shared_ptr<GripperGoalHandle> goal_handle) {return this->handle_cancel(goal_handle);},
+        [this](std::shared_ptr<GripperGoalHandle> goal_handle) {this->handle_accepted(goal_handle);},
+        rcl_action_server_get_default_options(),
+        get_node()->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive)
     );
     return CallbackReturn::SUCCESS;
 }
 
-controller_interface::CallbackReturn RobotiqGripperCommandController::on_activate(const rclcpp_lifecycle::State &)
+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_) {
@@ -94,50 +101,179 @@ controller_interface::CallbackReturn RobotiqGripperCommandController::on_activat
 
 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;
+    // Check if there is an active goal
+    if (current_goal_handle_) {
+
+        // Retrieve the current position of the gripper
+        double current_position = joint_position_state_interface_[0].get().get_value();
+
+        // Check if the gripper is stalled
+        if (is_stalled(current_position)) {
+            auto result = std::make_shared<robotiq_2f_msgs::action::GripperCommand::Result>();
+            result->final_position = current_position;
+            result->reached_goal = false;
+            result->stalled = true;
+            current_goal_handle_->abort(result);
+            current_goal_handle_.reset();  // Clear the current goal handle
+            process_next_goal();  // Process the next goal, if any
         }
 
-    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_);
+        // Check if the gripper has stopped after a cancel request
+        if (cancel_requested_ && is_stopped()) {
+        auto result = std::make_shared<robotiq_2f_msgs::action::GripperCommand::Result>();
+        result->final_position = current_position;
+        result->reached_goal = false;
+        result->stalled = false;
+        current_goal_handle_->canceled(result);  // Notify that the goal was canceled successfully
+        cancel_requested_ = false;  // Reset the cancel requested flag
+        current_goal_handle_.reset();  // Clear the current goal handle
+        process_next_goal();  // Proceed to the next goal if available
+
+        RCLCPP_INFO(get_node()->get_logger(), "Gripper Action was canceled and motion has stopped!");
+        }
+
+        // Publish feedback about the current position
+        auto feedback = std::make_shared<robotiq_2f_msgs::action::GripperCommand::Feedback>();
+        feedback->current_position = current_position;
+        current_goal_handle_->publish_feedback(feedback);
+
+        // Check if the target position is reached within a tolerance
+        if (std::abs(current_position - target_position_) < POSITION_TOLERANCE) {
+            auto result = std::make_shared<robotiq_2f_msgs::action::GripperCommand::Result>();
+            result->final_position = current_position;
+            result->reached_goal = true;
+            result->stalled = false;
+            current_goal_handle_->succeed(result);
+            current_goal_handle_.reset();  // Clear the current goal handle
+            process_next_goal();  // Process the next goal, if any
+            target_position_ = -1.0;
+        }
+    }
 
     return controller_interface::return_type::OK;
-    */
 }
 
 
-controller_interface::CallbackReturn RobotiqGripperCommandController::on_deactivate(const rclcpp_lifecycle::State &)
+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 &)
+controller_interface::CallbackReturn RobotiqGripperCommandController::on_cleanup(const rclcpp_lifecycle::State & )
 {
   return CallbackReturn::SUCCESS;
 }
 
-controller_interface::CallbackReturn RobotiqGripperCommandController::on_error(const rclcpp_lifecycle::State &)
+controller_interface::CallbackReturn RobotiqGripperCommandController::on_error(const rclcpp_lifecycle::State & )
+{
+    RCLCPP_ERROR(get_node()->get_logger(), "An error occurred, attempting to reset the controller");
+
+    // Attempt to stop any ongoing gripper motion as a safety precaution
+    try {
+        stop_motion(joint_position_state_interface_[0].get().get_value());  // Assume stop_motion sets the gripper commands to a safe state
+    } catch (const std::exception& e) {
+        RCLCPP_FATAL(get_node()->get_logger(), "Failed to stop the gripper during error handling: %s", e.what());
+        return controller_interface::CallbackReturn::ERROR;  // Fail if stopping motion fails
+    }
+
+    // Log that the controller is attempting to reactivate
+    RCLCPP_INFO(get_node()->get_logger(), "Attempting to reactivate the controller");
+
+    // Directly call the on_activate method to reset and reactivate the controller
+    return CallbackReturn::SUCCESS;
+}
+
+controller_interface::CallbackReturn RobotiqGripperCommandController::on_shutdown(const rclcpp_lifecycle::State & )
 {
   return CallbackReturn::SUCCESS;
 }
 
-controller_interface::CallbackReturn RobotiqGripperCommandController::on_shutdown(const rclcpp_lifecycle::State &)
+// Action Handling
+rclcpp_action::GoalResponse RobotiqGripperCommandController::handle_goal(const rclcpp_action::GoalUUID & goal_uuid,
+     std::shared_ptr<const robotiq_2f_msgs::action::GripperCommand::Goal> /*goal*/)
 {
-  return CallbackReturn::SUCCESS;
+        (void)goal_uuid;  // Suppress unused variable warning
+        RCLCPP_INFO(get_node()->get_logger(), "Received goal request");
+        return rclcpp_action::GoalResponse::ACCEPT_AND_DEFER;
 }
+
+rclcpp_action::CancelResponse RobotiqGripperCommandController::handle_cancel(const std::shared_ptr<GripperGoalHandle> goal_handle)
+{
+    if (current_goal_handle_ == goal_handle) {
+        RCLCPP_INFO(get_node()->get_logger(), "Received request to cancel goal");
+        stop_motion(joint_position_state_interface_[0].get().get_value());  // Attempt to stop the gripper immediately
+        cancel_requested_ = true;  // Set a flag indicating cancellation has been requested
+        return rclcpp_action::CancelResponse::ACCEPT;  // Accept the cancellation, pending completion
+    }
+    return rclcpp_action::CancelResponse::REJECT;  // Reject cancellation if it's not the current goal
+}
+
+void RobotiqGripperCommandController::handle_accepted(const std::shared_ptr<GripperGoalHandle> goal_handle)
+{
+    goal_queue_.push(goal_handle);
+    if (!current_goal_handle_) {  // Only start a new goal if one isn't already active
+        process_next_goal();
+    }
+}
+
+void RobotiqGripperCommandController::process_next_goal()
+{
+    std::lock_guard<std::mutex> lock(mutex_);
+    if (goal_queue_.empty()) {
+        current_goal_handle_.reset();
+        return;
+    }
+    current_goal_handle_ = goal_queue_.front();
+    goal_queue_.pop();
+
+    try {
+        auto goal = current_goal_handle_->get_goal();
+        target_position_ = goal->desired_position;
+        double desired_max_velocity = goal->max_velocity;
+        double desired_max_effort = goal->max_effort;
+
+        joint_position_command_interface_[0].get().set_value(target_position_);
+        max_velocity_command_interface_[0].get().set_value(desired_max_velocity);
+        max_effort_command_interface_[0].get().set_value(desired_max_effort);
+    } catch (const std::exception& e) {
+        RCLCPP_ERROR(get_node()->get_logger(), "Exception in process_next_goal: %s", e.what());
+    }
+}
+
+// Utility
+bool RobotiqGripperCommandController::is_stalled(double current_position)
+{
+    std::lock_guard<std::mutex> lock(mutex_);
+    if (std::abs(current_position - last_position_) < POSITION_TOLERANCE) {
+        stall_counter_++;
+    } else {
+        stall_counter_ = 0;  // Reset the stall counter if there is movement
+    }
+    last_position_ = current_position;
+
+return stall_counter_ > MAX_STALL_COUNT;  // Consider it stalled if no movement for too many cycles
+}
+
+bool RobotiqGripperCommandController::stop_motion(double current_position)
+{
+    joint_position_command_interface_[0].get().set_value(current_position);
+    max_velocity_command_interface_[0].get().set_value(MIN_SPEED);
+    max_effort_command_interface_[0].get().set_value(MIN_EFFORT);
+
+    return true;
+}
+
+bool RobotiqGripperCommandController::is_stopped()
+{
+    double current_position = joint_position_state_interface_[0].get().get_value();
+    bool stopped = std::abs(current_position - last_position_) < POSITION_TOLERANCE;
+    last_position_ = current_position;
+    return stopped;
+}
+
+
 }
 
 #include "pluginlib/class_list_macros.hpp"

src/robotiq_2f/robotiq_2f_msgs/action/GripperCommand.action

@@ -9,4 +9,4 @@ bool stalled                # True if the gripper is exerting max effort and not
 bool reached_goal           # True if the gripper position has reached the commanded setpoint
 ---
 # Feedback definition
-float64 current_progress    # in %
+float64 current_position    # in m