Staticial replanning

nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_path_valid_condition.hpp

@@ -58,7 +58,8 @@ class IsPathValidCondition : public BT::ConditionNode
   {
     return {
       BT::InputPort<nav_msgs::msg::Path>("path", "Path to Check"),
-      BT::InputPort<std::chrono::milliseconds>("server_timeout")
+      BT::InputPort<std::chrono::milliseconds>("server_timeout"),
+      BT::InputPort<bool>("consider_cost_change", true, "Consider cost changes")
     };
   }
 

nav2_behavior_tree/plugins/condition/is_path_valid_condition.cpp

@@ -36,10 +36,13 @@ BT::NodeStatus IsPathValidCondition::tick()
 {
   nav_msgs::msg::Path path;
   getInput("path", path);
+  bool consider_cost_change;
+  getInput("consider_cost_change", consider_cost_change);
 
   auto request = std::make_shared<nav2_msgs::srv::IsPathValid::Request>();
 
   request->path = path;
+  request->consider_cost_change = consider_cost_change;
   auto result = client_->async_send_request(request);
 
   if (rclcpp::spin_until_future_complete(node_, result, server_timeout_) ==

nav2_bringup/params/nav2_params.yaml

@@ -275,6 +275,7 @@ planner_server:
       tolerance: 0.5
       use_astar: false
       allow_unknown: true
+    z_score: 2.55
 
 smoother_server:
   ros__parameters:

nav2_msgs/srv/IsPathValid.srv

@@ -1,6 +1,7 @@
 #Determine if the current path is still valid
 
 nav_msgs/Path path
+bool consider_cost_change
 ---
 bool is_valid
 int32[] invalid_pose_indices 

nav2_planner/include/nav2_planner/planner_server.hpp

@@ -235,6 +235,7 @@ class PlannerServer : public nav2_util::LifecycleNode
   std::vector<std::string> planner_ids_;
   std::vector<std::string> planner_types_;
   double max_planner_duration_;
+  double z_score_;
   std::string planner_ids_concat_;
 
   // Clock

nav2_planner/src/planner_server.cpp

@@ -23,6 +23,7 @@
 #include <string>
 #include <vector>
 #include <utility>
+#include <numeric>
 
 #include "builtin_interfaces/msg/duration.hpp"
 #include "nav2_util/costmap.hpp"
@@ -52,6 +53,12 @@ PlannerServer::PlannerServer(const rclcpp::NodeOptions & options)
   declare_parameter("planner_plugins", default_ids_);
   declare_parameter("expected_planner_frequency", 1.0);
 
+  nav2_util::declare_parameter_if_not_declared(
+    this, "z_score", rclcpp::ParameterValue(2.55));
+
+
+  get_parameter("z_score", z_score_);
+
   get_parameter("planner_plugins", planner_ids_);
   if (planner_ids_ == default_ids_) {
     for (size_t i = 0; i < default_ids_.size(); ++i) {
@@ -541,13 +548,15 @@ void PlannerServer::isPathValid(
     return;
   }
 
+  // Find the closest point to the robot to evaluate from
+  // TODO add orientation element like `truncate_path_local_action` BT node for looping paths
   geometry_msgs::msg::PoseStamped current_pose;
   unsigned int closest_point_index = 0;
   if (costmap_ros_->getRobotPose(current_pose)) {
     float current_distance = std::numeric_limits<float>::max();
     float closest_distance = current_distance;
     geometry_msgs::msg::Point current_point = current_pose.pose.position;
-    for (unsigned int i = 0; i < request->path.poses.size(); ++i) {
+    for (unsigned int i = 0; i != request->path.poses.size(); ++i) {
       geometry_msgs::msg::Point path_point = request->path.poses[i].pose.position;
 
       current_distance = nav2_util::geometry_utils::euclidean_distance(
@@ -559,26 +568,68 @@ void PlannerServer::isPathValid(
         closest_distance = current_distance;
       }
     }
+  }
 
-    /**
-     * The lethal check starts at the closest point to avoid points that have already been passed
-     * and may have become occupied
-     */
-    unsigned int mx = 0;
-    unsigned int my = 0;
-    for (unsigned int i = closest_point_index; i < request->path.poses.size(); ++i) {
-      costmap_->worldToMap(
-        request->path.poses[i].pose.position.x,
-        request->path.poses[i].pose.position.y, mx, my);
-      unsigned int cost = costmap_->getCost(mx, my);
-
-      if (cost == nav2_costmap_2d::LETHAL_OBSTACLE ||
-        cost == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
-      {
-        response->is_valid = false;
-      }
+  // Checking path for collisions starting at the closest point to avoid those already passed
+  std::vector<unsigned int> current_costs;
+  current_costs.reserve(request->path.costs.size() - closest_point_index - 1);
+
+  unsigned int mx = 0;
+  unsigned int my = 0;
+  for (unsigned int i = closest_point_index; i != request->path.poses.size(); ++i) {
+    costmap_->worldToMap(
+      request->path.poses[i].pose.position.x,
+      request->path.poses[i].pose.position.y, mx, my);
+    unsigned int cost = costmap_->getCost(mx, my);
+    current_costs.push_back(cost);
+
+    if (cost == nav2_costmap_2d::LETHAL_OBSTACLE ||
+      cost == nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
+    {
+      response->is_valid = false;
+      return;
     }
   }
+
+  // Check using a statistical test if the cost changes are 'significant' enough
+  // to warrant replanning, due to changes in cost within the environment,
+  // when larger than the minimum sample size for the test
+  if (!request->consider_cost_change || request->path.costs.size() < 30) {
+    return;
+  }
+
+  std::vector<unsigned int> original_costs(
+    request->path.costs.begin() + closest_point_index, request->path.costs.end());
+
+  float mean_orginal = 0;
+  float mean_current = 0;
+  for (unsigned int i = 0; i != original_costs.size(); ++i) {
+    mean_orginal += static_cast<float>(original_costs[i]);
+    mean_current += static_cast<float>(current_costs[i]);
+  }
+  mean_orginal /= static_cast<float>(original_costs.size());
+  mean_current /= static_cast<float>(current_costs.size());
+
+  float var_orginal = 0;
+  float var_current = 0;
+  for (unsigned int i = 0; i != original_costs.size(); ++i) {
+    var_orginal += std::pow(static_cast<float>(original_costs[i]) - static_cast<float>(mean_orginal), 2);
+    var_current += std::pow(static_cast<float>(current_costs[i]) - static_cast<float>(mean_current), 2);
+  }
+  var_orginal /= static_cast<float>(original_costs.size() - 1);
+  var_current /= static_cast<float>(current_costs.size() - 1);
+
+  // Conduct a two sample Z-test, with the null hypothesis is that both path cost samples
+  // come from the same distribution (e.g. there is not a statistically significant change)
+  // Thus, the difference in population mean is 0 and the sample sizes are the same
+  float z = (mean_current - mean_orginal) / std::sqrt((var_current + var_orginal) / current_costs.size());
+
+  // TODO try single tail or tune strictness? Parameterize?
+  // 1.65 95% single tail; 2.55 for 99% dual, 2.33 99% single; 90% 1.65 dual, 90% 1.2 single.
+  if (z > z_score_) {  // critical z score for 95% level
+    RCLCPP_DEBUG_STREAM(get_logger(), "Z-test triggered new global plan. The z score was: " << z << "and the threshold was" << z_score_);
+    response->is_valid = false;
+  }
 }
 
 rcl_interfaces::msg::SetParametersResult