From 787228d507a5ac772e750deeb24a620285810c06 Mon Sep 17 00:00:00 2001 From: qlin960618 Date: Tue, 10 Dec 2024 23:44:07 +0900 Subject: [PATCH] added homing service to franka hand node --- CMakeLists.txt | 3 +- .../qros_effector_driver_franka_hand.hpp | 12 +++-- src/hand/qros_effector_driver_franka_hand.cpp | 44 ++++++++++++++++--- 3 files changed, 49 insertions(+), 10 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 68532be..d7036ba 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -14,6 +14,7 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(geometry_msgs REQUIRED) find_package(std_msgs REQUIRED) +find_package(std_srvs REQUIRED) find_package(sas_common REQUIRED) find_package(sas_core REQUIRED) find_package(sas_msgs REQUIRED) @@ -156,7 +157,7 @@ target_link_libraries(sas_robot_driver_franka add_library(qros_effector_driver_franka_hand src/hand/qros_effector_driver_franka_hand.cpp) ament_target_dependencies(qros_effector_driver_franka_hand - rclcpp sas_common sas_core + rclcpp sas_common sas_core std_srvs sas_robot_driver_franka_interfaces ) diff --git a/include/sas_robot_driver_franka/interfaces/qros_effector_driver_franka_hand.hpp b/include/sas_robot_driver_franka/interfaces/qros_effector_driver_franka_hand.hpp index 916e3b6..e26bcc0 100644 --- a/include/sas_robot_driver_franka/interfaces/qros_effector_driver_franka_hand.hpp +++ b/include/sas_robot_driver_franka/interfaces/qros_effector_driver_franka_hand.hpp @@ -56,7 +56,7 @@ #include #include #endif - +#include // using namespace DQ_robotics; // using namespace Eigen; @@ -99,19 +99,25 @@ private: std::mutex gripper_in_use_; Service::SharedPtr grasp_srv_; Service::SharedPtr move_srv_; + Service::SharedPtr homing_srv_; public: bool _grasp_srv_callback( - const std::shared_ptr req, + const std::shared_ptr &req, std::shared_ptr res ); bool _move_srv_callback( - const std::shared_ptr req, + const std::shared_ptr &req, std::shared_ptr res ); + bool _home_srv_callback( + const std::shared_ptr &req, + std::shared_ptr res + ); + EffectorDriverFrankaHand(const EffectorDriverFrankaHand&)=delete; EffectorDriverFrankaHand()=delete; ~EffectorDriverFrankaHand(); diff --git a/src/hand/qros_effector_driver_franka_hand.cpp b/src/hand/qros_effector_driver_franka_hand.cpp index 2dee779..0c514bf 100644 --- a/src/hand/qros_effector_driver_franka_hand.cpp +++ b/src/hand/qros_effector_driver_franka_hand.cpp @@ -63,6 +63,8 @@ namespace qros std::bind(&EffectorDriverFrankaHand::_grasp_srv_callback, this, _1, _2)); move_srv_ = node->create_service(driver_node_prefix_ + "/move", std::bind(&EffectorDriverFrankaHand::_move_srv_callback, this, _1, _2)); + homing_srv_ = node->create_service(driver_node_prefix_ + "/homing", + std::bind(&EffectorDriverFrankaHand::_home_srv_callback, this, _1, _2)); gripper_status_publisher_ = node->create_publisher( driver_node_prefix_ + "/gripper_status", 1); } @@ -177,7 +179,7 @@ namespace qros } - bool EffectorDriverFrankaHand::_grasp_srv_callback(const std::shared_ptr req, std::shared_ptr res) + bool EffectorDriverFrankaHand::_grasp_srv_callback(const std::shared_ptr &req, std::shared_ptr res) { RCLCPP_DEBUG_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_grasp_srv_callback::Grasping..."); auto force = req->force; @@ -192,13 +194,13 @@ namespace qros RCLCPP_DEBUG_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_grasp_srv_callback::force: " + std::to_string(force) + " speed: " + std::to_string(speed)); bool ret = false; bool function_ret = true; - gripper_in_use_.lock(); #ifdef IN_TESTING ret = true; std::this_thread::sleep_for(std::chrono::milliseconds(2000)); #else try { + std::lock_guard lock(gripper_in_use_); ret = gripper_sptr_->grasp(req->width, speed, force, epsilon_inner, epsilon_outer); }catch(franka::CommandException& e) { @@ -210,13 +212,12 @@ namespace qros function_ret = false; } #endif - gripper_in_use_.unlock(); res->set__success(ret); return function_ret; } - bool EffectorDriverFrankaHand::_move_srv_callback(const std::shared_ptr req, std::shared_ptr res) + bool EffectorDriverFrankaHand::_move_srv_callback(const std::shared_ptr &req, std::shared_ptr res) { RCLCPP_DEBUG_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_move_srv_callback::Moving..."); auto speed = req->speed; @@ -224,13 +225,13 @@ namespace qros RCLCPP_DEBUG_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_move_srv_callback::Speed: " + std::to_string(speed) + " Width: " + std::to_string(req->width)); bool ret = false; bool function_ret = true; - gripper_in_use_.lock(); #ifdef IN_TESTING ret = true; std::this_thread::sleep_for(std::chrono::milliseconds(2000)); #else try { + std::lock_guard lock(gripper_in_use_); ret = gripper_sptr_->move(req->width, speed); }catch(franka::CommandException& e) { @@ -242,7 +243,38 @@ namespace qros function_ret = false; } #endif - gripper_in_use_.unlock(); + res->set__success(ret); + return function_ret; + } + + + bool EffectorDriverFrankaHand::_home_srv_callback(const std::shared_ptr &req, std::shared_ptr res) + { + RCLCPP_DEBUG_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_home_srv_callback::Homing..."); + bool ret = false; + bool function_ret = true; +#ifdef IN_TESTING + ret = true; + std::this_thread::sleep_for(std::chrono::milliseconds(2000)); +#else + try + { + std::lock_guard lock(gripper_in_use_); + ret = gripper_sptr_->homing(); + }catch(franka::CommandException& e) + { + RCLCPP_ERROR_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_home_srv_callback::CommandException::" + std::string(e.what())); + res->set__message("_home_srv_callback::CommandException::" + std::string(e.what())); + function_ret = false; + ret = false; + }catch(franka::NetworkException& e) + { + RCLCPP_ERROR_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_home_srv_callback::NetworkException::" + std::string(e.what())); + res->set__message("_home_srv_callback::NetworkException::" + std::string(e.what())); + function_ret = false; + ret = false; + } +#endif res->set__success(ret); return function_ret; }