Return and use init result

Signed-off-by: methylDragon <[email protected]>
osrf · Nov 20, 2023 · 03afb8e · 03afb8e
1 parent 5563f3c
commit 03afb8e
Show file tree

Hide file tree

Showing 3 changed files with 9 additions and 5 deletions.
diff --git 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
@@ -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
@@ -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)