diff --git a/src/robotiq_2f/robotiq_2f_controllers/CMakeLists.txt b/src/robotiq_2f/robotiq_2f_controllers/CMakeLists.txt index 310a3b6..b0731c8 100644 --- a/src/robotiq_2f/robotiq_2f_controllers/CMakeLists.txt +++ b/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 diff --git a/src/robotiq_2f/robotiq_2f_controllers/include/robotiq_2f_controllers/robotiq_gripper_command_controller.hpp b/src/robotiq_2f/robotiq_2f_controllers/include/robotiq_2f_controllers/robotiq_gripper_command_controller.hpp index 2d190da..1c30876 100644 --- a/src/robotiq_2f/robotiq_2f_controllers/include/robotiq_2f_controllers/robotiq_gripper_command_controller.hpp +++ b/src/robotiq_2f/robotiq_2f_controllers/include/robotiq_2f_controllers/robotiq_gripper_command_controller.hpp @@ -7,11 +7,14 @@ #include #include #include +#include +#include #include #include #include #include +#include #include #include @@ -21,13 +24,15 @@ #include "robotiq_2f_msgs/action/gripper_command.hpp" +using GripperGoalHandle = rclcpp_action::ServerGoalHandle; + 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 command_interface_types_; std::vector state_interface_types_; - //Checker Methods - bool check_if_stalled(); + // Checker Methods + bool is_stalled(double current_position); + bool is_goal_valid(std::shared_ptr goal); + bool is_stopped(); + // Action Interface + std::queue> goal_queue_; + std::shared_ptr current_goal_handle_; + rclcpp_action::Server::SharedPtr action_server_; - // communication interface - bool new_action_ = false; - rclcpp::Subscription::SharedPtr command_action_server_; - std::shared_ptr command_action_; - realtime_tools::RealtimeBuffer> - action_external_ptr_; + rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID & goal_uuid, + std::shared_ptr goal); + rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr goal_handle); + void handle_accepted(const std::shared_ptr goal_handle); + void process_next_goal(); + void execute_goal(const std::shared_ptr goal_handle); + + // Error Related + bool stop_motion(double current_position); // Command Interfaces std::vector> @@ -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 diff --git a/src/robotiq_2f/robotiq_2f_controllers/src/robotiq_gripper_command_controller.cpp b/src/robotiq_2f/robotiq_2f_controllers/src/robotiq_gripper_command_controller.cpp index 9137453..6d01417 100644 --- a/src/robotiq_2f/robotiq_2f_controllers/src/robotiq_gripper_command_controller.cpp +++ b/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( - 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& uuid, std::shared_ptr goal) { + return this->handle_goal(uuid, goal); + }, + [this](std::shared_ptr goal_handle) {return this->handle_cancel(goal_handle);}, + [this](std::shared_ptr 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_) { - 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_); + // 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(); + 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 + } + + // Check if the gripper has stopped after a cancel request + if (cancel_requested_ && is_stopped()) { + auto result = std::make_shared(); + 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(); + 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(); + 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 /*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 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 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 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 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" diff --git a/src/robotiq_2f/robotiq_2f_msgs/action/GripperCommand.action b/src/robotiq_2f/robotiq_2f_msgs/action/GripperCommand.action index 7553e30..272f3a3 100644 --- a/src/robotiq_2f/robotiq_2f_msgs/action/GripperCommand.action +++ b/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 % \ No newline at end of file +float64 current_position # in m \ No newline at end of file