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 index ec86ad47162..0065e506c9d 100644 --- 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 request, std::shared_ptr 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 index 719b55c3a75..66a4cdc3642 100644 --- 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 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 index 42907e84498..90c2f073f18 100644 --- 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 /* unused */, std::shared_ptr 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 index 138b59f50a2..b8e0d4d5ba8 100644 --- 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 @@ -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