Better feedback on no grasps found

GT-RAIL · May 26, 2017 · 9f41b11 · 9f41b11
1 parent 048bcef
commit 9f41b11
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 1 deletion.
diff --git a/agile_grasp_base/src/agile_grasp/grasp_localizer.cpp b/agile_grasp_base/src/agile_grasp/grasp_localizer.cpp
@@ -144,7 +144,10 @@ void GraspLocalizer::executeFind(const rail_agile_grasp_msgs::FindGraspsGoalCons
         finalGrasps.grasps = createRailGraspsMsg(hands_);
         finalGrasps.workspace = goal->workspace;
         grasps_with_workspace_pub_.publish(finalGrasps);
-        result.success = true;
+        if (finalGrasps.grasps.grasps.empty())
+          result.success = false;
+        else
+          result.success = true;
         server.setSucceeded(result);
         return;
       }

diff --git a/rail_agile_nodes/src/GraspSampler.cpp b/rail_agile_nodes/src/GraspSampler.cpp
@@ -254,6 +254,8 @@ void GraspSampler::graspsCallback(const rail_agile_grasp_msgs::GraspsWithWorkspa
     if (croppedCloud->size() == 0)
     {
       ROS_INFO("No points in plane-removed point cloud, cannot calculate grasps...");
+      finalPoses.poses.clear();
+      graspsPublisher.publish(finalPoses);
       return;
     }
     kdTree->setInputCloud(croppedCloud);
@@ -284,6 +286,8 @@ void GraspSampler::graspsCallback(const rail_agile_grasp_msgs::GraspsWithWorkspa
     if (clusters.empty())
     {
       ROS_INFO("No clusters could be extracted after plane removal, cannot calculate grasps...");
+      finalPoses.poses.clear();
+      graspsPublisher.publish(finalPoses);
       return;
     }