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

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;
+    }
+}