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