Thrilla in Manilla (#570)

* Thrilla in Manilla

* Fixing Format issue

---------

Co-authored-by: brettpac <brettpac@system76-pc.localdomain>

smacc2_client_library/nav2z_client/custom_planners/undo_path_global_planner/src/undo_path_global_planner/undo_path_global_planner.cpp

@@ -112,9 +112,10 @@ void UndoPathGlobalPlanner::configure(
 void UndoPathGlobalPlanner::onForwardTrailMsg(const nav_msgs::msg::Path::SharedPtr forwardPath)
 {
   lastForwardPathMsg_ = *forwardPath;
-  RCLCPP_INFO_STREAM(
-    nh_->get_logger(), "[UndoPathGlobalPlanner] received backward path msg poses ["
-                         << lastForwardPathMsg_.poses.size() << "]");
+  RCLCPP_INFO_STREAM_THROTTLE(
+    nh_->get_logger(), *nh_, 1000,
+    "[UndoPathGlobalPlanner] received backward path msg poses [" << lastForwardPathMsg_.poses.size()
+                                                                 << "]");
 }
 
 /**
@@ -451,6 +452,9 @@ nav_msgs::msg::Path UndoPathGlobalPlanner::createPlan(
     }
   }
 
+  // if(planMsg.poses.size() == 1)
+  //   planMsg.poses.clear();
+
   //--------  PUBLISHING RESULTS ---------------------------------------
   RCLCPP_INFO_STREAM(
     nh_->get_logger(), "[UndoPathGlobalPlanner] plan publishing. size: " << plan.size());

smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/components/odom_tracker/cp_odom_tracker.hpp

@@ -35,6 +35,7 @@
 #include <nav_msgs/msg/odometry.hpp>
 #include <nav_msgs/msg/path.hpp>
 
+#include <nav2z_client/components/pose/cp_pose.hpp>
 #include <std_msgs/msg/header.hpp>
 
 namespace cl_nav2z
@@ -48,19 +49,31 @@ enum class WorkingMode : uint8_t
   IDLE = 2
 };
 
+enum class OdomTrackerStrategy
+{
+  ODOMETRY_SUBSCRIBER,
+  POSE_COMPONENT
+};
+
 /// This object tracks and saves the trajectories performed by the vehicle
 /// so that they can be used later to execute operations such as "undo motions"
 class CpOdomTracker : public smacc2::ISmaccComponent
 {
 public:
   // by default, the component start in record_path mode and publishing the
   // current path
-  CpOdomTracker(std::string odomtopicName = "/odom", std::string odomFrame = "odom");
+  CpOdomTracker(
+    std::string odomtopicName = "/odom", std::string odomFrame = "odom",
+    OdomTrackerStrategy strategy = OdomTrackerStrategy::ODOMETRY_SUBSCRIBER);
 
   // threadsafe
   /// odom callback: Updates the path - this must be called periodically for each odometry message.
   // The odom parameters is the main input of this tracker
-  virtual void processOdometryMessage(const nav_msgs::msg::Odometry::SharedPtr odom);
+  virtual void processNewPose(const geometry_msgs::msg::PoseStamped & odom);
+
+  virtual void odomMessageCallback(const nav_msgs::msg::Odometry::SharedPtr odom);
+
+  virtual void update();
 
   // ------ CONTROL COMMANDS ---------------------
   // threadsafe
@@ -100,6 +113,15 @@ class CpOdomTracker : public smacc2::ISmaccComponent
 
   void logStateString(bool debug = true);
 
+  // parameters update callback
+  void updateParameters();
+
+  inline void setOdomFrame(std::string odomFrame)
+  {
+    odomFrame_ = odomFrame;
+    getNode()->set_parameter(rclcpp::Parameter("odom_frame", odomFrame));
+  }
+
 protected:
   void onInitialize() override;
 
@@ -108,10 +130,10 @@ class CpOdomTracker : public smacc2::ISmaccComponent
   virtual void rtPublishPaths(rclcpp::Time timestamp);
 
   // this is called when a new odom message is received in record path mode
-  virtual bool updateRecordPath(const nav_msgs::msg::Odometry & odom);
+  virtual bool updateRecordPath(const geometry_msgs::msg::PoseStamped & odom);
 
   // this is called when a new odom message is received in clear path mode
-  virtual bool updateClearPath(const nav_msgs::msg::Odometry & odom);
+  virtual bool updateClearPath(const geometry_msgs::msg::PoseStamped & odom);
 
   void updateAggregatedStackPath();
 
@@ -124,6 +146,9 @@ class CpOdomTracker : public smacc2::ISmaccComponent
   // without any subscriber
   rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odomSub_;
 
+  cl_nav2z::Pose * robotPose_;
+  rclcpp::TimerBase::SharedPtr robotPoseTimer_;
+
   // -------------- PARAMETERS ----------------------
   /// How much distance there is between two points of the path
   double recordPointDistanceThreshold_;
@@ -162,12 +187,16 @@ class CpOdomTracker : public smacc2::ISmaccComponent
 
   nav_msgs::msg::Path aggregatedStackPathMsg_;
 
-  // subscribes to topic on init if true
-  bool subscribeToOdometryTopic_;
+  OdomTrackerStrategy strategy_;
 
   std::optional<geometry_msgs::msg::PoseStamped> currentMotionGoal_;
   std::string currentPathName_;
 
+  rcl_interfaces::msg::SetParametersResult parametersCallback(
+    const std::vector<rclcpp::Parameter> & parameters);
+
+  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_callback_handle_;
+
   std::mutex m_mutex_;
 };
 

smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/components/pose/cp_pose.hpp

@@ -62,6 +62,7 @@ class Pose : public smacc2::ISmaccComponent, public smacc2::ISmaccUpdatable
 
   inline geometry_msgs::msg::PoseStamped toPoseStampedMsg()
   {
+    RCLCPP_INFO_STREAM(getLogger(), "[Pose] ToPoseMsg ");
     std::lock_guard<std::mutex> guard(m_mutex_);
     return this->pose_;
   }
@@ -73,12 +74,22 @@ class Pose : public smacc2::ISmaccComponent, public smacc2::ISmaccUpdatable
   float getY();
   float getZ();
 
+  inline void setReferenceFrame(std::string referenceFrame) { referenceFrame_ = referenceFrame; }
+
   inline const std::string & getReferenceFrame() const { return referenceFrame_; }
 
   inline const std::string & getFrameId() const { return poseFrameName_; }
 
   bool isInitialized;
 
+  std::optional<rclcpp::Time> frozenReferenceFrameTime;
+  void freezeReferenceFrame()
+  {
+    frozenReferenceFrameTime = getNode()->now() - rclcpp::Duration::from_seconds(1);
+  }
+
+  void unfreezeReferenceFrame() { frozenReferenceFrameTime = std::nullopt; }
+
 private:
   geometry_msgs::msg::PoseStamped pose_;
 

smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_undo_path_backwards.cpp

@@ -53,20 +53,6 @@ void CbUndoPathBackwards::onEntry()
 
   auto goalCheckerSwitcher = nav2zClient_->getComponent<CpGoalCheckerSwitcher>();
 
-  if (options_ && options_->goalCheckerId_)
-  {
-    goalCheckerSwitcher->setGoalCheckerId(*options_->goalCheckerId_);
-  }
-  else
-  {
-    goalCheckerSwitcher->setGoalCheckerId("undo_path_backwards_goal_checker");
-  }
-
-  // WARNING: There might be some race condition with the remote undo global planner were the global path was not
-  // received yet
-  // TODO: waiting notification from global planner that it is loaded
-  rclcpp::sleep_for(1s);
-
   // this line is used to flush/reset backward planner in the case it were already there
   // plannerSwitcher->setDefaultPlanners();
   if (forwardpath.poses.size() > 0)
@@ -89,6 +75,27 @@ void CbUndoPathBackwards::onEntry()
       plannerSwitcher->setUndoPathBackwardPlanner();
     }
 
+    // WARNING: There might be some race condition with the remote undo global planner/controller were the global path was not
+    // received yet, thee goal switcher
+    // TODO: waiting notification from global planner that it is loaded
+
+    RCLCPP_ERROR_STREAM(
+      getLogger(), "[" << getName()
+                       << "] Waiting for 5 seconds to get planer/controller and wait goal checker "
+                          "ready");
+
+    // rclcpp::sleep_for(5s);
+    RCLCPP_ERROR_STREAM(getLogger(), "[" << getName() << "] activating undo navigation planner");
+
+    if (options_ && options_->goalCheckerId_)
+    {
+      goalCheckerSwitcher->setGoalCheckerId(*options_->goalCheckerId_);
+    }
+    else
+    {
+      goalCheckerSwitcher->setGoalCheckerId("undo_path_backwards_goal_checker");
+    }
+
     this->sendGoal(goal);
   }
 }