From 7544f1c15b6c65c682fc81387dfdd749a00e6d9d Mon Sep 17 00:00:00 2001 From: Naotaka Hatao Date: Fri, 27 Oct 2023 17:47:27 +0900 Subject: [PATCH 1/4] planner_3d: fix a bug in the cost calculation --- planner_cspace/src/grid_astar_model_3dof.cpp | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/planner_cspace/src/grid_astar_model_3dof.cpp b/planner_cspace/src/grid_astar_model_3dof.cpp index d1992557..068f6f7b 100644 --- a/planner_cspace/src/grid_astar_model_3dof.cpp +++ b/planner_cspace/src/grid_astar_model_3dof.cpp @@ -155,6 +155,10 @@ float GridAstarModel3D::euclidCostRough(const Vec& v) const float GridAstarModel3D::cost( const Vec& cur, const Vec& next, const std::vector& start, const Vec& goal) const { + if ((cm_[cur] > 99) || (cm_[next] > 99)) + { + return -1; + } Vec d_raw = next - cur; d_raw.cycle(map_info_.angle); const Vec d = d_raw; @@ -168,11 +172,6 @@ float GridAstarModel3D::cost( Vec pos = cur; for (int i = 0; i < std::abs(d[2]); i++) { - pos[2] += dir; - if (pos[2] < 0) - pos[2] += map_info_.angle; - else if (pos[2] >= static_cast(map_info_.angle)) - pos[2] -= map_info_.angle; const auto c = cm_[pos]; if (c > 99) return -1; @@ -180,6 +179,11 @@ float GridAstarModel3D::cost( { sum += c; } + pos[2] += dir; + if (pos[2] < 0) + pos[2] += map_info_.angle; + else if (pos[2] >= static_cast(map_info_.angle)) + pos[2] -= map_info_.angle; } const float turn_cost_ratio = cc_.weight_costmap_turn_ / 100.0; const float turn_heuristic_cost_ratio = From 450e4ae94f6f93923d83e1bade904e37c0b6125b Mon Sep 17 00:00:00 2001 From: Naotaka Hatao Date: Mon, 30 Oct 2023 17:37:31 +0900 Subject: [PATCH 2/4] Add test --- planner_cspace/test/src/test_planner_3d_cost.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/planner_cspace/test/src/test_planner_3d_cost.cpp b/planner_cspace/test/src/test_planner_3d_cost.cpp index 2febb5f3..6e8bcfd3 100644 --- a/planner_cspace/test/src/test_planner_3d_cost.cpp +++ b/planner_cspace/test/src/test_planner_3d_cost.cpp @@ -91,6 +91,16 @@ TEST(GridAstarModel3D, Cost) EXPECT_LT(c_straight, c_curve); EXPECT_LT(c_straight, c_drift); EXPECT_LT(c_straight, c_drift_curve); + + // These tests are added to confirm a bug is fixed by https://github.com/at-wat/neonavigation/pull/725 + const GridAstarModel3D::Vec start2(5, 5, 0); + const GridAstarModel3D::Vec occupied(10, 5, 0); + const GridAstarModel3D::Vec goal2(10, 5, 3); + cm[occupied] = 100; + // The cost toward the occupied cell should be negative. + EXPECT_LT(model.cost(start2, occupied, {GridAstarModel3D::VecWithCost(start2)}, occupied), 0); + // The cost from the occupied cell should be negative. + EXPECT_LT(model.cost(occupied, goal2, {GridAstarModel3D::VecWithCost(occupied)}, goal2), 0); } } // namespace planner_3d } // namespace planner_cspace From 2604942cbf00c0aa59edfca7c1fe12a78e04e6e8 Mon Sep 17 00:00:00 2001 From: Naotaka Hatao Date: Mon, 30 Oct 2023 18:27:42 +0900 Subject: [PATCH 3/4] Add more test --- planner_cspace/test/src/test_planner_3d_cost.cpp | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/planner_cspace/test/src/test_planner_3d_cost.cpp b/planner_cspace/test/src/test_planner_3d_cost.cpp index 6e8bcfd3..bea9ba46 100644 --- a/planner_cspace/test/src/test_planner_3d_cost.cpp +++ b/planner_cspace/test/src/test_planner_3d_cost.cpp @@ -66,6 +66,7 @@ TEST(GridAstarModel3D, Cost) cc.max_vel_ = 1.0; cc.max_ang_vel_ = 1.0; cc.angle_resolution_aspect_ = 1.0; + cc.turn_penalty_cost_threshold_ = 0; GridAstarModel3D model( map_info, @@ -92,7 +93,7 @@ TEST(GridAstarModel3D, Cost) EXPECT_LT(c_straight, c_drift); EXPECT_LT(c_straight, c_drift_curve); - // These tests are added to confirm a bug is fixed by https://github.com/at-wat/neonavigation/pull/725 + // These tests are added to confirm that some bugs are fixed by https://github.com/at-wat/neonavigation/pull/725 const GridAstarModel3D::Vec start2(5, 5, 0); const GridAstarModel3D::Vec occupied(10, 5, 0); const GridAstarModel3D::Vec goal2(10, 5, 3); @@ -101,6 +102,15 @@ TEST(GridAstarModel3D, Cost) EXPECT_LT(model.cost(start2, occupied, {GridAstarModel3D::VecWithCost(start2)}, occupied), 0); // The cost from the occupied cell should be negative. EXPECT_LT(model.cost(occupied, goal2, {GridAstarModel3D::VecWithCost(occupied)}, goal2), 0); + + const GridAstarModel3D::Vec start3(10, 20, 0); + cm[start3] = 99; + const GridAstarModel3D::Vec waypoint3(10, 20, 3); + const GridAstarModel3D::Vec goal3(10, 20, 6); + // The cost between start3 and waypoint3 is larger than the cost between waypoint3 and goal3 because start3 + // has a penalty. + EXPECT_GT(model.cost(start3, waypoint3, {GridAstarModel3D::VecWithCost(start3)}, waypoint3), + model.cost(waypoint3, waypoint3, {GridAstarModel3D::VecWithCost(goal3)}, goal3)); } } // namespace planner_3d } // namespace planner_cspace From 9b1f07c8cce25745c00d3c16000af025e7515a39 Mon Sep 17 00:00:00 2001 From: Naotaka Hatao Date: Mon, 30 Oct 2023 19:14:08 +0900 Subject: [PATCH 4/4] Fix variable names --- planner_cspace/test/src/test_planner_3d_cost.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/planner_cspace/test/src/test_planner_3d_cost.cpp b/planner_cspace/test/src/test_planner_3d_cost.cpp index bea9ba46..56122d26 100644 --- a/planner_cspace/test/src/test_planner_3d_cost.cpp +++ b/planner_cspace/test/src/test_planner_3d_cost.cpp @@ -95,13 +95,13 @@ TEST(GridAstarModel3D, Cost) // These tests are added to confirm that some bugs are fixed by https://github.com/at-wat/neonavigation/pull/725 const GridAstarModel3D::Vec start2(5, 5, 0); - const GridAstarModel3D::Vec occupied(10, 5, 0); + const GridAstarModel3D::Vec occupied_waypoint(10, 5, 0); const GridAstarModel3D::Vec goal2(10, 5, 3); - cm[occupied] = 100; + cm[occupied_waypoint] = 100; // The cost toward the occupied cell should be negative. - EXPECT_LT(model.cost(start2, occupied, {GridAstarModel3D::VecWithCost(start2)}, occupied), 0); + EXPECT_LT(model.cost(start2, occupied_waypoint, {GridAstarModel3D::VecWithCost(start2)}, occupied_waypoint), 0); // The cost from the occupied cell should be negative. - EXPECT_LT(model.cost(occupied, goal2, {GridAstarModel3D::VecWithCost(occupied)}, goal2), 0); + EXPECT_LT(model.cost(occupied_waypoint, goal2, {GridAstarModel3D::VecWithCost(occupied_waypoint)}, goal2), 0); const GridAstarModel3D::Vec start3(10, 20, 0); cm[start3] = 99; @@ -110,7 +110,7 @@ TEST(GridAstarModel3D, Cost) // The cost between start3 and waypoint3 is larger than the cost between waypoint3 and goal3 because start3 // has a penalty. EXPECT_GT(model.cost(start3, waypoint3, {GridAstarModel3D::VecWithCost(start3)}, waypoint3), - model.cost(waypoint3, waypoint3, {GridAstarModel3D::VecWithCost(goal3)}, goal3)); + model.cost(waypoint3, goal3, {GridAstarModel3D::VecWithCost(waypoint3)}, goal3)); } } // namespace planner_3d } // namespace planner_cspace