From 6e66b3b97d8d6663444349d1b377ae1de71c79f7 Mon Sep 17 00:00:00 2001
From: SebastianEbert <sebastian.ebert@tu-dresden.de>
Date: Fri, 30 Apr 2021 13:01:29 +0200
Subject: [PATCH] added plan only functionality
---
include/grasp_util.h | 4 ++--
src/grasping/grasp_util.cpp | 22 ++++++++++++++++++----
2 files changed, 20 insertions(+), 6 deletions(-)
diff --git a/include/grasp_util.h b/include/grasp_util.h
index a6545b0..8953a48 100644
--- a/include/grasp_util.h
+++ b/include/grasp_util.h
@@ -76,7 +76,7 @@ public:
*/
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);
+ std::string object_to_pick_id, bool plan_only = false);
/**
* Util method to place an object from above.
@@ -88,7 +88,7 @@ public:
* @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);
+ double open_amount, std::string supporting_surface_id, std::string object_to_place_id, bool plan_only = false);
};
diff --git a/src/grasping/grasp_util.cpp b/src/grasping/grasp_util.cpp
index 64f79e2..0a86985 100644
--- a/src/grasping/grasp_util.cpp
+++ b/src/grasping/grasp_util.cpp
@@ -75,7 +75,7 @@ GraspUtil::getPickFromAbovePose(geometry_msgs::Pose &object_pose_msg, geometry_m
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) {
+ 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);
@@ -112,7 +112,14 @@ GraspUtil::pickFromAbove(moveit::planning_interface::MoveGroupInterface &move_gr
}
move_group.setSupportSurfaceName(supporting_surface_id);
- moveit_msgs::MoveItErrorCodes miec = move_group.pick(object_to_pick_id, grasps);
+
+ moveit_msgs::MoveItErrorCodes miec;
+
+ if(plan_only){
+ miec = move_group.pick(object_to_pick_id, grasps, true);
+ }else{
+ miec = move_group.pick(object_to_pick_id, grasps);
+ }
if (miec.val == miec.SUCCESS) {
return true;
@@ -124,7 +131,7 @@ GraspUtil::pickFromAbove(moveit::planning_interface::MoveGroupInterface &move_gr
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) {
+ 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);
@@ -147,7 +154,14 @@ GraspUtil::placeFromAbove(moveit::planning_interface::MoveGroupInterface &move_g
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);
+
+ moveit_msgs::MoveItErrorCodes miec;
+
+ if(plan_only){
+ miec = move_group.place(object_to_place_id, place_location, true);
+ }else{
+ miec = move_group.place(object_to_place_id, place_location);
+ }
if (miec.val == miec.SUCCESS) {
return true;
--
GitLab