diff --git a/src/grasping/franka_gripper_util.cpp b/src/grasping/franka_gripper_util.cpp index 08a458f907efc3342a5d228cfc5d7cf439b897b1..66f1e2240705126dfb0d50cd821f4a0539e12559 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 d6ab94d3f7f6ff70541b29365a8ef00ee1e779a7..b89d05d4c7a49a20168f8ec3e5da4becd9aab221 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;