From 9dcb9fb2b86229aecb1d9dd57ea2e88fd38083e6 Mon Sep 17 00:00:00 2001
From: SebastianEbert <sebastian.ebert@tu-dresden.de>
Date: Tue, 29 Jun 2021 13:58:54 +0200
Subject: [PATCH] extension of services
---
include/grasp_util.h | 12 ++++++------
src/grasping/grasp_service.cpp | 8 ++++++--
src/grasping/grasp_util.cpp | 1 -
srv/PickObject.srv | 1 +
srv/PlaceObject.srv | 1 +
5 files changed, 14 insertions(+), 9 deletions(-)
diff --git a/include/grasp_util.h b/include/grasp_util.h
index ff34343..a615e5e 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 7e27a2a..ba89482 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 a51d2e5..4371cff 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 a7f8eb4..3d4a030 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 a7f8eb4..3d4a030 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
--
GitLab