minor improvements and reduced timeouts

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();

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;