Merge remote-tracking branch 'public/master' into feature/grasp_by_side

include/grasp_util.h

@@ -179,4 +179,12 @@ private:
                         tf2::Quaternion approach_orientation, float y_distance_diff, float x_distance_diff, double reach = FRANKA_GRIPPER_FINGERTIP_SIZE / 2);
 };
 
+
+    /**
+     * 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);
@@ -333,3 +335,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;
+    }
+}