Skip to content
Snippets Groups Projects
Commit 6e66b3b9 authored by Sebastian Ebert's avatar Sebastian Ebert
Browse files

added plan only functionality

parent f1ac4675
No related branches found
No related tags found
No related merge requests found
...@@ -76,7 +76,7 @@ public: ...@@ -76,7 +76,7 @@ public:
*/ */
bool pickFromAbove(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &pick_pose, 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, 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. * Util method to place an object from above.
...@@ -88,7 +88,7 @@ public: ...@@ -88,7 +88,7 @@ public:
* @param object_to_place_id ID of the object to place * @param object_to_place_id ID of the object to place
*/ */
bool placeFromAbove(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &place_pose, 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);
}; };
......
...@@ -75,7 +75,7 @@ GraspUtil::getPickFromAbovePose(geometry_msgs::Pose &object_pose_msg, geometry_m ...@@ -75,7 +75,7 @@ GraspUtil::getPickFromAbovePose(geometry_msgs::Pose &object_pose_msg, geometry_m
bool bool
GraspUtil::pickFromAbove(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &pick_pose, GraspUtil::pickFromAbove(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &pick_pose,
geometry_msgs::Vector3 dimensions, 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; std::vector<moveit_msgs::Grasp> grasps;
grasps.resize(1); grasps.resize(1);
...@@ -112,7 +112,14 @@ GraspUtil::pickFromAbove(moveit::planning_interface::MoveGroupInterface &move_gr ...@@ -112,7 +112,14 @@ GraspUtil::pickFromAbove(moveit::planning_interface::MoveGroupInterface &move_gr
} }
move_group.setSupportSurfaceName(supporting_surface_id); 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) { if (miec.val == miec.SUCCESS) {
return true; return true;
...@@ -124,7 +131,7 @@ GraspUtil::pickFromAbove(moveit::planning_interface::MoveGroupInterface &move_gr ...@@ -124,7 +131,7 @@ GraspUtil::pickFromAbove(moveit::planning_interface::MoveGroupInterface &move_gr
bool bool
GraspUtil::placeFromAbove(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &place_pose, 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; std::vector<moveit_msgs::PlaceLocation> place_location;
place_location.resize(1); place_location.resize(1);
...@@ -147,7 +154,14 @@ GraspUtil::placeFromAbove(moveit::planning_interface::MoveGroupInterface &move_g ...@@ -147,7 +154,14 @@ GraspUtil::placeFromAbove(moveit::planning_interface::MoveGroupInterface &move_g
setupOpenGripper(place_location[0].post_place_posture, open_amount); setupOpenGripper(place_location[0].post_place_posture, open_amount);
move_group.setSupportSurfaceName(supporting_surface_id); 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) { if (miec.val == miec.SUCCESS) {
return true; return true;
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment