diff --git a/README.md b/README.md index 4c495307811198d87d673d0d633d7c2ef9c9e4fa..507bf9538303a30a19e1af4a4b02e02622632034 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,7 @@ # Panda Grasping Package -Allows to pick and place objects. The current maximum height, where the panda can grip (from above) ist 50cm. This package uses the old pick-and-place pipeline of MoveIt. +Allows to pick and place objects. The current maximum height, where the panda can grip (from above) ist 50cm. This +package uses the old pick-and-place pipeline of MoveIt. ## How to install @@ -9,10 +10,13 @@ Allows to pick and place objects. The current maximum height, where the panda ca * catkin build & source the workspace * that's it -## How to use +## How to use * u can start the configurable pick and place with: ```roslaunch panda_grasping simulation.launch``` * to pick or place open a second console - * to pick type for example: ```rosservice call panda_grasping/PickObjectService '{pose: {position: {x: 0.5, y: 0.0, z: 0.2}, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}}, dimensions: {x: 0.02, y: 0.02, z: 0.2}}'``` - * to place type for example ```rosservice call panda_grasping/PlaceObjectService '{pose: {position: {x: 0.0, y: 0.5, z: 0.2}, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}}, dimensions: {x: 0.02, y: 0.02, z: 0.2}}'``` - * inside of the message u can update orientation (currenly only z is supported), position and size of the object to pick or place + * to pick type for + example: ```rosservice call panda_grasping/PickObjectService '{pose: {position: {x: 0.5, y: 0.0, z: 0.2}, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}}, dimensions: {x: 0.02, y: 0.02, z: 0.2}}'``` + * to place type for + example ```rosservice call panda_grasping/PlaceObjectService '{pose: {position: {x: 0.0, y: 0.5, z: 0.2}, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}}, dimensions: {x: 0.02, y: 0.02, z: 0.2}}'``` + * inside of the message u can update orientation (currenly only z is supported), position and size of the object to + pick or place diff --git a/include/environment_util.h b/include/environment_util.h index 01a6fe5fa06abb75e1f8018dfa131ae81be3e9b5..83918b7142b7572eaf8d19daed1c62bafc136600 100644 --- a/include/environment_util.h +++ b/include/environment_util.h @@ -18,42 +18,41 @@ /** * Util for the construction of the scene. */ -class EnvironmentUtil { - +class EnvironmentUtil +{ public: - - /** - * Adds a table plate to the planning scene. - * @param collision_objects current collision objects in the planning scene - * @param x_dimension width of the table - * @param y_dimension length of the table - */ - void constructPlate(std::vector<moveit_msgs::CollisionObject> &collision_objects, double x_dimension, - double y_dimension); - - /** - * Adds a support surface to the planning scene (with ID "bin"). - * @param collision_objects current collision objects in the planning scene - * @param id id of the support surface to create - * @param object_x_dimension x dimension object to be picked - * @param object_y_dimension y dimension object to be picked - * @param object_z_dimension z dimension object to be picked - *@param object_to_pick_pose pose of the object to be picked - */ - void constructSupport(std::vector<moveit_msgs::CollisionObject> &collision_objects, std::string id, - double object_x_dimension, double object_y_dimension, double object_z_dimension, - geometry_msgs::Pose &object_to_pick_pose); - - /** - * Adds a object to be picked/placed to the planning scene. - * @param collision_objects current collision objects in the planning scene - * @param id id of the object to create - * @param pose pose of the object to be picked - * @param dimensions dimensions of the object to be picked - */ - moveit_msgs::CollisionObject - constructObjectToPick(std::vector<moveit_msgs::CollisionObject> &collision_objects, std::string id, - geometry_msgs::Pose &pose, geometry_msgs::Vector3 &dimensions); + /** + * Adds a table plate to the planning scene. + * @param collision_objects current collision objects in the planning scene + * @param x_dimension width of the table + * @param y_dimension length of the table + */ + void constructPlate(std::vector<moveit_msgs::CollisionObject>& collision_objects, double x_dimension, + double y_dimension); + + /** + * Adds a support surface to the planning scene (with ID "bin"). + * @param collision_objects current collision objects in the planning scene + * @param id id of the support surface to create + * @param object_x_dimension x dimension object to be picked + * @param object_y_dimension y dimension object to be picked + * @param object_z_dimension z dimension object to be picked + *@param object_to_pick_pose pose of the object to be picked + */ + void constructSupport(std::vector<moveit_msgs::CollisionObject>& collision_objects, std::string id, + double object_x_dimension, double object_y_dimension, double object_z_dimension, + geometry_msgs::Pose& object_to_pick_pose); + + /** + * Adds a object to be picked/placed to the planning scene. + * @param collision_objects current collision objects in the planning scene + * @param id id of the object to create + * @param pose pose of the object to be picked + * @param dimensions dimensions of the object to be picked + */ + moveit_msgs::CollisionObject constructObjectToPick(std::vector<moveit_msgs::CollisionObject>& collision_objects, + std::string id, geometry_msgs::Pose& pose, + geometry_msgs::Vector3& dimensions); }; -#endif //PANDA_GRASPING_ENVIRONMENT_UTIL_H +#endif // PANDA_GRASPING_ENVIRONMENT_UTIL_H diff --git a/include/franka_gripper_util.h b/include/franka_gripper_util.h index b0b8ecc2d2cc2cda51f8e2ed742541fab024108c..ef2e06e583951dcdff73defd9b7f53b72edcb5d8 100644 --- a/include/franka_gripper_util.h +++ b/include/franka_gripper_util.h @@ -5,16 +5,13 @@ #ifndef PUBLIC_FRANKA_GRIPPER_UTIL_H #define PUBLIC_FRANKA_GRIPPER_UTIL_H - -class FrankaGripperUtil { - +class FrankaGripperUtil +{ public: - /** - * resets the real robot, to allow correct future movements and grasps - */ - void resetGripperForNextAction(); - + /** + * resets the real robot, to allow correct future movements and grasps + */ + void resetGripperForNextAction(); }; - -#endif //PUBLIC_FRANKA_GRIPPER_UTIL_H +#endif // PUBLIC_FRANKA_GRIPPER_UTIL_H diff --git a/include/grasp_util.h b/include/grasp_util.h index 66bdb7f373f7a92aaa51e3f2490c0ea6aa3eda33..0e125b00436a6d9d4a853e573506ffec1d675726 100644 --- a/include/grasp_util.h +++ b/include/grasp_util.h @@ -18,85 +18,84 @@ /** * Util for grasping. */ -class GraspUtil { - +class GraspUtil +{ public: - - /** - * distance of the center of the hand to the center of the finger tip - */ - static constexpr tf2Scalar FRANKA_GRIPPER_DISTANCE{0.1034}; - - /** - * length of a finger - */ - static constexpr tf2Scalar FRANKA_GRIPPER_FINGER_LENGTH{0.054}; - - /** - * size of the finger tip - */ - static constexpr tf2Scalar FRANKA_GRIPPER_FINGERTIP_SIZE{0.018}; - - /** - * Sets up how far the gripper should be opened for the next action. - * @param posture empty posture object defining grasp - * @param amount total opening amount (space between "forks") - */ - void setupOpenGripper(trajectory_msgs::JointTrajectory &posture, double amount); - - /** - * Sets up how much the gripper should be closed for the next action. - * @param posture empty posture object defining grasp - * @param amount total close amount (space between "forks") - */ - void setupClosedGripper(trajectory_msgs::JointTrajectory &posture, double amount); - - /** - * Compute the "pre-pick"-pose - * @param object_to_pick_pose pose of the object to pick (currently only vertically oriented objects are supported) - * @param dimensions of the object - * @param gripper_orientation current gripper orientation relative to the link it is attached to - * @param reach specifies how far the gripper reaches around the object (0 means the center of the finger tip is at the object's corner) - * @return the "pre-pick"-pose - */ - geometry_msgs::Pose - getPickFromAbovePose(geometry_msgs::Pose &object_to_pick_pose, geometry_msgs::Vector3 dimensions, - geometry_msgs::Quaternion gripper_orientation, - double reach = FRANKA_GRIPPER_FINGERTIP_SIZE / 2); - - /** - * Util method to pick an object from above. - * @param move_group an initialized MoveGroupInterface instance - * @param pick_pose via getPickFromAbovePose-method computed pre-pick-pose - * @param close_amount total close amount (space between "forks") - * @param open_amount total opening amount (space between "forks") - * @param supporting_surface_id ID of surface where object is picked from (e.g. the table) - * @param object_to_pick_id ID of the object to pick - * @return true if successfully picked - */ - bool pickFromAbove(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &pick_pose, - geometry_msgs::Vector3 dimensions, double open_amount, std::string supporting_surface_id, - std::string object_to_pick_id, bool plan_only = false); - - /** - * Util method to place an object from above. - * @param move_group an initialized MoveGroupInterface instance - * @param place_pose target place pose for the object - * @param close_amount total close amount (space between "forks") - * @param open_amount total opening amount (space between "forks") - * @param supporting_surface_id ID of surface where object is placed on (e.g. the table) - * @param object_to_place_id ID of the object to place - */ - bool placeFromAbove(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &place_pose, - double open_amount, std::string supporting_surface_id, std::string object_to_place_id, bool plan_only = false); - - /** - * Homes the gripper (in case it was abused before and is in an inconsistent state) - * This method is blocking with a maximum runtime of ~30s - * @return true if homing succeeded, false otherwise - */ - static bool homeGripper(); - + /** + * distance of the center of the hand to the center of the finger tip + */ + static constexpr tf2Scalar FRANKA_GRIPPER_DISTANCE{ 0.1034 }; + + /** + * length of a finger + */ + static constexpr tf2Scalar FRANKA_GRIPPER_FINGER_LENGTH{ 0.054 }; + + /** + * size of the finger tip + */ + static constexpr tf2Scalar FRANKA_GRIPPER_FINGERTIP_SIZE{ 0.018 }; + + /** + * Sets up how far the gripper should be opened for the next action. + * @param posture empty posture object defining grasp + * @param amount total opening amount (space between "forks") + */ + void setupOpenGripper(trajectory_msgs::JointTrajectory& posture, double amount); + + /** + * Sets up how much the gripper should be closed for the next action. + * @param posture empty posture object defining grasp + * @param amount total close amount (space between "forks") + */ + void setupClosedGripper(trajectory_msgs::JointTrajectory& posture, double amount); + + /** + * Compute the "pre-pick"-pose + * @param object_to_pick_pose pose of the object to pick (currently only vertically oriented objects are supported) + * @param dimensions of the object + * @param gripper_orientation current gripper orientation relative to the link it is attached to + * @param reach specifies how far the gripper reaches around the object (0 means the center of the finger tip is at + * the object's corner) + * @return the "pre-pick"-pose + */ + geometry_msgs::Pose getPickFromAbovePose(geometry_msgs::Pose& object_to_pick_pose, geometry_msgs::Vector3 dimensions, + geometry_msgs::Quaternion gripper_orientation, + double reach = FRANKA_GRIPPER_FINGERTIP_SIZE / 2); + + /** + * Util method to pick an object from above. + * @param move_group an initialized MoveGroupInterface instance + * @param pick_pose via getPickFromAbovePose-method computed pre-pick-pose + * @param close_amount total close amount (space between "forks") + * @param open_amount total opening amount (space between "forks") + * @param supporting_surface_id ID of surface where object is picked from (e.g. the table) + * @param object_to_pick_id ID of the object to pick + * @return true if successfully picked + */ + bool pickFromAbove(moveit::planning_interface::MoveGroupInterface& move_group, geometry_msgs::Pose& pick_pose, + geometry_msgs::Vector3 dimensions, double open_amount, std::string supporting_surface_id, + std::string object_to_pick_id, bool plan_only = false); + + /** + * Util method to place an object from above. + * @param move_group an initialized MoveGroupInterface instance + * @param place_pose target place pose for the object + * @param close_amount total close amount (space between "forks") + * @param open_amount total opening amount (space between "forks") + * @param supporting_surface_id ID of surface where object is placed on (e.g. the table) + * @param object_to_place_id ID of the object to place + */ + bool placeFromAbove(moveit::planning_interface::MoveGroupInterface& move_group, geometry_msgs::Pose& place_pose, + double open_amount, std::string supporting_surface_id, std::string object_to_place_id, + bool plan_only = false); + + /** + * Homes the gripper (in case it was abused before and is in an inconsistent state) + * This method is blocking with a maximum runtime of ~30s + * @return true if homing succeeded, false otherwise + */ + static bool homeGripper(); }; -#endif //PANDA_GRASPING_GRASP_UTIL_H +#endif // PANDA_GRASPING_GRASP_UTIL_H diff --git a/launch/simulation.launch b/launch/simulation.launch index 6ca38627f1a1b666f0ee7bc232a97d402cdeb0bf..f8f48409915161a59f4fc698d648ba22b3677cc2 100644 --- a/launch/simulation.launch +++ b/launch/simulation.launch @@ -1,10 +1,10 @@ <launch> <include file="$(find panda_simulation)/launch/simulation.launch"/> - <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" /> + <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch"/> <!--<node name="rqt_console" pkg="rqt_console" type="rqt_console" />--> - <node pkg="panda_grasping" type="grasp_service" name="grasp_service" output="screen" /> + <node pkg="panda_grasping" type="grasp_service" name="grasp_service" output="screen"/> <!-- sample commands --> <!-- rosservice call panda_grasping/PickObjectService '{pose: {position: {x: 0.5, y: 0.0, z: 0.2}, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}}, dimensions: {x: 0.02, y: 0.02, z: 0.2}}' --> diff --git a/package.xml b/package.xml index 94db6d09fcecffeab978784e6d6c1d115ae1e851..0ddc77e605dd980cf184db16b654a077bed4148b 100644 --- a/package.xml +++ b/package.xml @@ -1,44 +1,44 @@ <?xml version="1.0"?> <package format="2"> - <name>panda_grasping</name> - <version>0.1.0</version> - <description>The panda_grasping package</description> + <name>panda_grasping</name> + <version>0.1.0</version> + <description>The panda_grasping package</description> - <!-- One maintainer tag required, multiple allowed, one person per tag --> - <!-- Example: --> - <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> --> - <maintainer email="sebastian@todo.todo">sebastian</maintainer> + <!-- One maintainer tag required, multiple allowed, one person per tag --> + <!-- Example: --> + <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> --> + <maintainer email="sebastian@todo.todo">sebastian</maintainer> - <!-- One license tag required, multiple allowed, one license per tag --> - <!-- Commonly used license strings: --> - <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 --> - <license>TODO</license> + <!-- One license tag required, multiple allowed, one license per tag --> + <!-- Commonly used license strings: --> + <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 --> + <license>TODO</license> - <buildtool_depend>catkin</buildtool_depend> + <buildtool_depend>catkin</buildtool_depend> - <build_depend>message_generation</build_depend> - <build_depend>roscpp</build_depend> - <build_depend>std_msgs</build_depend> + <build_depend>message_generation</build_depend> + <build_depend>roscpp</build_depend> + <build_depend>std_msgs</build_depend> - <build_export_depend>roscpp</build_export_depend> - <build_export_depend>std_msgs</build_export_depend> + <build_export_depend>roscpp</build_export_depend> + <build_export_depend>std_msgs</build_export_depend> - <depend>moveit_core</depend> - <depend>moveit_ros_planning_interface</depend> - <depend>moveit_visual_tools</depend> - <depend>robot_state_publisher</depend> - <depend>tf2</depend> - <depend>franka_gripper</depend> + <depend>moveit_core</depend> + <depend>moveit_ros_planning_interface</depend> + <depend>moveit_visual_tools</depend> + <depend>robot_state_publisher</depend> + <depend>tf2</depend> + <depend>franka_gripper</depend> - <exec_depend>message_runtime</exec_depend> - <exec_depend>roscpp</exec_depend> - <exec_depend>std_msgs</exec_depend> - <exec_depend>pluginlib</exec_depend> + <exec_depend>message_runtime</exec_depend> + <exec_depend>roscpp</exec_depend> + <exec_depend>std_msgs</exec_depend> + <exec_depend>pluginlib</exec_depend> - <!-- The export tag contains other, unspecified, tags --> - <export> - <!-- Other tools can request additional information be placed here --> + <!-- The export tag contains other, unspecified, tags --> + <export> + <!-- Other tools can request additional information be placed here --> - </export> + </export> </package> diff --git a/src/grasping/environment_util.cpp b/src/grasping/environment_util.cpp index 54766834b5e4fb2a625ddabc44a907940babeea2..860f6160dc85e6a87a181f02e0786f100eab4d1e 100644 --- a/src/grasping/environment_util.cpp +++ b/src/grasping/environment_util.cpp @@ -5,80 +5,80 @@ #include "environment_util.h" -void EnvironmentUtil::constructPlate(std::vector<moveit_msgs::CollisionObject> &collision_objects, double x_dimension, - double y_dimension) { +void EnvironmentUtil::constructPlate(std::vector<moveit_msgs::CollisionObject>& collision_objects, double x_dimension, + double y_dimension) +{ + moveit_msgs::CollisionObject plate; - moveit_msgs::CollisionObject plate; + plate.header.frame_id = "panda_link0"; + plate.id = "plate"; - plate.header.frame_id = "panda_link0"; - plate.id = "plate"; + plate.primitives.resize(1); + plate.primitives[0].type = shape_msgs::SolidPrimitive::BOX; + plate.primitives[0].dimensions.resize(3); + plate.primitives[0].dimensions[0] = x_dimension; + plate.primitives[0].dimensions[1] = y_dimension; + plate.primitives[0].dimensions[2] = 0.1; - plate.primitives.resize(1); - plate.primitives[0].type = shape_msgs::SolidPrimitive::BOX; - plate.primitives[0].dimensions.resize(3); - plate.primitives[0].dimensions[0] = x_dimension; - plate.primitives[0].dimensions[1] = y_dimension; - plate.primitives[0].dimensions[2] = 0.1; + plate.primitive_poses.resize(1); + plate.primitive_poses[0].position.x = 0; + plate.primitive_poses[0].position.y = 0; + plate.primitive_poses[0].position.z = -0.1; - plate.primitive_poses.resize(1); - plate.primitive_poses[0].position.x = 0; - plate.primitive_poses[0].position.y = 0; - plate.primitive_poses[0].position.z = -0.1; + plate.operation = plate.ADD; - plate.operation = plate.ADD; - - collision_objects.push_back(plate); + collision_objects.push_back(plate); } -void EnvironmentUtil::constructSupport(std::vector<moveit_msgs::CollisionObject> &collision_objects, std::string id, +void EnvironmentUtil::constructSupport(std::vector<moveit_msgs::CollisionObject>& collision_objects, std::string id, double object_x_dimension, double object_y_dimension, double object_z_dimension, - geometry_msgs::Pose &object_to_pick_pose) { - - moveit_msgs::CollisionObject pick_support; + geometry_msgs::Pose& object_to_pick_pose) +{ + moveit_msgs::CollisionObject pick_support; - pick_support.header.frame_id = "panda_link0"; - pick_support.id = id; + pick_support.header.frame_id = "panda_link0"; + pick_support.id = id; - pick_support.primitives.resize(1); - pick_support.primitives[0].type = shape_msgs::SolidPrimitive::BOX; - pick_support.primitives[0].dimensions.resize(3); - pick_support.primitives[0].dimensions[0] = object_x_dimension; - pick_support.primitives[0].dimensions[1] = object_y_dimension; - pick_support.primitives[0].dimensions[2] = 0.04; + pick_support.primitives.resize(1); + pick_support.primitives[0].type = shape_msgs::SolidPrimitive::BOX; + pick_support.primitives[0].dimensions.resize(3); + pick_support.primitives[0].dimensions[0] = object_x_dimension; + pick_support.primitives[0].dimensions[1] = object_y_dimension; + pick_support.primitives[0].dimensions[2] = 0.04; - pick_support.primitive_poses.resize(1); - pick_support.primitive_poses[0].position.x = object_to_pick_pose.position.x; - pick_support.primitive_poses[0].position.y = object_to_pick_pose.position.y; - pick_support.primitive_poses[0].position.z = (object_to_pick_pose.position.z - (object_z_dimension / 2) - 0.02); + pick_support.primitive_poses.resize(1); + pick_support.primitive_poses[0].position.x = object_to_pick_pose.position.x; + pick_support.primitive_poses[0].position.y = object_to_pick_pose.position.y; + pick_support.primitive_poses[0].position.z = (object_to_pick_pose.position.z - (object_z_dimension / 2) - 0.02); - pick_support.operation = pick_support.ADD; + pick_support.operation = pick_support.ADD; - collision_objects.push_back(pick_support); + collision_objects.push_back(pick_support); } moveit_msgs::CollisionObject -EnvironmentUtil::constructObjectToPick(std::vector<moveit_msgs::CollisionObject> &collision_objects, - std::string id, geometry_msgs::Pose &pose, geometry_msgs::Vector3 &dimensions) { - - moveit_msgs::CollisionObject object_to_pick; +EnvironmentUtil::constructObjectToPick(std::vector<moveit_msgs::CollisionObject>& collision_objects, std::string id, + geometry_msgs::Pose& pose, geometry_msgs::Vector3& dimensions) +{ + moveit_msgs::CollisionObject object_to_pick; - object_to_pick.header.frame_id = "panda_link0"; - object_to_pick.id = id; + object_to_pick.header.frame_id = "panda_link0"; + object_to_pick.id = id; - object_to_pick.primitives.resize(1); - object_to_pick.primitives[0].type = shape_msgs::SolidPrimitive::BOX; - object_to_pick.primitives[0].dimensions.resize(3); - object_to_pick.primitives[0].dimensions[0] = dimensions.x; - object_to_pick.primitives[0].dimensions[1] = dimensions.y; - object_to_pick.primitives[0].dimensions[2] = dimensions.z; + object_to_pick.primitives.resize(1); + object_to_pick.primitives[0].type = shape_msgs::SolidPrimitive::BOX; + object_to_pick.primitives[0].dimensions.resize(3); + object_to_pick.primitives[0].dimensions[0] = dimensions.x; + object_to_pick.primitives[0].dimensions[1] = dimensions.y; + object_to_pick.primitives[0].dimensions[2] = dimensions.z; - object_to_pick.primitive_poses.resize(1); - object_to_pick.primitive_poses[0].position = pose.position; - object_to_pick.primitive_poses[0].orientation = pose.orientation; + object_to_pick.primitive_poses.resize(1); + object_to_pick.primitive_poses[0].position = pose.position; + object_to_pick.primitive_poses[0].orientation = pose.orientation; - object_to_pick.operation = object_to_pick.ADD; + object_to_pick.operation = object_to_pick.ADD; - collision_objects.push_back(object_to_pick); + collision_objects.push_back(object_to_pick); - return object_to_pick; + return object_to_pick; } \ No newline at end of file diff --git a/src/grasping/franka_gripper_util.cpp b/src/grasping/franka_gripper_util.cpp index 66f1e2240705126dfb0d50cd821f4a0539e12559..46484f2e31bef3219b2d2ccc4d36993b6b594e90 100644 --- a/src/grasping/franka_gripper_util.cpp +++ b/src/grasping/franka_gripper_util.cpp @@ -10,26 +10,29 @@ // Franka #include <franka_gripper/MoveAction.h> -void FrankaGripperUtil::resetGripperForNextAction() { - - actionlib::SimpleActionClient<franka_gripper::MoveAction> ac("franka_gripper/move", true); - - ROS_INFO("Waiting for reset action server to start."); - ac.waitForServer(); - - ROS_INFO("Reset action server started, sending goal."); - franka_gripper::MoveGoal goal; - goal.speed = 0.08; - goal.width = 0.0; - ac.sendGoal(goal); - - //wait for the action to return - bool finished_before_timeout = ac.waitForResult(ros::Duration(10.0)); - - if (finished_before_timeout) { - actionlib::SimpleClientGoalState state = ac.getState(); - ROS_INFO("Reset action finished: %s", state.toString().c_str()); - } else { - ROS_INFO("Reset action did not finish before the time out."); - } +void FrankaGripperUtil::resetGripperForNextAction() +{ + actionlib::SimpleActionClient<franka_gripper::MoveAction> ac("franka_gripper/move", true); + + ROS_INFO("Waiting for reset action server to start."); + ac.waitForServer(); + + ROS_INFO("Reset action server started, sending goal."); + franka_gripper::MoveGoal goal; + goal.speed = 0.08; + goal.width = 0.0; + ac.sendGoal(goal); + + // wait for the action to return + bool finished_before_timeout = ac.waitForResult(ros::Duration(10.0)); + + if (finished_before_timeout) + { + actionlib::SimpleClientGoalState state = ac.getState(); + ROS_INFO("Reset action finished: %s", state.toString().c_str()); + } + else + { + ROS_INFO("Reset action did not finish before the time out."); + } } diff --git a/src/grasping/grasp_service.cpp b/src/grasping/grasp_service.cpp index 7e27a2a4de28d4004599d4bd1a48a57efdc080fe..62b2eaae96bbe83536146bc8971c5c5171c0e806 100644 --- a/src/grasping/grasp_service.cpp +++ b/src/grasping/grasp_service.cpp @@ -16,8 +16,9 @@ #include <tf2_geometry_msgs/tf2_geometry_msgs.h> #include <tf2_ros/transform_listener.h> -namespace grasping_state { - bool object_picked = false; +namespace grasping_state +{ +bool object_picked = false; } /** @@ -33,59 +34,60 @@ tf2_ros::Buffer tfBuffer; * @param group a initialized MoveGroupInterface instance * @param n a valid nodehandle */ -bool pickObject(panda_grasping::PickObject::Request &req, - panda_grasping::PickObject::Response &res, ros::NodeHandle &n, - moveit::planning_interface::PlanningSceneInterface &planning_scene_interface, - moveit::planning_interface::MoveGroupInterface &group) { - - if (!grasping_state::object_picked) { - - grasping_state::object_picked = true; - - EnvironmentUtil env_util; - GraspUtil grasp_util; - - std::vector<moveit_msgs::CollisionObject> collision_objects; - - ROS_INFO("Adding plate."); - env_util.constructPlate(collision_objects, 3.0, 3.0); - - ROS_INFO("Adding pick support."); - env_util.constructSupport(collision_objects, "pick_support", req.dimensions.x, req.dimensions.y, - req.dimensions.z, - req.pose); - - ROS_INFO("Adding object."); - moveit_msgs::CollisionObject po = env_util.constructObjectToPick(collision_objects, "pick_object", req.pose, - req.dimensions); - - ROS_INFO("Applying to scene."); - if (!planning_scene_interface.applyCollisionObjects(collision_objects)) { - res.success = false; - return false; - } - - ROS_INFO("Retrieving the orientation of the gripper wrt. to the hand."); - geometry_msgs::Transform gripper_transform = tfBuffer.lookupTransform("panda_hand", "panda_link8", - ros::Time(0)).transform; +bool pickObject(panda_grasping::PickObject::Request& req, panda_grasping::PickObject::Response& res, ros::NodeHandle& n, + moveit::planning_interface::PlanningSceneInterface& planning_scene_interface, + moveit::planning_interface::MoveGroupInterface& group) +{ + if (!grasping_state::object_picked) + { + grasping_state::object_picked = true; + + EnvironmentUtil env_util; + GraspUtil grasp_util; + + std::vector<moveit_msgs::CollisionObject> collision_objects; + + ROS_INFO("Adding plate."); + env_util.constructPlate(collision_objects, 3.0, 3.0); + + ROS_INFO("Adding pick support."); + env_util.constructSupport(collision_objects, "pick_support", req.dimensions.x, req.dimensions.y, req.dimensions.z, + req.pose); + + ROS_INFO("Adding object."); + moveit_msgs::CollisionObject po = + env_util.constructObjectToPick(collision_objects, "pick_object", req.pose, req.dimensions); + + ROS_INFO("Applying to scene."); + if (!planning_scene_interface.applyCollisionObjects(collision_objects)) + { + res.success = false; + return false; + } - ROS_INFO("Computing pre-grasp-pose."); - geometry_msgs::Pose pick_pose = grasp_util.getPickFromAbovePose(req.pose, req.dimensions, gripper_transform.rotation); + ROS_INFO("Retrieving the orientation of the gripper wrt. to the hand."); + geometry_msgs::Transform gripper_transform = + tfBuffer.lookupTransform("panda_hand", "panda_link8", ros::Time(0)).transform; - ROS_INFO("Picking up ..."); - // 0.08 = 8cm = maximum gripper opening - if (!grasp_util.pickFromAbove(group, pick_pose, req.dimensions, 0.08, "pick_support", "pick_object", req.plan_only)) { - res.success = false; - return false; - } + ROS_INFO("Computing pre-grasp-pose."); + geometry_msgs::Pose pick_pose = + grasp_util.getPickFromAbovePose(req.pose, req.dimensions, gripper_transform.rotation); - res.success = true; - return true; + ROS_INFO("Picking up ..."); + // 0.08 = 8cm = maximum gripper opening + if (!grasp_util.pickFromAbove(group, pick_pose, req.dimensions, 0.08, "pick_support", "pick_object", req.plan_only)) + { + res.success = false; + return false; } - ROS_ERROR("Error: Cannot pick object. An other object is already picked."); - res.success = false; - return false; + res.success = true; + return true; + } + + ROS_ERROR("Error: Cannot pick object. An other object is already picked."); + res.success = false; + return false; } /** @@ -96,70 +98,70 @@ bool pickObject(panda_grasping::PickObject::Request &req, * @param group a initialized MoveGroupInterface instance * @param n a valid nodehandle */ -bool -placeObject(panda_grasping::PlaceObject::Request &req, panda_grasping::PlaceObject::Response &res, ros::NodeHandle &n, - moveit::planning_interface::PlanningSceneInterface &planning_scene_interface, - moveit::planning_interface::MoveGroupInterface &group) { - - ROS_INFO_STREAM("received new place-request: " << req); - - if (grasping_state::object_picked) { - EnvironmentUtil env_util; - GraspUtil grasp_util; - - std::vector<moveit_msgs::CollisionObject> collision_objects; - - ROS_INFO("Adding place support."); - env_util.constructSupport(collision_objects, "place_support", req.dimensions.x, req.dimensions.y, - req.dimensions.z, - req.pose); - - ROS_INFO("Applying to scene."); - if (!planning_scene_interface.applyCollisionObjects(collision_objects)) { - res.success = false; - return false; - } - - ROS_INFO("Placing object."); - if (!grasp_util.placeFromAbove(group, req.pose, 0.08, "place_support", "pick_object", req.plan_only)) { - res.success = false; - return false; - } - - grasping_state::object_picked = false; - res.success = true; - return true; +bool placeObject(panda_grasping::PlaceObject::Request& req, panda_grasping::PlaceObject::Response& res, + ros::NodeHandle& n, moveit::planning_interface::PlanningSceneInterface& planning_scene_interface, + moveit::planning_interface::MoveGroupInterface& group) +{ + ROS_INFO_STREAM("received new place-request: " << req); + + if (grasping_state::object_picked) + { + EnvironmentUtil env_util; + GraspUtil grasp_util; + + std::vector<moveit_msgs::CollisionObject> collision_objects; + + ROS_INFO("Adding place support."); + env_util.constructSupport(collision_objects, "place_support", req.dimensions.x, req.dimensions.y, req.dimensions.z, + req.pose); + + ROS_INFO("Applying to scene."); + if (!planning_scene_interface.applyCollisionObjects(collision_objects)) + { + res.success = false; + return false; } - ROS_ERROR("Error: Could not place object. Reason: There is currently no object attached to the gripper."); - res.success = false; - return false; -} - + ROS_INFO("Placing object."); + if (!grasp_util.placeFromAbove(group, req.pose, 0.08, "place_support", "pick_object", req.plan_only)) + { + res.success = false; + return false; + } -int main(int argc, char **argv) { + grasping_state::object_picked = false; + res.success = true; + return true; + } - ros::init(argc, argv, "grasp_service"); - ros::NodeHandle n("panda_grasping"); - ros::AsyncSpinner spinner(2); - spinner.start(); + ROS_ERROR("Error: Could not place object. Reason: There is currently no object attached to the gripper."); + res.success = false; + return false; +} - tf2_ros::TransformListener tfListener(tfBuffer); +int main(int argc, char** argv) +{ + ros::init(argc, argv, "grasp_service"); + ros::NodeHandle n("panda_grasping"); + ros::AsyncSpinner spinner(2); + spinner.start(); - moveit::planning_interface::PlanningSceneInterface planning_scene_interface; - moveit::planning_interface::MoveGroupInterface group("panda_arm"); + tf2_ros::TransformListener tfListener(tfBuffer); - ros::ServiceServer pick_object_service = n.advertiseService<panda_grasping::PickObject::Request, panda_grasping::PickObject::Response>( - "PickObjectService", - boost::bind(&pickObject, _1, _2, boost::ref(n), boost::ref(planning_scene_interface), boost::ref(group))); + moveit::planning_interface::PlanningSceneInterface planning_scene_interface; + moveit::planning_interface::MoveGroupInterface group("panda_arm"); + ros::ServiceServer pick_object_service = + n.advertiseService<panda_grasping::PickObject::Request, panda_grasping::PickObject::Response>( + "PickObjectService", + boost::bind(&pickObject, _1, _2, boost::ref(n), boost::ref(planning_scene_interface), boost::ref(group))); - ros::ServiceServer place_object_service = n.advertiseService<panda_grasping::PlaceObject::Request, panda_grasping::PlaceObject::Response>( - "PlaceObjectService", - boost::bind(&placeObject, _1, _2, boost::ref(n), boost::ref(planning_scene_interface), boost::ref(group))); + ros::ServiceServer place_object_service = + n.advertiseService<panda_grasping::PlaceObject::Request, panda_grasping::PlaceObject::Response>( + "PlaceObjectService", + boost::bind(&placeObject, _1, _2, boost::ref(n), boost::ref(planning_scene_interface), boost::ref(group))); - ros::waitForShutdown(); + ros::waitForShutdown(); - return 0; + return 0; } - diff --git a/src/grasping/grasp_util.cpp b/src/grasping/grasp_util.cpp index b89d05d4c7a49a20168f8ec3e5da4becd9aab221..d3e5bbcc6a2286fff38944f39753f6705462104e 100644 --- a/src/grasping/grasp_util.cpp +++ b/src/grasping/grasp_util.cpp @@ -7,182 +7,198 @@ #include <franka_gripper/MoveAction.h> #include <franka_gripper/HomingGoal.h> -void GraspUtil::setupOpenGripper(trajectory_msgs::JointTrajectory &posture, double amount) { - posture.joint_names.resize(2); - posture.joint_names[0] = "panda_finger_joint1"; - posture.joint_names[1] = "panda_finger_joint2"; - - posture.points.resize(1); - posture.points[0].positions.resize(2); - posture.points[0].positions[0] = amount / 2; - posture.points[0].positions[1] = amount / 2; - posture.points[0].time_from_start = ros::Duration(0.5); +void GraspUtil::setupOpenGripper(trajectory_msgs::JointTrajectory& posture, double amount) +{ + posture.joint_names.resize(2); + posture.joint_names[0] = "panda_finger_joint1"; + posture.joint_names[1] = "panda_finger_joint2"; + + posture.points.resize(1); + posture.points[0].positions.resize(2); + posture.points[0].positions[0] = amount / 2; + posture.points[0].positions[1] = amount / 2; + posture.points[0].time_from_start = ros::Duration(0.5); } -void GraspUtil::setupClosedGripper(trajectory_msgs::JointTrajectory &posture, double amount) { - posture.joint_names.resize(2); - posture.joint_names[0] = "panda_finger_joint1"; - posture.joint_names[1] = "panda_finger_joint2"; - - posture.points.resize(1); - posture.points[0].positions.resize(2); - posture.points[0].positions[0] = amount / 2; - posture.points[0].positions[1] = amount / 2; - posture.points[0].time_from_start = ros::Duration(0.5); +void GraspUtil::setupClosedGripper(trajectory_msgs::JointTrajectory& posture, double amount) +{ + posture.joint_names.resize(2); + posture.joint_names[0] = "panda_finger_joint1"; + posture.joint_names[1] = "panda_finger_joint2"; + + posture.points.resize(1); + posture.points[0].positions.resize(2); + posture.points[0].positions[0] = amount / 2; + posture.points[0].positions[1] = amount / 2; + posture.points[0].time_from_start = ros::Duration(0.5); } -geometry_msgs::Pose -GraspUtil::getPickFromAbovePose(geometry_msgs::Pose &object_pose_msg, geometry_msgs::Vector3 dimensions, - geometry_msgs::Quaternion gripper_orientation_msg, double reach) { - - // object orientation - tf2::Quaternion object_orientation; - tf2::fromMsg(object_pose_msg.orientation, object_orientation); - - // gripper orientation - tf2::Quaternion gripper_orientation; - tf2::fromMsg(gripper_orientation_msg, gripper_orientation); - - // approach orientation - double approach_roll, approach_pitch, approach_yaw; - approach_roll = M_PI; // turn upside down - approach_pitch = 0; - approach_yaw = 0; - tf2::Quaternion approach_orientation; - approach_orientation.setRPY(approach_roll, approach_pitch, approach_yaw); - - tf2::Quaternion orientation = object_orientation * gripper_orientation * approach_orientation; - - // Actually, the maximum reach should be FRANKA_GRIPPER_FINGER_LENGTH - FRANKA_GRIPPER_FINGERTIP_SIZE/2, but half a - // finger length is considered a safety distance. - if (reach > FRANKA_GRIPPER_FINGER_LENGTH - FRANKA_GRIPPER_FINGERTIP_SIZE) { - ROS_WARN_STREAM("Reach around object reduced from " << reach << " to " << FRANKA_GRIPPER_FINGER_LENGTH - - FRANKA_GRIPPER_FINGERTIP_SIZE << "."); - reach = FRANKA_GRIPPER_FINGER_LENGTH - FRANKA_GRIPPER_FINGERTIP_SIZE; - } else if (reach < -FRANKA_GRIPPER_FINGERTIP_SIZE / 2) { - ROS_WARN_STREAM("Reach around object increased from " << reach << " to 0." << ". Minimum allowed reach is " - << (-FRANKA_GRIPPER_FINGERTIP_SIZE / 2) << "."); - reach = 0; - } - - // The pose is 10 cm straight above the object to pick. This only works on upright boxes. - geometry_msgs::Pose target_pose; - target_pose.orientation = tf2::toMsg(orientation); - target_pose.position.x = object_pose_msg.position.x; - target_pose.position.y = object_pose_msg.position.y; - target_pose.position.z = object_pose_msg.position.z + (0.5 * dimensions.z) + FRANKA_GRIPPER_DISTANCE - reach; - - return target_pose; +geometry_msgs::Pose GraspUtil::getPickFromAbovePose(geometry_msgs::Pose& object_pose_msg, + geometry_msgs::Vector3 dimensions, + geometry_msgs::Quaternion gripper_orientation_msg, double reach) +{ + // object orientation + tf2::Quaternion object_orientation; + tf2::fromMsg(object_pose_msg.orientation, object_orientation); + + // gripper orientation + tf2::Quaternion gripper_orientation; + tf2::fromMsg(gripper_orientation_msg, gripper_orientation); + + // approach orientation + double approach_roll, approach_pitch, approach_yaw; + approach_roll = M_PI; // turn upside down + approach_pitch = 0; + approach_yaw = 0; + tf2::Quaternion approach_orientation; + approach_orientation.setRPY(approach_roll, approach_pitch, approach_yaw); + + tf2::Quaternion orientation = object_orientation * gripper_orientation * approach_orientation; + + // Actually, the maximum reach should be FRANKA_GRIPPER_FINGER_LENGTH - FRANKA_GRIPPER_FINGERTIP_SIZE/2, but half a + // finger length is considered a safety distance. + if (reach > FRANKA_GRIPPER_FINGER_LENGTH - FRANKA_GRIPPER_FINGERTIP_SIZE) + { + ROS_WARN_STREAM("Reach around object reduced from " + << reach << " to " << FRANKA_GRIPPER_FINGER_LENGTH - FRANKA_GRIPPER_FINGERTIP_SIZE << "."); + reach = FRANKA_GRIPPER_FINGER_LENGTH - FRANKA_GRIPPER_FINGERTIP_SIZE; + } + else if (reach < -FRANKA_GRIPPER_FINGERTIP_SIZE / 2) + { + ROS_WARN_STREAM("Reach around object increased from " << reach << " to 0." + << ". Minimum allowed reach is " + << (-FRANKA_GRIPPER_FINGERTIP_SIZE / 2) << "."); + reach = 0; + } + + // The pose is 10 cm straight above the object to pick. This only works on upright boxes. + geometry_msgs::Pose target_pose; + target_pose.orientation = tf2::toMsg(orientation); + target_pose.position.x = object_pose_msg.position.x; + target_pose.position.y = object_pose_msg.position.y; + target_pose.position.z = object_pose_msg.position.z + (0.5 * dimensions.z) + FRANKA_GRIPPER_DISTANCE - reach; + + return target_pose; } -bool -GraspUtil::pickFromAbove(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &pick_pose, - geometry_msgs::Vector3 dimensions, - double open_amount, std::string supporting_surface_id, std::string object_to_pick_id, bool plan_only) { - - std::vector<moveit_msgs::Grasp> grasps; - grasps.resize(1); - grasps[0].grasp_pose.header.frame_id = "panda_link0"; - grasps[0].grasp_pose.pose = pick_pose; - - // Setup pre-grasp approach - grasps[0].pre_grasp_approach.direction.header.frame_id = "panda_link0"; - grasps[0].pre_grasp_approach.direction.vector.z = -1.0; - grasps[0].pre_grasp_approach.min_distance = 0.095; - grasps[0].pre_grasp_approach.desired_distance = 0.115; - - // Setup post-grasp retreat - grasps[0].post_grasp_retreat.direction.header.frame_id = "panda_link0"; - grasps[0].post_grasp_retreat.direction.vector.z = 1.0; - grasps[0].post_grasp_retreat.min_distance = 0.1; - grasps[0].post_grasp_retreat.desired_distance = 0.25; - - // Setup the opening / closing - mechanism of the gripper - - setupOpenGripper(grasps[0].pre_grasp_posture, open_amount); - - if (dimensions.x <= 0.08) { - setupClosedGripper(grasps[0].grasp_posture, dimensions.x); - } - - if (dimensions.x > 0.08 && dimensions.y <= 0.08) { - setupClosedGripper(grasps[0].grasp_posture, dimensions.y); - } - - if (dimensions.x > 0.08 && dimensions.y > 0.08) { - ROS_ERROR("Could not grasp: Object to big (in any dimension)."); - return false; - } - - move_group.setSupportSurfaceName(supporting_surface_id); - - moveit_msgs::MoveItErrorCodes miec = move_group.pick(object_to_pick_id, grasps, plan_only); - - if (miec.val == miec.SUCCESS) { - return true; - } - - ROS_ERROR_STREAM("Error while executing pick task. MoveItErrorCode: " << miec.val); +bool GraspUtil::pickFromAbove(moveit::planning_interface::MoveGroupInterface& move_group, + geometry_msgs::Pose& pick_pose, geometry_msgs::Vector3 dimensions, double open_amount, + std::string supporting_surface_id, std::string object_to_pick_id, bool plan_only) +{ + std::vector<moveit_msgs::Grasp> grasps; + grasps.resize(1); + grasps[0].grasp_pose.header.frame_id = "panda_link0"; + grasps[0].grasp_pose.pose = pick_pose; + + // Setup pre-grasp approach + grasps[0].pre_grasp_approach.direction.header.frame_id = "panda_link0"; + grasps[0].pre_grasp_approach.direction.vector.z = -1.0; + grasps[0].pre_grasp_approach.min_distance = 0.095; + grasps[0].pre_grasp_approach.desired_distance = 0.115; + + // Setup post-grasp retreat + grasps[0].post_grasp_retreat.direction.header.frame_id = "panda_link0"; + grasps[0].post_grasp_retreat.direction.vector.z = 1.0; + grasps[0].post_grasp_retreat.min_distance = 0.1; + grasps[0].post_grasp_retreat.desired_distance = 0.25; + + // Setup the opening / closing - mechanism of the gripper + + setupOpenGripper(grasps[0].pre_grasp_posture, open_amount); + + if (dimensions.x <= 0.08) + { + setupClosedGripper(grasps[0].grasp_posture, dimensions.x); + } + + if (dimensions.x > 0.08 && dimensions.y <= 0.08) + { + setupClosedGripper(grasps[0].grasp_posture, dimensions.y); + } + + if (dimensions.x > 0.08 && dimensions.y > 0.08) + { + ROS_ERROR("Could not grasp: Object to big (in any dimension)."); return false; -} - -bool -GraspUtil::placeFromAbove(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &place_pose, - double open_amount, std::string supporting_surface_id, std::string object_to_place_id, bool plan_only) { - - std::vector<moveit_msgs::PlaceLocation> place_location; - place_location.resize(1); - - place_location[0].place_pose.header.frame_id = "panda_link0"; - // tf2::Quaternion orientation; - // orientation.setRPY(0, 0, M_PI / 2); - place_location[0].place_pose.pose.orientation.w = 1;// = tf2::toMsg(orientation); - place_location[0].place_pose.pose.position = place_pose.position; + } - place_location[0].pre_place_approach.direction.header.frame_id = "panda_link0"; - place_location[0].pre_place_approach.direction.vector.z = -1.0; - place_location[0].pre_place_approach.min_distance = 0.095; - place_location[0].pre_place_approach.desired_distance = 0.115; + move_group.setSupportSurfaceName(supporting_surface_id); - place_location[0].post_place_retreat.direction.header.frame_id = "panda_link0"; - place_location[0].post_place_retreat.direction.vector.z = 1.0; - place_location[0].post_place_retreat.min_distance = 0.1; - place_location[0].post_place_retreat.desired_distance = 0.25; + moveit_msgs::MoveItErrorCodes miec = move_group.pick(object_to_pick_id, grasps, plan_only); - setupOpenGripper(place_location[0].post_place_posture, open_amount); - move_group.setSupportSurfaceName(supporting_surface_id); + if (miec.val == miec.SUCCESS) + { + return true; + } - moveit_msgs::MoveItErrorCodes miec = move_group.place(object_to_place_id, place_location, plan_only); - - if (miec.val == miec.SUCCESS) { - return true; - } + ROS_ERROR_STREAM("Error while executing pick task. MoveItErrorCode: " << miec.val); + return false; +} - ROS_ERROR_STREAM("Error while executing place task. MoveItErrorCode: " << miec.val); - return false; +bool GraspUtil::placeFromAbove(moveit::planning_interface::MoveGroupInterface& move_group, + geometry_msgs::Pose& place_pose, double open_amount, std::string supporting_surface_id, + std::string object_to_place_id, bool plan_only) +{ + std::vector<moveit_msgs::PlaceLocation> place_location; + place_location.resize(1); + + place_location[0].place_pose.header.frame_id = "panda_link0"; + // tf2::Quaternion orientation; + // orientation.setRPY(0, 0, M_PI / 2); + place_location[0].place_pose.pose.orientation.w = 1; // = tf2::toMsg(orientation); + place_location[0].place_pose.pose.position = place_pose.position; + + place_location[0].pre_place_approach.direction.header.frame_id = "panda_link0"; + place_location[0].pre_place_approach.direction.vector.z = -1.0; + place_location[0].pre_place_approach.min_distance = 0.095; + place_location[0].pre_place_approach.desired_distance = 0.115; + + place_location[0].post_place_retreat.direction.header.frame_id = "panda_link0"; + place_location[0].post_place_retreat.direction.vector.z = 1.0; + place_location[0].post_place_retreat.min_distance = 0.1; + place_location[0].post_place_retreat.desired_distance = 0.25; + + setupOpenGripper(place_location[0].post_place_posture, open_amount); + move_group.setSupportSurfaceName(supporting_surface_id); + + moveit_msgs::MoveItErrorCodes miec = move_group.place(object_to_place_id, place_location, plan_only); + + if (miec.val == miec.SUCCESS) + { + return true; + } + + ROS_ERROR_STREAM("Error while executing place task. MoveItErrorCode: " << miec.val); + return false; } -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(15.0))) { - actionlib::SimpleClientGoalState state = ac.getState(); - ROS_INFO_STREAM("Homing finished: " << state.toString()); - return state == actionlib::SimpleClientGoalState::SUCCEEDED; - } else { - ROS_ERROR("Gripper homing failed (with timeout)."); - return false; - } +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(15.0))) + { + actionlib::SimpleClientGoalState state = ac.getState(); + ROS_INFO_STREAM("Homing finished: " << state.toString()); + return state == actionlib::SimpleClientGoalState::SUCCEEDED; + } + else + { + ROS_ERROR("Gripper homing failed (with timeout)."); + return false; + } }