From 03afb8e898fa76ef41507727368769927de1d0ae Mon Sep 17 00:00:00 2001 From: methylDragon Date: Mon, 20 Nov 2023 11:58:02 -0800 Subject: [PATCH] Return and use init result Signed-off-by: methylDragon --- nexus_motion_planner/src/motion_plan_cache.cpp | 4 ++-- nexus_motion_planner/src/motion_plan_cache.hpp | 2 +- nexus_motion_planner/src/motion_planner_server.cpp | 8 ++++++-- 3 files changed, 9 insertions(+), 5 deletions(-) diff --git a/nexus_motion_planner/src/motion_plan_cache.cpp b/nexus_motion_planner/src/motion_plan_cache.cpp index 9ad674b..7e0d3d8 100644 --- a/nexus_motion_planner/src/motion_plan_cache.cpp +++ b/nexus_motion_planner/src/motion_plan_cache.cpp @@ -52,7 +52,7 @@ MotionPlanCache::MotionPlanCache(const rclcpp::Node::SharedPtr& node) db_ = loader.loadDatabase(); } -void MotionPlanCache::init( +bool MotionPlanCache::init( const std::string& db_path, uint32_t db_port, double exact_match_precision) { RCLCPP_INFO( @@ -62,7 +62,7 @@ void MotionPlanCache::init( exact_match_precision_ = exact_match_precision; db_->setParams(db_path, db_port); - db_->connect(); + return db_->connect(); } // ============================================================================= diff --git a/nexus_motion_planner/src/motion_plan_cache.hpp b/nexus_motion_planner/src/motion_plan_cache.hpp index 4f916ad..e637e35 100644 --- a/nexus_motion_planner/src/motion_plan_cache.hpp +++ b/nexus_motion_planner/src/motion_plan_cache.hpp @@ -88,7 +88,7 @@ class MotionPlanCache // it... explicit MotionPlanCache(const rclcpp::Node::SharedPtr& node); - void init( + bool init( const std::string& db_path = ":memory:", uint32_t db_port = 0, double exact_match_precision = 1e-6); diff --git a/nexus_motion_planner/src/motion_planner_server.cpp b/nexus_motion_planner/src/motion_planner_server.cpp index 58d9208..4f7dc27 100644 --- a/nexus_motion_planner/src/motion_planner_server.cpp +++ b/nexus_motion_planner/src/motion_planner_server.cpp @@ -304,8 +304,12 @@ auto MotionPlannerServer::on_configure(const LifecycleState& /*state*/) if (_cache_mode != PlannerDatabaseMode::Unset) { - _motion_plan_cache->init( - _cache_db_host, _cache_db_port, _cache_exact_match_tolerance); + if (!_motion_plan_cache->init( + _cache_db_host, _cache_db_port, _cache_exact_match_tolerance)) + { + RCLCPP_ERROR(this->get_logger(), "Could not init motion plan cache."); + return CallbackReturn::ERROR; + } } if (_use_move_group_interfaces)