planner_cspace: replan immediately when path is blocked by new obstacles

planner_cspace/src/planner_3d.cpp

@@ -44,8 +44,8 @@
 #include <diagnostic_updater/diagnostic_updater.h>
 #include <geometry_msgs/PoseArray.h>
 #include <nav_msgs/GetPlan.h>
-#include <nav_msgs/Path.h>
 #include <nav_msgs/OccupancyGrid.h>
+#include <nav_msgs/Path.h>
 #include <planner_cspace_msgs/PlannerStatus.h>
 #include <sensor_msgs/PointCloud.h>
 #include <std_srvs/Empty.h>
@@ -1275,29 +1275,77 @@ class Planner3dNode
     act_->start();
     act_tolerant_->start();
   }
-  void spin()
+
+  GridAstarModel3D::Vec pathPose2Grid(const geometry_msgs::PoseStamped& pose) const
   {
-    ros::Rate wait(freq_);
-    ROS_DEBUG("Initialized");
+    GridAstarModel3D::Vec grid_vec;
+    grid_metric_converter::metric2Grid(map_info_, grid_vec[0], grid_vec[1], grid_vec[2],
+                                       pose.pose.position.x, pose.pose.position.y, tf2::getYaw(pose.pose.orientation));
+    grid_vec.cycleUnsigned(map_info_.angle);
+    return grid_vec;
+  }
 
+  void waitUntil(const ros::Time& next_replan_time, const nav_msgs::Path& previous_path)
+  {
     while (ros::ok())
     {
-      wait.sleep();
+      const ros::Time prev_map_update_stamp = last_costmap_;
       ros::spinOnce();
-
-      const ros::Time now = ros::Time::now();
+      const bool costmap_udpated = last_costmap_ != prev_map_update_stamp;
 
       if (has_map_)
       {
         updateStart();
 
         if (jump_.detectJump())
+        {
           bbf_costmap_.clear();
+          // Robot pose jumped.
+          return;
+        }
 
-        if (!goal_updated_ && has_goal_)
-          updateGoal();
+        if (costmap_udpated && previous_path.poses.size() > 1)
+        {
+          const GridAstarModel3D::VecWithCost start_grid(pathPose2Grid(previous_path.poses.front()));
+          const GridAstarModel3D::Vec last_grid = pathPose2Grid(previous_path.poses.back());
+          GridAstarModel3D::Vec prev_grid = start_grid.v_;
+          for (size_t i = 1; i < previous_path.poses.size(); ++i)
+          {
+            const GridAstarModel3D::Vec cur_grid = pathPose2Grid(previous_path.poses[i]);
+            if (model_->cost(prev_grid, cur_grid, {start_grid}, last_grid) < 0)  // NOLINT(whitespace/braces)
+            {
+              // Obstacle on the path.
+              return;
+            }
+            prev_grid = cur_grid;
+          }
+        }
       }
+      if (ros::Time::now() > next_replan_time)
+      {
+        return;
+      }
+      ros::Duration(0.01).sleep();
+    }
+  }
+
+  void spin()
+  {
+    ROS_DEBUG("Initialized");
+
+    ros::Time next_replan_time = ros::Time::now();
+    nav_msgs::Path previous_path;
+
+    while (ros::ok())
+    {
+      waitUntil(next_replan_time, previous_path);
+
+      const ros::Time now = ros::Time::now();
 
+      if (has_map_ && !goal_updated_ && has_goal_)
+      {
+        updateGoal();
+      }
       bool has_costmap(false);
       if (costmap_watchdog_ > ros::Duration(0))
       {
@@ -1308,6 +1356,7 @@ class Planner3dNode
               last_costmap_.toSec());
           status_.error = planner_cspace_msgs::PlannerStatus::DATA_MISSING;
           publishEmptyPath();
+          previous_path.poses.clear();
         }
         else
         {
@@ -1319,6 +1368,7 @@ class Planner3dNode
         has_costmap = true;
       }
 
+      bool is_path_switchback = false;
       if (has_map_ && has_goal_ && has_start_ && has_costmap)
       {
         if (act_->isActive())
@@ -1377,6 +1427,8 @@ class Planner3dNode
             has_goal_ = false;
 
             publishEmptyPath();
+            previous_path.poses.clear();
+            next_replan_time += ros::Duration(1.0 / freq_);
             ROS_ERROR("Exceeded max_retry_num:%d", max_retry_num_);
 
             if (act_->isActive())
@@ -1406,14 +1458,11 @@ class Planner3dNode
           {
             pub_path_.publish(path);
           }
+          previous_path = path;
 
           if (sw_wait_ > 0.0)
           {
-            if (switchDetect(path))
-            {
-              ROS_INFO("Planned path has switchback");
-              ros::Duration(sw_wait_).sleep();
-            }
+            is_path_switchback = switchDetect(path);
           }
         }
       }
@@ -1422,9 +1471,20 @@ class Planner3dNode
         if (!retain_last_error_status_)
           status_.error = planner_cspace_msgs::PlannerStatus::GOING_WELL;
         publishEmptyPath();
+        previous_path.poses.clear();
       }
       pub_status_.publish(status_);
       diag_updater_.force_update();
+
+      if (is_path_switchback)
+      {
+        next_replan_time += ros::Duration(sw_wait_);
+        ROS_INFO("Planned path has switchback. Planner will stop until: %f", next_replan_time.toSec());
+      }
+      else
+      {
+        next_replan_time += ros::Duration(1.0 / freq_);
+      }
     }
   }
 

planner_cspace/test/src/test_abort.cpp

@@ -100,6 +100,8 @@ class AbortTest : public ::testing::Test
 
 TEST_F(AbortTest, AbortByGoalInRock)
 {
+  // Assure that goal is received after map in planner_3d.
+  ros::Duration(0.5).sleep();
   // Send a goal which is in Rock
   move_base_->sendGoal(CreateGoalInRock());
   while (move_base_->getState().state_ !=

planner_cspace/test/src/test_costmap_watchdog.cpp

@@ -76,6 +76,8 @@ TEST(Planner3D, CostmapWatchdog)
   goal.pose.position.x = 1.9;
   goal.pose.position.y = 2.8;
   goal.pose.orientation.w = 1.0;
+  // Assure that goal is received after map in planner_3d.
+  ros::Duration(0.5).sleep();
   pub_goal.publish(goal);
 
   ros::Rate rate(10);

planner_cspace/test/src/test_navigate_boundary.cpp

@@ -101,8 +101,6 @@ TEST_F(NavigateBoundary, StartPositionScan)
     for (double y = -10; y < 13; y += 2.0)
     {
       publishTransform(x, y);
-      ros::Duration(0.05).sleep();
-      ros::spinOnce();
 
       path_ = nullptr;
       for (int i = 0; i < 100; ++i)