diff --git a/franka_gripper/src/franka_gripper.cpp b/franka_gripper/src/franka_gripper.cpp index 3aeefe4abfcfcac426427f0b1e86a854b4b18174..53fc6d124f34257a8a7bcb532c04ee83beda1ba5 100644 --- a/franka_gripper/src/franka_gripper.cpp +++ b/franka_gripper/src/franka_gripper.cpp @@ -33,8 +33,6 @@ void gripperCommandExecuteCallback( double default_speed, actionlib::SimpleActionServer<control_msgs::GripperCommandAction>* action_server, const control_msgs::GripperCommandGoalConstPtr& goal) { - constexpr double kSamePositionThreshold = 1e-4; - auto gripper_command_handler = [goal, grasp_epsilon, default_speed, &gripper]() { // HACK: As one gripper finger is <mimic>, MoveIt!'s trajectory execution manager // only sends us the width of one finger. Multiply by 2 to get the intended width. @@ -46,6 +44,7 @@ void gripperCommandExecuteCallback( << state.max_width << " command = " << target_width); return false; } + constexpr double kSamePositionThreshold = 1e-4; if (std::abs(target_width - state.width) < kSamePositionThreshold) { return true; }