From 71643c7f8009d79005bc09af0cf9af39ab98ea9a Mon Sep 17 00:00:00 2001 From: Niko Date: Wed, 17 Apr 2024 14:25:48 +0200 Subject: [PATCH] Started with GripperCommand Controller --- .../controller_plugin.xml | 7 +- .../robotiq_gripper_command_controller.hpp | 113 ++++++++++++++ .../src/robotiq_forward_controller.cpp | 2 +- .../robotiq_gripper_command_controller.cpp | 145 ++++++++++++++++++ .../config/robotiq_controllers.yaml | 2 +- src/robotiq_2f/robotiq_2f_msgs/CMakeLists.txt | 4 +- .../action/GripperCommand.action | 12 ++ src/robotiq_2f/robotiq_2f_msgs/package.xml | 1 + 8 files changed, 282 insertions(+), 4 deletions(-) create mode 100644 src/robotiq_2f/robotiq_2f_msgs/action/GripperCommand.action diff --git a/src/robotiq_2f/robotiq_2f_controllers/controller_plugin.xml b/src/robotiq_2f/robotiq_2f_controllers/controller_plugin.xml index 98e2bdd..dad6fbc 100644 --- a/src/robotiq_2f/robotiq_2f_controllers/controller_plugin.xml +++ b/src/robotiq_2f/robotiq_2f_controllers/controller_plugin.xml @@ -1,7 +1,12 @@ - + Forward controller for Robotiq 2F Gripper. + + + Gripper Command controller for Robotiq 2F Gripper. + + \ No newline at end of file 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 e69de29..2d190da 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 @@ -0,0 +1,113 @@ + + +#ifndef ROBOTIQ_GRIPPER_COMMAND_CONTROLLER_HPP +#define ROBOTIQ_GRIPPER_COMMAND_CONTROLLER_HPP + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +#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 command_interface_types_; + std::vector state_interface_types_; + + //Checker Methods + bool check_if_stalled(); + + // communication interface + bool new_action_ = false; + rclcpp::Subscription::SharedPtr command_action_server_; + std::shared_ptr command_action_; + realtime_tools::RealtimeBuffer> + action_external_ptr_; + + // Command Interfaces + std::vector> + joint_position_command_interface_; + std::vector> + max_velocity_command_interface_; + std::vector> + max_effort_command_interface_; + + std::unordered_map> *> + command_interface_map_ = { + {"position", &joint_position_command_interface_}, + {"max_velocity", &max_velocity_command_interface_}, + {"max_effort", &max_effort_command_interface_}}; + + // State Interfaces + std::vector> + joint_position_state_interface_; + + std::unordered_map> *> + 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 diff --git a/src/robotiq_2f/robotiq_2f_controllers/src/robotiq_forward_controller.cpp b/src/robotiq_2f/robotiq_2f_controllers/src/robotiq_forward_controller.cpp index 9ddc007..16b1dd5 100644 --- a/src/robotiq_2f/robotiq_2f_controllers/src/robotiq_forward_controller.cpp +++ b/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; } 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 e69de29..9137453 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 @@ -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("joint", joint_name_); + command_interface_types_ = + auto_declare>("command_interfaces", command_interface_types_); + state_interface_types_ = + auto_declare>("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( + 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) + diff --git a/src/robotiq_2f/robotiq_2f_description/config/robotiq_controllers.yaml b/src/robotiq_2f/robotiq_2f_description/config/robotiq_controllers.yaml index 215d737..7b5ead7 100644 --- a/src/robotiq_2f/robotiq_2f_description/config/robotiq_controllers.yaml +++ b/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 diff --git a/src/robotiq_2f/robotiq_2f_msgs/CMakeLists.txt b/src/robotiq_2f/robotiq_2f_msgs/CMakeLists.txt index 6abf2a0..386aecf 100644 --- a/src/robotiq_2f/robotiq_2f_msgs/CMakeLists.txt +++ b/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) diff --git a/src/robotiq_2f/robotiq_2f_msgs/action/GripperCommand.action b/src/robotiq_2f/robotiq_2f_msgs/action/GripperCommand.action new file mode 100644 index 0000000..7553e30 --- /dev/null +++ b/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 % \ No newline at end of file diff --git a/src/robotiq_2f/robotiq_2f_msgs/package.xml b/src/robotiq_2f/robotiq_2f_msgs/package.xml index de5b5cf..c0161f7 100644 --- a/src/robotiq_2f/robotiq_2f_msgs/package.xml +++ b/src/robotiq_2f/robotiq_2f_msgs/package.xml @@ -11,6 +11,7 @@ std_msgs builtin_interfaces + action_msgs rosidl_default_generators rosidl_default_runtime