More Verbose CpOdomTracker

robosoft-ai · Dec 11, 2024 · 9b2ff16 · 9b2ff16
1 parent 9a4d926
commit 9b2ff16
Show file tree

Hide file tree

Showing 84 changed files with 33 additions and 21,908 deletions.
diff --git a/smacc2_client_library/keyboard_client/include/keyboard_client/cl_keyboard.hpp b/smacc2_client_library/keyboard_client/include/keyboard_client/cl_keyboard.hpp
@@ -26,7 +26,7 @@
 
 namespace cl_keyboard
 {
-//----------------- TIMER sc::event DEFINITION ----------------------------------------------
+//----------------- KEYBOARD EVENT DEFINITIONS ----------------------------------------------
 template <typename TSource, typename TOrthogonal>
 struct EvKeyPressA : sc::event<EvKeyPressA<TSource, TOrthogonal>>
 {

diff --git a/...ry/nav2z_client/nav2z_client/src/nav2z_client/components/odom_tracker/cp_odom_tracker.cpp b/...ry/nav2z_client/nav2z_client/src/nav2z_client/components/odom_tracker/cp_odom_tracker.cpp
@@ -99,7 +99,6 @@ void CpOdomTracker::onInitialize()
  */
 void CpOdomTracker::setWorkingMode(WorkingMode workingMode)
 {
-  // RCLCPP_INFO(getLogger(),"odom_tracker m_mutex acquire");
   std::lock_guard<std::mutex> lock(m_mutex_);
 
   switch (workingMode)
@@ -126,6 +125,14 @@ void CpOdomTracker::setWorkingMode(WorkingMode workingMode)
       RCLCPP_INFO_STREAM(getLogger(), "[CpOdomTracker] setting working mode to <UNKNOWN>");
   }
 
+  if (this->odomSub_->get_publisher_count() == 0)
+  {
+    RCLCPP_ERROR_STREAM(
+      getLogger(),
+      "[CpOdomTracker] Odom tracker cannot record the path because there is no publisher to the "
+      "odometry topic");
+  }
+
   workingMode_ = workingMode;
   // RCLCPP_INFO(getLogger(),"odom_tracker m_mutex release");
 }
@@ -150,7 +157,15 @@ void CpOdomTracker::pushPath(std::string pathname)
 {
   RCLCPP_INFO(getLogger(), "odom_tracker m_mutex acquire");
   std::lock_guard<std::mutex> lock(m_mutex_);
+
   this->logStateString(false);
+  if (this->odomSub_->get_publisher_count() == 0)
+  {
+    RCLCPP_ERROR_STREAM(
+      getLogger(),
+      "[CpOdomTracker] Odom tracker cannot record the path because there is no publisher to the "
+      "odometry topic");
+  }
 
   pathInfos_.push_back({currentPathName_, this->currentMotionGoal_});
   pathStack_.push_back(baseTrajectory_);
@@ -182,6 +197,14 @@ void CpOdomTracker::popPath(int popCount, bool keepPreviousPath)
   RCLCPP_INFO(getLogger(), "POP PATH ENTRY");
   this->logStateString();
 
+  if (this->odomSub_->get_publisher_count() == 0)
+  {
+    RCLCPP_ERROR_STREAM(
+      getLogger(),
+      "[CpOdomTracker] Odom tracker cannot record the path because there is no publisher to the "
+      "odometry topic");
+  }
+
   if (!keepPreviousPath)
   {
     baseTrajectory_.poses.clear();
@@ -239,6 +262,14 @@ void CpOdomTracker::logStateString(bool debug)
   }
   ss << "---";
 
+  if (this->odomSub_->get_publisher_count() == 0)
+  {
+    ss << std::endl
+       << "[CpOdomTracker] Odom tracker cannot record the path because there is no publisher to "
+          "the odometry topic"
+       << std::endl;
+  }
+
   if (debug)
     RCLCPP_DEBUG(getLogger(), ss.str().c_str());
   else