added homing service to franka hand node

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
 )
 

include/sas_robot_driver_franka/interfaces/qros_effector_driver_franka_hand.hpp

@@ -56,7 +56,7 @@
 #include <sas_robot_driver_franka_interfaces/srv/move.hpp>
 #include <sas_robot_driver_franka_interfaces/msg/gripper_state.hpp>
 #endif
-
+#include <std_srvs/srv/trigger.hpp>
 
 // using namespace DQ_robotics;
 // using namespace Eigen;
@@ -99,19 +99,25 @@ private:
     std::mutex gripper_in_use_;
     Service<srv::Grasp>::SharedPtr grasp_srv_;
     Service<srv::Move>::SharedPtr move_srv_;
+    Service<std_srvs::srv::Trigger>::SharedPtr homing_srv_;
 
 public:
 
     bool _grasp_srv_callback(
-        const std::shared_ptr<srv::Grasp::Request> req,
+        const std::shared_ptr<srv::Grasp::Request> &req,
         std::shared_ptr<srv::Grasp::Response> res
         );
 
     bool _move_srv_callback(
-        const std::shared_ptr<srv::Move::Request> req,
+        const std::shared_ptr<srv::Move::Request> &req,
         std::shared_ptr<srv::Move::Response> res
         );
 
+    bool _home_srv_callback(
+        const std::shared_ptr<std_srvs::srv::Trigger::Request> &req,
+        std::shared_ptr<std_srvs::srv::Trigger::Response> res
+        );
+
     EffectorDriverFrankaHand(const EffectorDriverFrankaHand&)=delete;
     EffectorDriverFrankaHand()=delete;
     ~EffectorDriverFrankaHand();

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<srv::Move>(driver_node_prefix_ + "/move",
             std::bind(&EffectorDriverFrankaHand::_move_srv_callback, this, _1, _2));
+        homing_srv_ = node->create_service<std_srvs::srv::Trigger>(driver_node_prefix_ + "/homing",
+            std::bind(&EffectorDriverFrankaHand::_home_srv_callback, this, _1, _2));
         gripper_status_publisher_ = node->create_publisher<msg::GripperState>(
             driver_node_prefix_ + "/gripper_status", 1);
     }
@@ -177,7 +179,7 @@ namespace qros
     }
 
 
-    bool EffectorDriverFrankaHand::_grasp_srv_callback(const std::shared_ptr<srv::Grasp::Request> req, std::shared_ptr<srv::Grasp::Response> res)
+    bool EffectorDriverFrankaHand::_grasp_srv_callback(const std::shared_ptr<srv::Grasp::Request> &req, std::shared_ptr<srv::Grasp::Response> 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<std::mutex> 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<srv::Move::Request> req, std::shared_ptr<srv::Move::Response> res)
+    bool EffectorDriverFrankaHand::_move_srv_callback(const std::shared_ptr<srv::Move::Request> &req, std::shared_ptr<srv::Move::Response> 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<std::mutex> 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<std_srvs::srv::Trigger::Request> &req, std::shared_ptr<std_srvs::srv::Trigger::Response> 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<std::mutex> 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;
     }