From 3bff2140abf5793d9f841caec7b3ed77c7247d2e Mon Sep 17 00:00:00 2001 From: Naotaka Hatao Date: Tue, 3 Mar 2020 20:08:25 +0900 Subject: [PATCH 1/3] Replan immediately when path is blocked by new obstacles --- planner_cspace/src/planner_3d.cpp | 89 ++++++++++++++++--- planner_cspace/test/src/test_abort.cpp | 1 + .../test/src/test_costmap_watchdog.cpp | 2 + .../test/src/test_navigate_boundary.cpp | 2 - 4 files changed, 78 insertions(+), 16 deletions(-) diff --git a/planner_cspace/src/planner_3d.cpp b/planner_cspace/src/planner_3d.cpp index d501782c..49faa4d3 100644 --- a/planner_cspace/src/planner_3d.cpp +++ b/planner_cspace/src/planner_3d.cpp @@ -44,8 +44,8 @@ #include #include #include -#include #include +#include #include #include #include @@ -1275,28 +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 (!goal_updated_ && has_goal_ && updateGoal()) + { + // Goal was updated. + return; + } + 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(); bool has_costmap(false); if (costmap_watchdog_ > ros::Duration(0)) @@ -1308,6 +1357,7 @@ class Planner3dNode last_costmap_.toSec()); status_.error = planner_cspace_msgs::PlannerStatus::DATA_MISSING; publishEmptyPath(); + previous_path.poses.clear(); } else { @@ -1319,6 +1369,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 +1428,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 +1459,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 +1472,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_); + } } } diff --git a/planner_cspace/test/src/test_abort.cpp b/planner_cspace/test/src/test_abort.cpp index 6e6c3dff..04e70c0d 100644 --- a/planner_cspace/test/src/test_abort.cpp +++ b/planner_cspace/test/src/test_abort.cpp @@ -101,6 +101,7 @@ class AbortTest : public ::testing::Test TEST_F(AbortTest, AbortByGoalInRock) { // Send a goal which is in Rock + ros::Duration(1.0).sleep(); move_base_->sendGoal(CreateGoalInRock()); while (move_base_->getState().state_ != actionlib::SimpleClientGoalState::ACTIVE) diff --git a/planner_cspace/test/src/test_costmap_watchdog.cpp b/planner_cspace/test/src/test_costmap_watchdog.cpp index 18e4a218..530aa016 100644 --- a/planner_cspace/test/src/test_costmap_watchdog.cpp +++ b/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); diff --git a/planner_cspace/test/src/test_navigate_boundary.cpp b/planner_cspace/test/src/test_navigate_boundary.cpp index 0177973e..140af712 100644 --- a/planner_cspace/test/src/test_navigate_boundary.cpp +++ b/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) From a676fcfadc9df7191a3c8aff3d28201d41d35bdb Mon Sep 17 00:00:00 2001 From: Naotaka Hatao Date: Tue, 3 Mar 2020 21:46:24 +0900 Subject: [PATCH 2/3] Fix test_abort --- planner_cspace/src/planner_3d.cpp | 9 ++++----- planner_cspace/test/src/test_abort.cpp | 3 ++- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/planner_cspace/src/planner_3d.cpp b/planner_cspace/src/planner_3d.cpp index 49faa4d3..6ea007fb 100644 --- a/planner_cspace/src/planner_3d.cpp +++ b/planner_cspace/src/planner_3d.cpp @@ -1304,11 +1304,6 @@ class Planner3dNode return; } - if (!goal_updated_ && has_goal_ && updateGoal()) - { - // Goal was updated. - return; - } if (costmap_udpated && previous_path.poses.size() > 1) { const GridAstarModel3D::VecWithCost start_grid(pathPose2Grid(previous_path.poses.front())); @@ -1347,6 +1342,10 @@ class Planner3dNode 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)) { diff --git a/planner_cspace/test/src/test_abort.cpp b/planner_cspace/test/src/test_abort.cpp index 04e70c0d..6697831f 100644 --- a/planner_cspace/test/src/test_abort.cpp +++ b/planner_cspace/test/src/test_abort.cpp @@ -100,8 +100,9 @@ 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 - ros::Duration(1.0).sleep(); move_base_->sendGoal(CreateGoalInRock()); while (move_base_->getState().state_ != actionlib::SimpleClientGoalState::ACTIVE) From e432de1b823be83aed5b48dc17ecfadb544a865c Mon Sep 17 00:00:00 2001 From: Naotaka Hatao Date: Wed, 4 Mar 2020 17:32:09 +0900 Subject: [PATCH 3/3] Use costmap to check obstacle on previous path --- planner_cspace/src/planner_3d.cpp | 9 ++------- 1 file changed, 2 insertions(+), 7 deletions(-) diff --git a/planner_cspace/src/planner_3d.cpp b/planner_cspace/src/planner_3d.cpp index 6ea007fb..8be93589 100644 --- a/planner_cspace/src/planner_3d.cpp +++ b/planner_cspace/src/planner_3d.cpp @@ -1306,18 +1306,13 @@ class Planner3dNode 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) + for (const auto& path_pose : previous_path.poses) { - 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) + if (cm_[pathPose2Grid(path_pose)] == 100) { // Obstacle on the path. return; } - prev_grid = cur_grid; } } }