diff --git a/include/grasp_util.h b/include/grasp_util.h index ff343430432a336e9d8a13888f3fc1c5aa4ae98b..a615e5ee8ba44d74411a6b09f7aaac89a62d4c1f 100644 --- a/include/grasp_util.h +++ b/include/grasp_util.h @@ -92,12 +92,12 @@ public: bool plan_only = false, int max_planning_retries = 1); /** - * TODO: TEST - * @param move_group - * @param object_pose_msg - * @param open_amount - * @param dimensions - * @return + * TODO: TEST / FIX + * @param move_group an initialized MoveGroupInterface instance + * @param object_pose_msg pose of the object to pick + * @param open_amount total opening amount (space between "forks") + * @param dimensions of the object + * @return true on success */ bool pickObjectOnRobotfrontFromSide(moveit::planning_interface::MoveGroupInterface &move_group, std::string object_to_pick_id, geometry_msgs::Pose &object_pose_msg, double open_amount, geometry_msgs::Vector3 dimensions, int max_planning_retries = 1); diff --git a/src/grasping/grasp_service.cpp b/src/grasping/grasp_service.cpp index 7e27a2a4de28d4004599d4bd1a48a57efdc080fe..ba894822cf9da41bec6274d616368f98ced5e233 100644 --- a/src/grasping/grasp_service.cpp +++ b/src/grasping/grasp_service.cpp @@ -73,8 +73,10 @@ bool pickObject(panda_grasping::PickObject::Request &req, geometry_msgs::Pose pick_pose = grasp_util.getPickFromAbovePose(req.pose, req.dimensions, gripper_transform.rotation); ROS_INFO("Picking up ..."); + int planing_retries = req.planing_retries <= 0 ? 1 : req.planing_retries; + // 0.08 = 8cm = maximum gripper opening - if (!grasp_util.pickFromAbove(group, pick_pose, req.dimensions, 0.08, "pick_support", "pick_object", req.plan_only)) { + if (!grasp_util.pickFromAbove(group, pick_pose, req.dimensions, 0.08, "pick_support", "pick_object", req.plan_only, planing_retries)) { res.success = false; return false; } @@ -121,7 +123,9 @@ placeObject(panda_grasping::PlaceObject::Request &req, panda_grasping::PlaceObje } ROS_INFO("Placing object."); - if (!grasp_util.placeFromAbove(group, req.pose, 0.08, "place_support", "pick_object", req.plan_only)) { + int planing_retries = req.planing_retries <= 0 ? 1 : req.planing_retries; + + if (!grasp_util.placeFromAbove(group, req.pose, 0.08, "place_support", "pick_object", req.plan_only, planing_retries)) { res.success = false; return false; } diff --git a/src/grasping/grasp_util.cpp b/src/grasping/grasp_util.cpp index a51d2e5def48b26358f6bbb4e6d32f62f703d06b..4371cfff462a535bee8aaaba50fe02ea88738750 100644 --- a/src/grasping/grasp_util.cpp +++ b/src/grasping/grasp_util.cpp @@ -75,7 +75,6 @@ GraspUtil::getPickFromAbovePose(geometry_msgs::Pose &object_pose_msg, geometry_m bool GraspUtil::pickObjectOnRobotfrontFromSide(moveit::planning_interface::MoveGroupInterface &move_group, std::string object_to_pick_id, geometry_msgs::Pose &object_pose_msg, double open_amount, geometry_msgs::Vector3 dimensions, int max_planning_retries) { - std::vector<moveit_msgs::Grasp> grasps; grasps.resize(1); diff --git a/srv/PickObject.srv b/srv/PickObject.srv index a7f8eb4ec899df0ded89677aec37db7d5c5c7ee0..3d4a0307469ab974bb7a3d0a77c9db31ca8544db 100644 --- a/srv/PickObject.srv +++ b/srv/PickObject.srv @@ -1,5 +1,6 @@ geometry_msgs/Pose pose geometry_msgs/Vector3 dimensions bool plan_only +int8 planing_retries --- bool success \ No newline at end of file diff --git a/srv/PlaceObject.srv b/srv/PlaceObject.srv index a7f8eb4ec899df0ded89677aec37db7d5c5c7ee0..3d4a0307469ab974bb7a3d0a77c9db31ca8544db 100644 --- a/srv/PlaceObject.srv +++ b/srv/PlaceObject.srv @@ -1,5 +1,6 @@ geometry_msgs/Pose pose geometry_msgs/Vector3 dimensions bool plan_only +int8 planing_retries --- bool success \ No newline at end of file