Brettpac branch (#568)

* Modifying DEBUG to INFO for undo_path_global_planner * undoing if statement * Commenting clnav2z * Minor comments * tweaking comments * Bob's comment review * oops * More Verbose CpOdomTracker --------- Co-authored-by: brettpac <[email protected]>
robosoft-ai · Dec 13, 2024 · 4763cda · 4763cda
1 parent 9724b5d
commit 4763cda
Show file tree

Hide file tree

Showing 86 changed files with 47 additions and 21,926 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/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/nav2z_client.hpp b/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/nav2z_client.hpp
@@ -1,4 +1,4 @@
-// Copyright 2021 RobosoftAI Inc.
+// Copyright 2024 Robosoft Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -12,32 +12,30 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-/*****************************************************************************************************************
- *
- * 	 Authors: Pablo Inigo Blasco, Brett Aldrich
- *
- ******************************************************************************************************************/
 #pragma once
-
+#include <nav2_msgs/action/navigate_to_pose.hpp>
 #include <smacc2/client_bases/smacc_action_client_base.hpp>
 #include <smacc2/smacc.hpp>
 
-#include <nav2_msgs/action/navigate_to_pose.hpp>
-
 namespace cl_nav2z
 {
+// Declare Client class, inherit from SmaccActionClientBase template with default action argument.
 class ClNav2Z
 : public smacc2::client_bases::SmaccActionClientBase<nav2_msgs::action::NavigateToPose>
 {
 public:
+  // Bring the SmaccActionClientBase types into the current scope.
   using smacc2::client_bases::SmaccActionClientBase<nav2_msgs::action::NavigateToPose>::GoalHandle;
   using smacc2::client_bases::SmaccActionClientBase<
     nav2_msgs::action::NavigateToPose>::ResultCallback;
 
+  // Define "SmaccNavigateResultSignal" as a SmaccSignal that points to the WrappedResult type of SmaccActionClientBase.
   typedef smacc2::SmaccSignal<void(const WrappedResult &)> SmaccNavigateResultSignal;
 
+  // Declare Client class constructor with string set to the right ROS action.
   ClNav2Z(std::string navigateToPoseAction = "/navigate_to_pose");
 
+  // Declare Client class destructor.
   virtual ~ClNav2Z();
 };
 

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

diff --git a/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/nav2z_client.cpp b/smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/nav2z_client.cpp
@@ -1,4 +1,4 @@
-// Copyright 2021 RobosoftAI Inc.
+// Copyright 2024 Robosoft Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -12,28 +12,26 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-/*****************************************************************************************************************
- *
- * 	 Authors: Pablo Inigo Blasco, Brett Aldrich
- *
- ******************************************************************************************************************/
 #include <nav2z_client/nav2z_client.hpp>
-
-#include <string>
-
 #include <pluginlib/class_list_macros.hpp>
+#include <string>
 
 namespace cl_nav2z
 {
+// Define "Base" as a SmaccActionClientBase with NavigateToPose as action argument, tie to WrappedResult.
 typedef smacc2::client_bases::SmaccActionClientBase<nav2_msgs::action::NavigateToPose> Base;
 typedef Base::WrappedResult WrappedResult;
 
+// Define Client class constructor.
 ClNav2Z::ClNav2Z(std::string moveBaseName) : Base(moveBaseName)
 {
   // RCLCPP_INFO(getLogger(),"Smacc Move Base Action Client");
 }
 
+// Define Client class destructor.
 ClNav2Z::~ClNav2Z() {}
+
 }  // namespace cl_nav2z
 
+// Export the ClNav2Z class as type smacc2::ISmaccClient as an implementation of the ISmaccClient interface.
 PLUGINLIB_EXPORT_CLASS(cl_nav2z::ClNav2Z, smacc2::ISmaccClient)