From 179eb3e0f619c8ea5f06bf17c39eec44aa0d4531 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 20 Jul 2022 10:47:43 -0700 Subject: [PATCH] Smac Planner 2D changes (#3083) * removing 4-connected option from smac; fixing 2D heuristic and traversal function from other planner's changes * fix name of variable since no longer a neighborhood * partial test updates * ported unit tests fully * revert to no costmap downsampling --- nav2_smac_planner/README.md | 4 +-- .../include/nav2_smac_planner/constants.hpp | 21 +++++--------- nav2_smac_planner/src/node_2d.cpp | 29 +++++++------------ nav2_smac_planner/src/smac_planner_2d.cpp | 29 ++----------------- nav2_smac_planner/test/test_a_star.cpp | 17 +++++------ nav2_smac_planner/test/test_node2d.cpp | 15 ++-------- nav2_smac_planner/test/test_smac_2d.cpp | 4 --- 7 files changed, 33 insertions(+), 86 deletions(-) diff --git a/nav2_smac_planner/README.md b/nav2_smac_planner/README.md index 086403edf05..0f58190b692 100644 --- a/nav2_smac_planner/README.md +++ b/nav2_smac_planner/README.md @@ -3,7 +3,7 @@ The SmacPlanner is a plugin for the Nav2 Planner server. It includes currently 3 distinct plugins: - `SmacPlannerHybrid`: a highly optimized fully reconfigurable Hybrid-A* implementation supporting Dubin and Reeds-Shepp models (legged, ackermann and car models). - `SmacPlannerLattice`: a highly optimized fully reconfigurable State Lattice implementation supporting configurable minimum control sets, with provided control sets for Ackermann, Legged, Differential and Omnidirectional models. -- `SmacPlanner2D`: a highly optimized fully reconfigurable grid-based A* implementation supporting Moore and Von Neumann models. +- `SmacPlanner2D`: a highly optimized fully reconfigurable grid-based A* implementation supporting 8-connected neighborhood models. It also introduces the following basic building blocks: - `CostmapDownsampler`: A library to take in a costmap object and downsample it to another resolution. @@ -111,7 +111,7 @@ planner_server: max_iterations: 1000000 # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable max_on_approach_iterations: 1000 # maximum number of iterations to attempt to reach goal once in tolerance, 2D only max_planning_time: 3.5 # max time in s for planner to plan, smooth, and upsample. Will scale maximum smoothing and upsampling times based on remaining time after planning. - motion_model_for_search: "DUBIN" # 2D Moore, Von Neumann; Hybrid Dubin, Redds-Shepp; State Lattice set internally + motion_model_for_search: "DUBIN" # For Hybrid Dubin, Redds-Shepp cost_travel_multiplier: 2.0 # For 2D: Cost multiplier to apply to search to steer away from high cost areas. Larger values will place in the center of aisles more exactly (if non-`FREE` cost potential field exists) but take slightly longer to compute. To optimize for speed, a value of 1.0 is reasonable. A reasonable tradeoff value is 2.0. A value of 0.0 effective disables steering away from obstacles and acts like a naive binary search A*. angle_quantization_bins: 64 # For Hybrid nodes: Number of angle bins for search, must be 1 for 2D node (no angle search) analytic_expansion_ratio: 3.5 # For Hybrid/Lattice nodes: The ratio to attempt analytic expansions during search for final approach. diff --git a/nav2_smac_planner/include/nav2_smac_planner/constants.hpp b/nav2_smac_planner/include/nav2_smac_planner/constants.hpp index 8bd0db55754..a7ff03fdca2 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/constants.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/constants.hpp @@ -22,20 +22,17 @@ namespace nav2_smac_planner enum class MotionModel { UNKNOWN = 0, - VON_NEUMANN = 1, - MOORE = 2, - DUBIN = 3, - REEDS_SHEPP = 4, - STATE_LATTICE = 5, + TWOD = 1, + DUBIN = 2, + REEDS_SHEPP = 3, + STATE_LATTICE = 4, }; inline std::string toString(const MotionModel & n) { switch (n) { - case MotionModel::VON_NEUMANN: - return "Von Neumann"; - case MotionModel::MOORE: - return "Moore"; + case MotionModel::TWOD: + return "2D"; case MotionModel::DUBIN: return "Dubin"; case MotionModel::REEDS_SHEPP: @@ -49,10 +46,8 @@ inline std::string toString(const MotionModel & n) inline MotionModel fromString(const std::string & n) { - if (n == "VON_NEUMANN") { - return MotionModel::VON_NEUMANN; - } else if (n == "MOORE") { - return MotionModel::MOORE; + if (n == "2D") { + return MotionModel::TWOD; } else if (n == "DUBIN") { return MotionModel::DUBIN; } else if (n == "REEDS_SHEPP") { diff --git a/nav2_smac_planner/src/node_2d.cpp b/nav2_smac_planner/src/node_2d.cpp index 7917d062f02..442fb62b687 100644 --- a/nav2_smac_planner/src/node_2d.cpp +++ b/nav2_smac_planner/src/node_2d.cpp @@ -72,9 +72,10 @@ float Node2D::getTraversalCost(const NodePtr & child) // If a diagonal move, travel cost is sqrt(2) not 1.0. if ((dx * dx + dy * dy) > 1.05) { - return sqrt_2 + cost_travel_multiplier * normalized_cost; + return sqrt_2 * (1.0 + cost_travel_multiplier * normalized_cost); } + // Length = 1.0 return 1.0 + cost_travel_multiplier * normalized_cost; } @@ -85,34 +86,24 @@ float Node2D::getHeuristicCost( { // Using Moore distance as it more accurately represents the distances // even a Van Neumann neighborhood robot can navigate. - return fabs(goal_coordinates.x - node_coords.x) + - fabs(goal_coordinates.y - node_coords.y); + return hypotf(goal_coordinates.x - node_coords.x, goal_coordinates.y - node_coords.y); } void Node2D::initMotionModel( - const MotionModel & neighborhood, + const MotionModel & motion_model, unsigned int & x_size_uint, unsigned int & /*size_y*/, unsigned int & /*num_angle_quantization*/, SearchInfo & search_info) { + if (motion_model != MotionModel::TWOD) { + throw std::runtime_error("Invalid motion model for 2D node."); + } + int x_size = static_cast(x_size_uint); cost_travel_multiplier = search_info.cost_penalty; - switch (neighborhood) { - case MotionModel::UNKNOWN: - throw std::runtime_error("Unknown neighborhood type selected."); - case MotionModel::VON_NEUMANN: - _neighbors_grid_offsets = {-1, +1, -x_size, +x_size}; - break; - case MotionModel::MOORE: - _neighbors_grid_offsets = {-1, +1, -x_size, +x_size, -x_size - 1, - -x_size + 1, +x_size - 1, +x_size + 1}; - break; - default: - throw std::runtime_error( - "Invalid neighborhood type selected. " - "Von-Neumann and Moore are valid for Node2D."); - } + _neighbors_grid_offsets = {-1, +1, -x_size, +x_size, -x_size - 1, + -x_size + 1, +x_size - 1, +x_size + 1}; } void Node2D::getNeighbors( diff --git a/nav2_smac_planner/src/smac_planner_2d.cpp b/nav2_smac_planner/src/smac_planner_2d.cpp index cf8657118ff..606f6861efd 100644 --- a/nav2_smac_planner/src/smac_planner_2d.cpp +++ b/nav2_smac_planner/src/smac_planner_2d.cpp @@ -91,17 +91,7 @@ void SmacPlanner2D::configure( node, name + ".max_planning_time", rclcpp::ParameterValue(2.0)); node->get_parameter(name + ".max_planning_time", _max_planning_time); - nav2_util::declare_parameter_if_not_declared( - node, name + ".motion_model_for_search", rclcpp::ParameterValue(std::string("MOORE"))); - node->get_parameter(name + ".motion_model_for_search", _motion_model_for_search); - _motion_model = fromString(_motion_model_for_search); - if (_motion_model == MotionModel::UNKNOWN) { - RCLCPP_WARN( - _logger, - "Unable to get MotionModel search type. Given '%s', " - "valid options are MOORE, VON_NEUMANN, DUBIN, REEDS_SHEPP.", - _motion_model_for_search.c_str()); - } + _motion_model = MotionModel::TWOD; if (_max_on_approach_iterations <= 0) { RCLCPP_INFO( @@ -154,10 +144,9 @@ void SmacPlanner2D::configure( RCLCPP_INFO( _logger, "Configured plugin %s of type SmacPlanner2D with " "tolerance %.2f, maximum iterations %i, " - "max on approach iterations %i, and %s. Using motion model: %s.", + "max on approach iterations %i, and %s.", _name.c_str(), _tolerance, _max_iterations, _max_on_approach_iterations, - _allow_unknown ? "allowing unknown traversal" : "not allowing unknown traversal", - toString(_motion_model).c_str()); + _allow_unknown ? "allowing unknown traversal" : "not allowing unknown traversal"); } void SmacPlanner2D::activate() @@ -392,18 +381,6 @@ SmacPlanner2D::dynamicParametersCallback(std::vector paramete _max_on_approach_iterations = std::numeric_limits::max(); } } - } else if (type == ParameterType::PARAMETER_STRING) { - if (name == _name + ".motion_model_for_search") { - reinit_a_star = true; - _motion_model = fromString(parameter.as_string()); - if (_motion_model == MotionModel::UNKNOWN) { - RCLCPP_WARN( - _logger, - "Unable to get MotionModel search type. Given '%s', " - "valid options are MOORE, VON_NEUMANN, DUBIN, REEDS_SHEPP.", - _motion_model_for_search.c_str()); - } - } } } diff --git a/nav2_smac_planner/test/test_a_star.cpp b/nav2_smac_planner/test/test_a_star.cpp index 68af08df501..00852e5298e 100644 --- a/nav2_smac_planner/test/test_a_star.cpp +++ b/nav2_smac_planner/test/test_a_star.cpp @@ -41,7 +41,7 @@ TEST(AStarTest, test_a_star_2d) { nav2_smac_planner::SearchInfo info; nav2_smac_planner::AStarAlgorithm a_star( - nav2_smac_planner::MotionModel::MOORE, info); + nav2_smac_planner::MotionModel::TWOD, info); int max_iterations = 10000; float tolerance = 0.0; float some_tolerance = 20.0; @@ -69,7 +69,7 @@ TEST(AStarTest, test_a_star_2d) a_star.setGoal(80u, 80u, 0); nav2_smac_planner::Node2D::CoordinateVector path; EXPECT_TRUE(a_star.createPath(path, num_it, tolerance)); - EXPECT_EQ(num_it, 102); + EXPECT_EQ(num_it, 2414); // check path is the right size and collision free EXPECT_EQ(path.size(), 81u); @@ -84,7 +84,7 @@ TEST(AStarTest, test_a_star_2d) path.clear(); // failure cases with invalid inputs nav2_smac_planner::AStarAlgorithm a_star_2( - nav2_smac_planner::MotionModel::VON_NEUMANN, info); + nav2_smac_planner::MotionModel::TWOD, info); a_star_2.initialize(false, max_iterations, it_on_approach, max_planning_time, 0, 1); num_it = 0; EXPECT_THROW(a_star_2.createPath(path, num_it, tolerance), std::runtime_error); @@ -104,7 +104,7 @@ TEST(AStarTest, test_a_star_2d) a_star_2.setStart(20, 20, 0); // valid a_star_2.setGoal(50, 50, 0); // invalid EXPECT_TRUE(a_star_2.createPath(path, num_it, some_tolerance)); - EXPECT_EQ(path.size(), 42u); + EXPECT_EQ(path.size(), 20u); for (unsigned int i = 0; i != path.size(); i++) { EXPECT_EQ(costmapA->getCost(path[i].x, path[i].y), 0); } @@ -286,10 +286,8 @@ TEST(AStarTest, test_constants) { nav2_smac_planner::MotionModel mm = nav2_smac_planner::MotionModel::UNKNOWN; // unknown EXPECT_EQ(nav2_smac_planner::toString(mm), std::string("Unknown")); - mm = nav2_smac_planner::MotionModel::VON_NEUMANN; // vonneumann - EXPECT_EQ(nav2_smac_planner::toString(mm), std::string("Von Neumann")); - mm = nav2_smac_planner::MotionModel::MOORE; // moore - EXPECT_EQ(nav2_smac_planner::toString(mm), std::string("Moore")); + mm = nav2_smac_planner::MotionModel::TWOD; // 2d + EXPECT_EQ(nav2_smac_planner::toString(mm), std::string("2D")); mm = nav2_smac_planner::MotionModel::DUBIN; // dubin EXPECT_EQ(nav2_smac_planner::toString(mm), std::string("Dubin")); mm = nav2_smac_planner::MotionModel::REEDS_SHEPP; // reeds-shepp @@ -297,8 +295,7 @@ TEST(AStarTest, test_constants) EXPECT_EQ( nav2_smac_planner::fromString( - "VON_NEUMANN"), nav2_smac_planner::MotionModel::VON_NEUMANN); - EXPECT_EQ(nav2_smac_planner::fromString("MOORE"), nav2_smac_planner::MotionModel::MOORE); + "2D"), nav2_smac_planner::MotionModel::TWOD); EXPECT_EQ(nav2_smac_planner::fromString("DUBIN"), nav2_smac_planner::MotionModel::DUBIN); EXPECT_EQ( nav2_smac_planner::fromString( diff --git a/nav2_smac_planner/test/test_node2d.cpp b/nav2_smac_planner/test/test_node2d.cpp index f4520bd4961..f3af941c4bc 100644 --- a/nav2_smac_planner/test/test_node2d.cpp +++ b/nav2_smac_planner/test/test_node2d.cpp @@ -50,7 +50,7 @@ TEST(Node2DTest, test_node_2d) info.cost_penalty = 1.0; unsigned int size = 10; nav2_smac_planner::Node2D::initMotionModel( - nav2_smac_planner::MotionModel::MOORE, size, size, size, info); + nav2_smac_planner::MotionModel::TWOD, size, size, size, info); // test reset testA.reset(); @@ -68,7 +68,7 @@ TEST(Node2DTest, test_node_2d) // check heuristic cost computation nav2_smac_planner::Node2D::Coordinates A(0.0, 0.0); nav2_smac_planner::Node2D::Coordinates B(10.0, 5.0); - EXPECT_NEAR(testB.getHeuristicCost(A, B, nullptr), 15., 0.01); + EXPECT_NEAR(testB.getHeuristicCost(A, B, nullptr), 11.18, 0.02); // check operator== works on index unsigned char costC = '2'; @@ -106,18 +106,9 @@ TEST(Node2DTest, test_node_2d_neighbors) unsigned int size_y = 10u; unsigned int quant = 0u; // test neighborhood computation - nav2_smac_planner::Node2D::initMotionModel( - nav2_smac_planner::MotionModel::VON_NEUMANN, size_x, - size_y, quant, info); - EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets.size(), 4u); - EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets[0], -1); - EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets[1], 1); - EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets[2], -10); - EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets[3], 10); - size_x = 100u; nav2_smac_planner::Node2D::initMotionModel( - nav2_smac_planner::MotionModel::MOORE, size_x, size_y, + nav2_smac_planner::MotionModel::TWOD, size_x, size_y, quant, info); EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets.size(), 8u); EXPECT_EQ(nav2_smac_planner::Node2D::_neighbors_grid_offsets[0], -1); diff --git a/nav2_smac_planner/test/test_smac_2d.cpp b/nav2_smac_planner/test/test_smac_2d.cpp index ebbac996cd7..1ebe215b30f 100644 --- a/nav2_smac_planner/test/test_smac_2d.cpp +++ b/nav2_smac_planner/test/test_smac_2d.cpp @@ -108,7 +108,6 @@ TEST(SmacTest, test_smac_2d_reconfigure) { rclcpp::Parameter("test.downsampling_factor", 2), rclcpp::Parameter("test.max_iterations", -1), rclcpp::Parameter("test.max_on_approach_iterations", -1), - rclcpp::Parameter("test.motion_model_for_search", "UNKNOWN"), rclcpp::Parameter("test.use_final_approach_orientation", false)}); rclcpp::spin_until_future_complete( @@ -128,9 +127,6 @@ TEST(SmacTest, test_smac_2d_reconfigure) { EXPECT_EQ( node2D->get_parameter("test.max_on_approach_iterations").as_int(), -1); - EXPECT_EQ( - node2D->get_parameter("test.motion_model_for_search").as_string(), - "UNKNOWN"); results = rec_param->set_parameters_atomically( {rclcpp::Parameter("test.downsample_costmap", true)});