diff --git a/CMakeLists.txt b/CMakeLists.txt index 3f2d73c..c7c91fe 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -98,6 +98,8 @@ find_package(graspit) ApproachToContact.srv FindInitialContact.srv DynamicAutoGraspComplete.srv + GetCheckCollision.srv + SetCheckCollision.srv ) diff --git a/include/graspit_interface.h b/include/graspit_interface.h index f11078c..bb17beb 100644 --- a/include/graspit_interface.h +++ b/include/graspit_interface.h @@ -46,6 +46,8 @@ #include #include #include +#include +#include // ActionServer includes #include @@ -77,6 +79,9 @@ class GraspitInterface : public QObject, public Plugin ros::ServiceServer getDynamics_srv; ros::ServiceServer setDynamics_srv; + ros::ServiceServer getCheckCollision_srv; + ros::ServiceServer setCheckCollision_srv; + ros::ServiceServer autoGrasp_srv; ros::ServiceServer autoOpen_srv; ros::ServiceServer forceRobotDOF_srv; @@ -112,6 +117,7 @@ class GraspitInterface : public QObject, public Plugin SimAnnPlanner *mPlanner; bool firstTimeInMainLoop; + bool checkCollision; // Service callbacks bool getRobotCB(graspit_interface::GetRobot::Request &request, @@ -201,6 +207,12 @@ class GraspitInterface : public QObject, public Plugin bool dynamicAutoGraspCompleteCB(graspit_interface::DynamicAutoGraspComplete::Request &request, graspit_interface::DynamicAutoGraspComplete::Response &response); + bool getCheckCollisionCB(graspit_interface::GetCheckCollision::Request &request, + graspit_interface::GetCheckCollision::Response &response); + + bool setCheckCollisionCB(graspit_interface::SetCheckCollision::Request &request, + graspit_interface::SetCheckCollision::Response &response); + //ActionServer callbacks void PlanGraspsCB(const graspit_interface::PlanGraspsGoalConstPtr &goal); diff --git a/src/graspit_interface.cpp b/src/graspit_interface.cpp index f58c6b5..baa8923 100644 --- a/src/graspit_interface.cpp +++ b/src/graspit_interface.cpp @@ -58,6 +58,9 @@ int GraspitInterface::init(int argc, char** argv) getDynamics_srv = nh->advertiseService("getDynamics", &GraspitInterface::getDynamicsCB, this); setDynamics_srv = nh->advertiseService("setDynamics", &GraspitInterface::setDynamicsCB, this); + getCheckCollision_srv = nh->advertiseService("getCheckCollision", &GraspitInterface::getCheckCollisionCB, this); + setCheckCollision_srv = nh->advertiseService("setCheckCollision", &GraspitInterface::setCheckCollisionCB, this); + autoGrasp_srv = nh->advertiseService("autoGrasp", &GraspitInterface::autoGraspCB, this); autoOpen_srv = nh->advertiseService("autoOpen", &GraspitInterface::autoOpenCB, this); @@ -88,6 +91,7 @@ int GraspitInterface::init(int argc, char** argv) plan_grasps_as->start(); firstTimeInMainLoop = true; + checkCollision = true; mPlanner = NULL; mHandObjectState = NULL; @@ -388,6 +392,20 @@ bool GraspitInterface::setDynamicsCB(graspit_interface::SetDynamics::Request &re return true; } +bool GraspitInterface::getCheckCollisionCB(graspit_interface::GetCheckCollision::Request &request, + graspit_interface::GetCheckCollision::Response &response) +{ + response.collisionCheckEnabled = checkCollision; + return true; +} + +bool GraspitInterface::setCheckCollisionCB(graspit_interface::SetCheckCollision::Request &request, + graspit_interface::SetCheckCollision::Response &response) +{ + checkCollision = request.enableCollisionCheck; + return true; +} + bool GraspitInterface::autoGraspCB(graspit_interface::AutoGrasp::Request &request, graspit_interface::AutoGrasp::Response &response) { @@ -643,15 +661,20 @@ bool GraspitInterface::computeQualityCB(graspit_interface::ComputeQuality::Reque graspit_interface::ComputeQuality::Response &response) { CollisionReport colReport; - // first test whether the hand is in collision now - int numCols = graspitCore->getWorld()->getCollisionReport(&colReport); - // if it is in collision, then there should be no reason to calculate the quality - if(numCols>0){ - response.result = response.RESULT_COLLISION; - response.epsilon = -1.0; - response.volume = -1.0; - return true; + + if (checkCollision) + { + // first test whether the hand is in collision now + int numCols = graspitCore->getWorld()->getCollisionReport(&colReport); + // if it is in collision, then there should be no reason to calculate the quality + if(numCols>0){ + response.result = response.RESULT_COLLISION; + response.epsilon = -1.0; + response.volume = -1.0; + return true; + } } + Hand *mHand =graspitCore->getWorld()->getHand(request.id); if (mHand==NULL) { diff --git a/srv/GetCheckCollision.srv b/srv/GetCheckCollision.srv new file mode 100644 index 0000000..7751436 --- /dev/null +++ b/srv/GetCheckCollision.srv @@ -0,0 +1,3 @@ + +--- +bool collisionCheckEnabled diff --git a/srv/SetCheckCollision.srv b/srv/SetCheckCollision.srv new file mode 100644 index 0000000..e9815bc --- /dev/null +++ b/srv/SetCheckCollision.srv @@ -0,0 +1,2 @@ +bool enableCollisionCheck +---