Update "Store OMPL Planning Data" PR

moveit_planners/ompl/ompl_interface/CMakeLists.txt

@@ -23,6 +23,7 @@ set_target_properties(moveit_ompl_interface
                       PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
 
 find_package(OpenMP REQUIRED)
+find_package(std_srvs)
 
 # Used to link in ODE, an OMPL dependency, on macOS
 if(APPLE)
@@ -36,6 +37,7 @@ ament_target_dependencies(
   moveit_ros_planning
   rclcpp
   pluginlib
+  std_srvs
   tf2_eigen
   tf2_ros
   OMPL

moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/ompl_interface.h

@@ -42,9 +42,10 @@
 #include <moveit/planning_interface/planning_interface.h>
 #include <moveit_msgs/msg/motion_plan_request.hpp>
 #include <moveit_msgs/msg/motion_plan_response.hpp>
-#include <rclcpp/node.hpp>
 #include <string>
 #include <map>
+#include <rclcpp/rclcpp.hpp>
+#include <std_srvs/srv/trigger.hpp>
 
 /** \brief The MoveIt interface to OMPL */
 namespace ompl_interface
@@ -69,6 +70,13 @@ 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,
+                        const std::shared_ptr<std_srvs::srv::Trigger::Response>& response);
+
   /** @brief Specify configurations for the planners.
       @param pconfig Configurations for the different planners */
   void setPlannerConfigurations(const planning_interface::PlannerConfigurationMap& pconfig);
@@ -151,5 +159,7 @@ class OMPLInterface
 
 private:
   constraint_sampler_manager_loader::ConstraintSamplerManagerLoaderPtr constraint_sampler_manager_loader_;
+
+  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr store_planner_data_service_;
 };
 }  // namespace ompl_interface

moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/planning_context_manager.h

@@ -52,7 +52,10 @@ class MultiQueryPlannerAllocator
 {
 public:
   MultiQueryPlannerAllocator() = default;
-  ~MultiQueryPlannerAllocator();
+
+  /** @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,
@@ -83,6 +86,10 @@ class PlanningContextManager
                          constraint_samplers::ConstraintSamplerManagerPtr csm);
   ~PlanningContextManager();
 
+  /** @brief Store the OMPL planner data in a file.
+   */
+  bool storePlannerData();
+
   /** @brief Specify configurations for the planners.
       @param pconfig Configurations for the different planners */
   void setPlannerConfigurations(const planning_interface::PlannerConfigurationMap& pconfig);

moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp

@@ -63,6 +63,13 @@ OMPLInterface::OMPLInterface(const moveit::core::RobotModelConstPtr& robot_model
   RCLCPP_DEBUG(getLogger(), "Initializing OMPL interface using ROS parameters");
   loadPlannerConfigurations();
   loadConstraintSamplers();
+
+  store_planner_data_service_ = node_->create_service<std_srvs::srv::Trigger>(
+      "store_planner_data",
+      [this](const std::shared_ptr<std_srvs::srv::Trigger::Request>& request,
+             const std::shared_ptr<std_srvs::srv::Trigger::Response>& response) -> void {
+        storePlannerData(request, response);
+      });
 }
 
 OMPLInterface::OMPLInterface(const moveit::core::RobotModelConstPtr& robot_model,
@@ -78,10 +85,27 @@ OMPLInterface::OMPLInterface(const moveit::core::RobotModelConstPtr& robot_model
   RCLCPP_DEBUG(getLogger(), "Initializing OMPL interface using specified configuration");
   setPlannerConfigurations(pconfig);
   loadConstraintSamplers();
+
+  store_planner_data_service_ = node_->create_service<std_srvs::srv::Trigger>(
+      "store_planner_data",
+      [this](const std::shared_ptr<std_srvs::srv::Trigger::Request>& request,
+             const std::shared_ptr<std_srvs::srv::Trigger::Response>& response) -> void {
+        storePlannerData(request, response);
+      });
 }
 
 OMPLInterface::~OMPLInterface() = default;
 
+void OMPLInterface::storePlannerData(const std::shared_ptr<std_srvs::srv::Trigger::Request>& /* unused */,
+                                     const std::shared_ptr<std_srvs::srv::Trigger::Response>& response)
+{
+  response->success = context_manager_.storePlannerData();
+  if (!response->success)
+  {
+    RCLCPP_ERROR(getLogger(), "Motion planning graph is empty");
+  }
+}
+
 void OMPLInterface::setPlannerConfigurations(const planning_interface::PlannerConfigurationMap& pconfig)
 {
   planning_interface::PlannerConfigurationMap pconfig2 = pconfig;

moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp

@@ -96,17 +96,24 @@ struct PlanningContextManager::CachedContexts
   std::mutex lock_;
 };
 
-MultiQueryPlannerAllocator::~MultiQueryPlannerAllocator()
+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)
+    {
+      RCLCPP_ERROR_STREAM(getLogger(), "Could not fetch space information or space information is empty.");
+      return false;
+    }
     RCLCPP_INFO_STREAM(getLogger(), "Storing planner data. NumEdges: " << data.numEdges()
                                                                        << ", NumVertices: " << data.numVertices());
     storage_.store(data, entry.second.c_str());
   }
+
+  return true;
 }
 
 template <typename T>
@@ -269,6 +276,11 @@ PlanningContextManager::PlanningContextManager(moveit::core::RobotModelConstPtr
 
 PlanningContextManager::~PlanningContextManager() = default;
 
+bool PlanningContextManager::storePlannerData()
+{
+  return planner_allocator_.storePlannerData();
+}
+
 ConfiguredPlannerAllocator PlanningContextManager::plannerSelector(const std::string& planner) const
 {
   auto it = known_planners_.find(planner);