From 7bc0919815fbcdef192fbf67bb667754b7dbe64c Mon Sep 17 00:00:00 2001
From: Johannes Mey <johannes.mey@tu-dresden.de>
Date: Tue, 26 Apr 2022 09:43:26 +0200
Subject: [PATCH] minor improvements and reduced timeouts

---
 src/grasping/franka_gripper_util.cpp |  2 +-
 src/grasping/grasp_util.cpp          | 10 +++++++++-
 2 files changed, 10 insertions(+), 2 deletions(-)

diff --git a/src/grasping/franka_gripper_util.cpp b/src/grasping/franka_gripper_util.cpp
index 08a458f..66f1e22 100644
--- a/src/grasping/franka_gripper_util.cpp
+++ b/src/grasping/franka_gripper_util.cpp
@@ -24,7 +24,7 @@ void FrankaGripperUtil::resetGripperForNextAction() {
     ac.sendGoal(goal);
 
     //wait for the action to return
-    bool finished_before_timeout = ac.waitForResult(ros::Duration(30.0));
+    bool finished_before_timeout = ac.waitForResult(ros::Duration(10.0));
 
     if (finished_before_timeout) {
         actionlib::SimpleClientGoalState state = ac.getState();
diff --git a/src/grasping/grasp_util.cpp b/src/grasping/grasp_util.cpp
index d6ab94d..b89d05d 100644
--- a/src/grasping/grasp_util.cpp
+++ b/src/grasping/grasp_util.cpp
@@ -4,6 +4,7 @@
 #include "grasp_util.h"
 #include <tf2/transform_datatypes.h>
 #include <franka_gripper/HomingAction.h>
+#include <franka_gripper/MoveAction.h>
 #include <franka_gripper/HomingGoal.h>
 
 void GraspUtil::setupOpenGripper(trajectory_msgs::JointTrajectory &posture, double amount) {
@@ -162,14 +163,21 @@ GraspUtil::placeFromAbove(moveit::planning_interface::MoveGroupInterface &move_g
 }
 
 bool GraspUtil::homeGripper() {
+    actionlib::SimpleActionClient<franka_gripper::MoveAction> moveActionClient("franka_gripper/move", true);
+    if (moveActionClient.waitForServer(ros::Duration(1.0))) {
+        moveActionClient.cancelAllGoals();
+    }
+
     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;
     }
+
+    ac.cancelAllGoals();
     franka_gripper::HomingGoal goal;
     ac.sendGoal(goal);
-    if (ac.waitForResult(ros::Duration(20.0))) {
+    if (ac.waitForResult(ros::Duration(15.0))) {
         actionlib::SimpleClientGoalState state = ac.getState();
         ROS_INFO_STREAM("Homing finished: " <<  state.toString());
         return state == actionlib::SimpleClientGoalState::SUCCEEDED;
-- 
GitLab