Document functions and check if planner graph is empty

moveit · Sep 14, 2023 · 3b96cbc · 3b96cbc
1 parent 7152724
commit 3b96cbc
Show file tree

Hide file tree

Showing 4 changed files with 29 additions and 14 deletions.
diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/ompl_interface.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/ompl_interface.h
@@ -71,6 +71,10 @@ class OMPLInterface
 
   virtual ~OMPLInterface();
 
+  /** @brief Store the OMPL planner data in a file.
+   *  @param request the service request
+   *  @param response the service response
+   */
   void storePlannerData(const std::shared_ptr<std_srvs::srv::Trigger::Request> request,
                         std::shared_ptr<std_srvs::srv::Trigger::Response> response);
 

diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h
@@ -52,9 +52,10 @@ class MultiQueryPlannerAllocator
 {
 public:
   MultiQueryPlannerAllocator() = default;
-  ~MultiQueryPlannerAllocator();
 
-  void storePlannerData();
+  /** @brief Store the OMPL planner data in a file.
+   */
+  bool storePlannerData();
 
   template <typename T>
   ob::PlannerPtr allocatePlanner(const ob::SpaceInformationPtr& si, const std::string& new_name,
@@ -85,7 +86,9 @@ class PlanningContextManager
                          constraint_samplers::ConstraintSamplerManagerPtr csm);
   ~PlanningContextManager();
 
-  void storePlannerData();
+  /** @brief Store the OMPL planner data in a file.
+   */
+  bool storePlannerData();
 
   /** @brief Specify configurations for the planners.
       @param pconfig Configurations for the different planners */

diff --git a/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp b/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp
@@ -86,9 +86,16 @@ OMPLInterface::~OMPLInterface() = default;
 void OMPLInterface::storePlannerData(const std::shared_ptr<std_srvs::srv::Trigger::Request> /* unused */,
                                      std::shared_ptr<std_srvs::srv::Trigger::Response> response)
 {
-  context_manager_.storePlannerData();
-  response->success = true;
-  RCLCPP_INFO(LOGGER, "Stored motion planner data");
+  bool success = context_manager_.storePlannerData();
+  response->success = success;
+  if (success)
+  {
+    RCLCPP_INFO(LOGGER, "Stored motion planner data");
+  }
+  else
+  {
+    RCLCPP_ERROR(LOGGER, "Motion planning graph is empty");
+  }
 }
 
 void OMPLInterface::setPlannerConfigurations(const planning_interface::PlannerConfigurationMap& pconfig)

diff --git a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp
@@ -89,22 +89,23 @@ struct PlanningContextManager::CachedContexts
   std::mutex lock_;
 };
 
-MultiQueryPlannerAllocator::~MultiQueryPlannerAllocator()
-{
-  storePlannerData();
-}
-
-void MultiQueryPlannerAllocator::storePlannerData()
+bool MultiQueryPlannerAllocator::storePlannerData()
 {
   // Store all planner data
   for (const auto& entry : planner_data_storage_paths_)
   {
     ob::PlannerData data(planners_[entry.first]->getSpaceInformation());
     planners_[entry.first]->getPlannerData(data);
+    if (data.numVertices() == 0)
+    {
+      return false;
+    }
     RCLCPP_INFO_STREAM(LOGGER, "Storing planner data. NumEdges: " << data.numEdges()
                                                                   << ", NumVertices: " << data.numVertices());
     storage_.store(data, entry.second.c_str());
   }
+
+  return true;
 }
 
 template <typename T>
@@ -267,9 +268,9 @@ PlanningContextManager::PlanningContextManager(moveit::core::RobotModelConstPtr
 
 PlanningContextManager::~PlanningContextManager() = default;
 
-void PlanningContextManager::storePlannerData()
+bool PlanningContextManager::storePlannerData()
 {
-  planner_allocator_.storePlannerData();
+  return planner_allocator_.storePlannerData();
 }
 
 ConfiguredPlannerAllocator PlanningContextManager::plannerSelector(const std::string& planner) const