From 27c92ac6e62027eb51dd5fcb10b194a3a156be66 Mon Sep 17 00:00:00 2001
From: Johannes Mey <johannes.mey@tu-dresden.de>
Date: Fri, 10 Dec 2021 14:04:19 +0100
Subject: [PATCH] add homeing method

---
 include/grasp_util.h        |  7 +++++++
 src/grasping/grasp_util.cpp | 20 ++++++++++++++++++++
 2 files changed, 27 insertions(+)

diff --git a/include/grasp_util.h b/include/grasp_util.h
index 8953a48..66bdb7f 100644
--- a/include/grasp_util.h
+++ b/include/grasp_util.h
@@ -90,6 +90,13 @@ public:
     bool placeFromAbove(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &place_pose,
                         double open_amount, std::string supporting_surface_id, std::string object_to_place_id, bool plan_only = false);
 
+    /**
+     * Homes the gripper (in case it was abused before and is in an inconsistent state)
+     * This method is blocking with a maximum runtime of ~30s
+     * @return true if homing succeeded, false otherwise
+     */
+    static bool homeGripper();
+
 };
 
 #endif //PANDA_GRASPING_GRASP_UTIL_H
diff --git a/src/grasping/grasp_util.cpp b/src/grasping/grasp_util.cpp
index e8d90c2..d6ab94d 100644
--- a/src/grasping/grasp_util.cpp
+++ b/src/grasping/grasp_util.cpp
@@ -3,6 +3,8 @@
 //
 #include "grasp_util.h"
 #include <tf2/transform_datatypes.h>
+#include <franka_gripper/HomingAction.h>
+#include <franka_gripper/HomingGoal.h>
 
 void GraspUtil::setupOpenGripper(trajectory_msgs::JointTrajectory &posture, double amount) {
     posture.joint_names.resize(2);
@@ -158,3 +160,21 @@ GraspUtil::placeFromAbove(moveit::planning_interface::MoveGroupInterface &move_g
     ROS_ERROR_STREAM("Error while executing place task. MoveItErrorCode: " << miec.val);
     return false;
 }
+
+bool GraspUtil::homeGripper() {
+    actionlib::SimpleActionClient<franka_gripper::HomingAction> ac("/franka_gripper/homing", true);
+    if (!ac.waitForServer(ros::Duration(10.0))) {
+        ROS_ERROR("Gripper homing failed (action server not available).");
+        return false;
+    }
+    franka_gripper::HomingGoal goal;
+    ac.sendGoal(goal);
+    if (ac.waitForResult(ros::Duration(20.0))) {
+        actionlib::SimpleClientGoalState state = ac.getState();
+        ROS_INFO_STREAM("Homing finished: " <<  state.toString());
+        return state == actionlib::SimpleClientGoalState::SUCCEEDED;
+    } else {
+        ROS_ERROR("Gripper homing failed (with timeout).");
+        return false;
+    }
+}
-- 
GitLab