diff --git a/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp b/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp index af4bdfa063..e9865193ee 100644 --- a/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp +++ b/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp @@ -60,18 +60,9 @@ OMPLInterface::OMPLInterface(const moveit::core::RobotModelConstPtr& robot_model store_planner_data_service_ = node_->create_service( "store_planner_data", - [this](const std::shared_ptr& /* unused */, + [this](const std::shared_ptr& request, const std::shared_ptr& response) -> void { - 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"); - } + storePlannerData(request, response); }); } @@ -91,18 +82,9 @@ OMPLInterface::OMPLInterface(const moveit::core::RobotModelConstPtr& robot_model store_planner_data_service_ = node_->create_service( "store_planner_data", - [this](const std::shared_ptr& /* unused */, + [this](const std::shared_ptr& request, const std::shared_ptr& response) -> void { - 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"); - } + storePlannerData(request, response); }); }