Filter by suction voxel overlap in parallel

include/moveit_grasps/grasp_candidate.h

@@ -73,6 +73,46 @@ struct GraspFilterCode
     GRASP_INVALID,                    // An error occured while processing the grasp
     LAST                              // Used to track last value in the base class when inheriting
   };
+
+  static bool errorCodeToString(int code, std::string& code_string)
+  {
+    if (code == NOT_FILTERED)
+    {
+      code_string = "GraspFilterCode::NOT_FILTERED";
+      return true;
+    }
+    else if (code == GRASP_FILTERED_BY_IK)
+    {
+      code_string = "GraspFilterCode::GRASP_FILTERED_BY_IK";
+      return true;
+    }
+    else if (code == GRASP_FILTERED_BY_CUTTING_PLANE)
+    {
+      code_string = "GraspFilterCode::GRASP_FILTERED_BY_CUTTING_PLANE";
+      return true;
+    }
+    else if (code == GRASP_FILTERED_BY_ORIENTATION)
+    {
+      code_string = "GraspFilterCode::GRASP_FILTERED_BY_ORIENTATION";
+      return true;
+    }
+    else if (code == GRASP_FILTERED_BY_IK_CLOSED)
+    {
+      code_string = "GraspFilterCode::GRASP_FILTERED_BY_IK_CLOSED";
+      return true;
+    }
+    else if (code == PREGRASP_FILTERED_BY_IK)
+    {
+      code_string = "GraspFilterCode::PREGRASP_FILTERED_BY_IK";
+      return true;
+    }
+    else if (code == GRASP_INVALID)
+    {
+      code_string = "GraspFilterCode::GRASP_INVALID";
+      return true;
+    }
+    return false;
+  }
 };
 
 /**

include/moveit_grasps/grasp_filter.h

@@ -48,6 +48,7 @@
 // Grasping
 #include <moveit_grasps/grasp_generator.h>
 #include <moveit_grasps/grasp_candidate.h>
+#include <moveit_grasps/state_validity_callback.h>
 
 // Rviz
 #include <moveit_visual_tools/moveit_visual_tools.h>
@@ -111,7 +112,8 @@ struct IkThreadStruct
                  const planning_scene::PlanningScenePtr& planning_scene, Eigen::Isometry3d& link_transform,
                  std::size_t grasp_id, kinematics::KinematicsBaseConstPtr kin_solver,
                  const robot_state::RobotStatePtr& robot_state, double timeout, bool filter_pregrasp, bool visual_debug,
-                 std::size_t thread_id, const std::string& grasp_target_object_id)
+                 std::size_t thread_id, const std::string& grasp_target_object_id,
+                 const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools, double animation_speed)
     : grasp_candidates_(grasp_candidates)
     , planning_scene_(planning_scene::PlanningScene::clone(planning_scene))
     , link_transform_(link_transform)
@@ -123,6 +125,9 @@ struct IkThreadStruct
     , visual_debug_(visual_debug)
     , thread_id_(thread_id)
     , grasp_target_object_id_(grasp_target_object_id)
+    , constraint_fn_(boost::bind(
+        &isGraspStateValid, planning_scene_.get(),
+        visual_debug, animation_speed, visual_tools, _1, _2, _3))
   {
   }
 
@@ -144,6 +149,9 @@ struct IkThreadStruct
   // Used within processing function
   geometry_msgs::PoseStamped ik_pose_;  // Set from grasp candidate
   std::vector<double> ik_seed_state_;
+
+  // Used for IK animation
+  const moveit::core::GroupStateValidityCallbackFn constraint_fn_;
 };
 typedef std::shared_ptr<IkThreadStruct> IkThreadStructPtr;
 
@@ -280,8 +288,7 @@ class GraspFilter
    * \return true on success
    */
   bool findIKSolution(std::vector<double>& ik_solution, const IkThreadStructPtr& ik_thread_struct,
-                      const GraspCandidatePtr& grasp_candidate,
-                      const moveit::core::GroupStateValidityCallbackFn& constraint_fn) const;
+                      const GraspCandidatePtr& grasp_candidate) const;
 
   /**
    * \brief add a cutting plane

include/moveit_grasps/grasp_planner.h

@@ -143,6 +143,9 @@ class GraspPlanner
   // A shared node handle
   ros::NodeHandle nh_;
 
+  // Name for logging
+  std::string name_;
+
   // Class for publishing stuff to rviz
   moveit_visual_tools::MoveItVisualToolsPtr visual_tools_;
 

include/moveit_grasps/suction_grasp_candidate.h

@@ -51,6 +51,19 @@ struct SuctionGraspFilterCode : public GraspFilterCode
   {
     GRASP_FILTERED_BY_SUCTION_VOXEL_OVERLAP = LAST + 1,  // No suction voxel is in sufficient contact with the target
   };
+
+  static bool errorCodeToString(int code, std::string& code_string)
+  {
+    if (code == GRASP_FILTERED_BY_SUCTION_VOXEL_OVERLAP)
+    {
+      code_string = "SuctionGraspFilterCode::GRASP_FILTERED_BY_SUCTION_VOXEL_OVERLAP";
+      return true;
+    }
+    else
+    {
+      return GraspFilterCode::errorCodeToString(code, code_string);
+    }
+  }
 };
 
 /**

include/moveit_grasps/suction_grasp_filter.h

@@ -54,20 +54,6 @@ class SuctionGraspFilter : public GraspFilter
   SuctionGraspFilter(const robot_state::RobotStatePtr& robot_state,
                      const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools);
 
-  /**
-   * \brief Return grasps that are kinematically feasible
-   * \param grasp_candidates - all possible grasps that this will test. this vector is returned modified
-   * \param arm_jmg - the arm to solve the IK problem on
-   * \param filter_pregrasp -whether to also check ik feasibility for the pregrasp position
-   * \param visualize - visualize IK filtering
-   * \return number of grasps remaining
-   */
-  std::size_t filterGraspsHelper(std::vector<GraspCandidatePtr>& grasp_candidates,
-                                 const planning_scene::PlanningScenePtr& planning_scene_monitor,
-                                 const robot_model::JointModelGroup* arm_jmg,
-                                 const moveit::core::RobotStatePtr& seed_state, bool filter_pregrasp, bool visualize,
-                                 const std::string& target_object_id = "") override;
-
   /**
    * \brief Print grasp filtering statistics
    */
@@ -79,10 +65,10 @@ class SuctionGraspFilter : public GraspFilter
   bool processCandidateGrasp(const IkThreadStructPtr& ik_thread_struct) override;
 
   /**
-   * \brief Filter grasps that do not have a valid suction voxel overlap
-   * \param grasp_candidates - all possible grasps that this will test. this vector is returned modified
+   * \brief Filter one grasp by checking if it has a suction voxel with a valid overlap
+   * \param grasp_candidate - a populated grasp candidate with a populated suction_voxel_overlap_ vector
    */
-  bool filterGraspsBySuctionVoxelOverlapCutoff(std::vector<GraspCandidatePtr>& grasp_candidates);
+  bool filterGraspBySuctionVoxelOverlapCutoff(const SuctionGraspCandidatePtr& suction_grasp_candidate) const;
 
   /**
    * \brief  For suction grippers, set the cutoff threshold used by preFilterBySuctionVoxelOverlap to
@@ -121,8 +107,8 @@ class SuctionGraspFilter : public GraspFilter
   // Name for logging
   const std::string name_;
 
-  // A cutoff threshold [0,1] where at least one suction voxe must have more than this fraction overlap
-  // with the target object
+  // A cutoff threshold [0,1] where at least one suction voxel must have more than this fraction overlap
+  // with the target object. Set to a negative value to disable this check. Initialized to -1
   double suction_voxel_overlap_cutoff_;
 
 };  // end of class

src/demo/grasp_pipeline_demo.cpp

@@ -63,6 +63,9 @@ namespace moveit_grasps_demo
 {
 static const std::string LOGNAME = "grasp_pipeline_demo";
 
+// Allow an interrupt to be called that waits for user input, useful for debugging
+typedef boost::function<void(std::string message)> WaitForNextStepCallback;
+
 namespace
 {
 bool isStateValid(const planning_scene::PlanningScene* planning_scene,
@@ -74,11 +77,17 @@ bool isStateValid(const planning_scene::PlanningScene* planning_scene,
   return !planning_scene->isStateColliding(*robot_state, group->getName());
 }
 
-void waitForNextStep(const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools, const std::string& prompt)
+void waitForNextStepBlocking(const moveit_visual_tools::MoveItVisualToolsPtr& visual_tools, const std::string& prompt)
 {
   visual_tools->prompt(prompt);
 }
 
+void waitForNextStepNonBlocking(const std::string& prompt)
+{
+  ROS_INFO_STREAM("waitForNextStepNonBlocking:" << prompt);
+  ros::Duration(0.5).sleep();
+}
+
 }  // end annonymous namespace
 
 class GraspPipelineDemo
@@ -141,6 +150,10 @@ class GraspPipelineDemo
 
   void setupGraspPipeline()
   {
+    // ---------------------------------------------------------------------------------------------
+    // Set waitForNextStep callback to be non-blocking
+    setWaitForNextStepCallback(boost::bind(&waitForNextStepNonBlocking, _1));
+
     // ---------------------------------------------------------------------------------------------
     // Load grasp data specific to our robot
     grasp_data_ =
@@ -183,7 +196,7 @@ class GraspPipelineDemo
     grasp_planner_ = std::make_shared<moveit_grasps::GraspPlanner>(visual_tools_);
 
     // MoveIt Grasps allows for a manual breakpoint debugging tool to be optionally passed in
-    grasp_planner_->setWaitForNextStepCallback(boost::bind(&waitForNextStep, visual_tools_, _1));
+    // grasp_planner_->setWaitForNextStepCallback(boost::bind(&waitForNextStepBlocking, visual_tools_, _1));
 
     // -----------------------------------------------------
     // Load the motion planning pipeline
@@ -466,6 +479,17 @@ class GraspPipelineDemo
     return success;
   }
 
+  void waitForNextStep(const std::string& message)
+  {
+    if (wait_for_next_step_callback_)
+      wait_for_next_step_callback_(message);
+  }
+
+  void setWaitForNextStepCallback(WaitForNextStepCallback callback)
+  {
+    wait_for_next_step_callback_ = callback;
+  }
+
 private:
   // A shared node handle
   ros::NodeHandle nh_;
@@ -501,6 +525,7 @@ class GraspPipelineDemo
   std::string ee_group_name_;
   std::string planning_group_name_;
 
+  WaitForNextStepCallback wait_for_next_step_callback_;
 };  // end of class
 
 }  // namespace