controller server publisher fix

Signed-off-by: silanus23 <[email protected]>

Author: silanus23

nav2_controller/include/nav2_controller/controller_server.hpp

@@ -173,6 +173,10 @@ class ControllerServer : public nav2::LifecycleNode
    * @param velocity Twist velocity to be published
    */
   void publishVelocity(const geometry_msgs::msg::TwistStamped & velocity);
+  /**
+   * @brief Calculates and publishes the TrackingError's all components
+   */
+  void publishTrackingState();
   /**
    * @brief Calls velocity publisher to publish zero velocity
    */
@@ -237,6 +241,7 @@ class ControllerServer : public nav2::LifecycleNode
   std::unique_ptr<nav2_util::OdomSmoother> odom_sub_;
   std::unique_ptr<nav2_util::TwistPublisher> vel_publisher_;
   nav2::Subscription<nav2_msgs::msg::SpeedLimit>::SharedPtr speed_limit_sub_;
+    rclcpp::Publisher<nav2_msgs::msg::TrackingError>::SharedPtr tracking_error_pub_;
 
   // Progress Checker Plugin
   pluginlib::ClassLoader<nav2_core::ProgressChecker> progress_checker_loader_;
@@ -269,6 +274,8 @@ class ControllerServer : public nav2::LifecycleNode
   double min_x_velocity_threshold_;
   double min_y_velocity_threshold_;
   double min_theta_velocity_threshold_;
+  double search_window_;
+  int start_index_;
 
   double failure_tolerance_;
   bool use_realtime_priority_;