BACKPORT Add reset function and publish chi2

teb_local_planner/include/teb_local_planner/teb_local_planner_ros.h

@@ -116,6 +116,7 @@ class TebLocalPlannerROS : public nav2_core::Controller
   void activate() override;
   void deactivate() override;
   void cleanup() override;
+  void reset() override;
 
   /**
     * @brief Initializes the teb plugin

teb_local_planner/include/teb_local_planner/visualization.h

@@ -65,6 +65,7 @@
 #include <nav_msgs/msg/odometry.hpp>
 #include <nav_msgs/msg/path.hpp>
 #include <std_msgs/msg/color_rgba.hpp>
+#include <std_msgs/msg/float64.hpp>
 #include <tf2/transform_datatypes.h>
 #include <visualization_msgs/msg/marker.hpp>
 
@@ -213,6 +214,7 @@ class TebVisualization
    * @param obstacles Container of obstacles
    */
   void publishFeedbackMessage(const TebOptimalPlanner& teb_planner, const ObstContainer& obstacles);
+  void publishChi2(const double &chi2);
 
   nav2_util::CallbackReturn on_configure();
   nav2_util::CallbackReturn on_activate();
@@ -246,6 +248,7 @@ class TebVisualization
   rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PoseArray>::SharedPtr teb_poses_pub_; //!< Publisher for the trajectory pose sequence
   rclcpp_lifecycle::LifecyclePublisher<visualization_msgs::msg::Marker>::SharedPtr teb_marker_pub_; //!< Publisher for visualization markers
   rclcpp_lifecycle::LifecyclePublisher<teb_msgs::msg::FeedbackMsg>::SharedPtr feedback_pub_; //!< Publisher for the feedback message for analysis and debug purposes
+  rclcpp_lifecycle::LifecyclePublisher<std_msgs::msg::Float64>::SharedPtr chi2_pub_; //!< Publisher for the feedback message for analysis and debug purposes
 
   const TebConfig* cfg_; //!< Config class that stores and manages all related parameters
 

teb_local_planner/src/optimal_planner.cpp

@@ -1024,9 +1024,6 @@ void TebOptimalPlanner::AddEdgesVelocityObstacleRatio()
 
 bool TebOptimalPlanner::hasDiverged() const
 {
-  // Early returns if divergence detection is not active
-  if (!cfg_->recovery.divergence_detection_enable)
-    return false;
 
   auto stats_vector = optimizer_->batchStatistics();
 
@@ -1037,6 +1034,13 @@ bool TebOptimalPlanner::hasDiverged() const
   // Grab the statistics of the final iteration
   const auto last_iter_stats = stats_vector.back();
 
+  // publish chi2
+  visualization_->publishChi2(last_iter_stats.chi2);
+
+  // Return if divergence detection is not active
+  if (!cfg_->recovery.divergence_detection_enable)
+    return false;
+
   return last_iter_stats.chi2 > cfg_->recovery.divergence_detection_max_chi_squared;
 }
 

teb_local_planner/src/teb_local_planner_ros.cpp

@@ -1146,6 +1146,11 @@ void TebLocalPlannerROS::cleanup() {
   return;
 }
 
+void TebLocalPlannerROS::reset() {
+  planner_->clearPlanner();
+  return;
+}
+
 } // end namespace teb_local_planner
 
 // register this planner as a nav2_core::Controller plugin

teb_local_planner/src/visualization.cpp

@@ -493,6 +493,12 @@ void TebVisualization::publishFeedbackMessage(const TebOptimalPlanner& teb_plann
   feedback_pub_->publish(msg);
 }
 
+void TebVisualization::publishChi2(const double &chi2) {
+  std_msgs::msg::Float64 msg;
+  msg.data = chi2;
+  chi2_pub_->publish(msg);
+}
+
 std_msgs::msg::ColorRGBA TebVisualization::toColorMsg(double a, double r, double g, double b)
 {
   std_msgs::msg::ColorRGBA color;
@@ -521,6 +527,7 @@ nav2_util::CallbackReturn TebVisualization::on_configure()
   teb_poses_pub_ = nh_->create_publisher<geometry_msgs::msg::PoseArray>("teb_poses", 1);
   teb_marker_pub_ = nh_->create_publisher<visualization_msgs::msg::Marker>("teb_markers", 1);
   feedback_pub_ = nh_->create_publisher<teb_msgs::msg::FeedbackMsg>("teb_feedback", 1);
+  chi2_pub_ = nh_->create_publisher<std_msgs::msg::Float64>("chi2", 1);
 
   initialized_ = true;
   return nav2_util::CallbackReturn::SUCCESS;
@@ -534,6 +541,7 @@ TebVisualization::on_activate()
   teb_poses_pub_->on_activate();
   teb_marker_pub_->on_activate();
   feedback_pub_->on_activate();
+  chi2_pub_->on_activate();
   return nav2_util::CallbackReturn::SUCCESS;
 }
 
@@ -545,6 +553,7 @@ TebVisualization::on_deactivate()
   teb_poses_pub_->on_deactivate();
   teb_marker_pub_->on_deactivate();
   feedback_pub_->on_deactivate();
+  chi2_pub_->on_deactivate();
   return nav2_util::CallbackReturn::SUCCESS;
 }
 
@@ -556,6 +565,7 @@ TebVisualization::on_cleanup()
   teb_poses_pub_.reset();
   teb_marker_pub_.reset();
   feedback_pub_.reset();
+  chi2_pub_.reset();
 
   return nav2_util::CallbackReturn::SUCCESS;
 }