From 9c8e88cfe83d252c54067c792616d4731b1d7dd1 Mon Sep 17 00:00:00 2001 From: James Ward Date: Thu, 8 Oct 2020 02:13:48 +0800 Subject: [PATCH] Analytic expansion (#43) * Use OMPL to generate heuristics The calculation is run at every planning cycle. It does not seem to slow down the planner - the calculation time seems to be quick enough that the improvement in graph expansion accounts for it. * Use OMPL to calculate analytic solution when near goal * Make angles multiples of the bin size to stop looping behaviour * Uncrustify * Use faster std::sqrt function * Fix analytic path so that the collision checker has coordinates to check! * Pre-allocate variables in analytic path expansion * Rename typedef to NodeGetter to more accurately describe function * Use distance rather than heuristic to determine when to perform analytic expansion Also force the analytic expansion to run on first iteration in case path is trivial. * Move the check for motion model into the main A* loop * Add copyright notices * Remove comment about relaxing node match tolerances The analytic expansion removes the need for this. * Correctly reset node coordinates when aborting from analytic expansion * Move analytic expansion logic to separate function * Uncrustify * Remove unneeded call to get goal coordinates * Fix the calculation of intervals in the analytic path Reserve the number of candidate nodes we are expecting. Base calculations on intervals rather than points - makes distances between nodes work properly. * Rescale heuristic so that analytic expansions are based on distance * Repeatedly split analytic path in half when checking for collision * Add parameter to control rate of analytic expansion attempts * Uncrustify * Fix incorrect type in templated function * Cpplint * Revert "Repeatedly split analytic path in half when checking for collision" This reverts commit 94d9ee0257c47fa6bd5a396ce74d180430c1c814. There was a marginal speed gain (perhaps!) and the splitting approach made the code harder to understand and maintain. * Uncrustify * Add doxygen comments * Add parameter description for analytic expansion ratio * Set lower limit of 2 on number of iterations between analytic expansions * Reduce expected number of iterations because of analytic completion * Refactor analytic expansion ratio calcs to make logic easier to understand --- doc/parameters/param_list.md | 7 +- smac_planner/include/smac_planner/a_star.hpp | 19 +++ .../include/smac_planner/node_se2.hpp | 17 +- smac_planner/include/smac_planner/types.hpp | 1 + smac_planner/src/a_star.cpp | 153 +++++++++++++++++- smac_planner/src/node_2d.cpp | 1 + smac_planner/src/node_se2.cpp | 50 +++--- smac_planner/src/smac_planner.cpp | 3 + smac_planner/test/test_a_star.cpp | 2 +- smac_planner/test/test_nodese2.cpp | 58 +++---- 10 files changed, 253 insertions(+), 58 deletions(-) diff --git a/doc/parameters/param_list.md b/doc/parameters/param_list.md index fc4d6b10b7..bd260225e2 100644 --- a/doc/parameters/param_list.md +++ b/doc/parameters/param_list.md @@ -166,7 +166,7 @@ When `plugins` parameter is not overridden, the following default plugins are lo | ``.marking | true | Whether source should mark in costmap | | ``.clearing | false | Whether source should raytrace clear in costmap | | ``.obstacle_range | 2.5 | Maximum range to mark obstacles in costmap | -| ``.raytrace_range | 3.0 | Maximum range to raytrace clear obstacles from costmap | +| ``.raytrace_range | 3.0 | Maximum range to raytrace clear obstacles from costmap | ## keepout filter @@ -285,7 +285,7 @@ When `controller_plugins`\`progress_checker_plugin`\`goal_checker_plugin` parame | ``.``.x_only_threshold | 0.05 | Threshold to check in the X velocity direction | | ``.``.scale | 1.0 | Weighed scale for critic | -## kinematic_parameters +## kinematic_parameters | Parameter | Default | Description | | ----------| --------| ------------| @@ -304,7 +304,7 @@ When `controller_plugins`\`progress_checker_plugin`\`goal_checker_plugin` parame | ``.decel_lim_y | 0.0 | Maximum deceleration Y (m/s^2) | | ``.decel_lim_theta | 0.0 | Maximum deceleration rotation (rad/s^2) | -## xy_theta_iterator +## xy_theta_iterator | Parameter | Default | Description | | ----------| --------| ------------| @@ -505,6 +505,7 @@ When `planner_plugins` parameter is not overridden, the following default plugin | ``.change_penalty | 0.5 | Penalty to apply to SE2 node if changing direction | | ``.non_straight_penalty | 1.1 | Penalty to apply to SE2 node if non-straight direction | | ``.cost_penalty | 1.2 | Penalty to apply to SE2 node for cost at pose | +| ``.analytic_expansion_ratio | 2.0 | For SE2 nodes the planner will attempt an analytic path expansion every N iterations, where N = closest_distance_to_goal / analytic_expansion_ratio. Higher ratios result in more frequent expansions | | ``.smoother.smoother.w_curve | 1.5 | Smoother weight on curvature of path | | ``.smoother.smoother.w_dist | 0.0 | Smoother weight on distance from original path | | ``.smoother.smoother.w_smooth | 15000 | Smoother weight on distance between nodes | diff --git a/smac_planner/include/smac_planner/a_star.hpp b/smac_planner/include/smac_planner/a_star.hpp index e1c70a6be8..02bf542131 100644 --- a/smac_planner/include/smac_planner/a_star.hpp +++ b/smac_planner/include/smac_planner/a_star.hpp @@ -1,4 +1,5 @@ // Copyright (c) 2020, Samsung Research America +// Copyright (c) 2020, Applied Electric Vehicles Pty Ltd // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -58,6 +59,7 @@ class AStarAlgorithm typedef typename NodeT::Coordinates Coordinates; typedef typename NodeT::CoordinateVector CoordinateVector; typedef typename NodeVector::iterator NeighborIterator; + typedef std::function NodeGetter; /** * @struct smac_planner::NodeComparator @@ -149,6 +151,14 @@ class AStarAlgorithm */ void setFootprint(nav2_costmap_2d::Footprint footprint, bool use_radius); + /** + * @brief Perform an analytic path expansion to the goal + * @param node The node to start the analytic path from + * @param getter The function object that gets valid nodes from the graph + * @return Node pointer to goal node if successful, else return nullptr + */ + NodePtr getAnalyticPath(const NodePtr & node, const NodeGetter & getter); + /** * @brief Set the starting pose for planning, as a node index * @param node Node pointer to the goal node to backtrace @@ -272,6 +282,15 @@ class AStarAlgorithm */ inline void clearGraph(); + /** + * @brief Attempt an analytic path completion + * @return Node pointer reference to goal node if successful, else + * return nullptr + */ + inline NodePtr tryAnalyticExpansion( + const NodePtr & current_node, + const NodeGetter & getter, int & iterations, int & best_cost); + bool _traverse_unknown; int _max_iterations; int _max_on_approach_iterations; diff --git a/smac_planner/include/smac_planner/node_se2.hpp b/smac_planner/include/smac_planner/node_se2.hpp index 277adb9e08..4fb1ae579d 100644 --- a/smac_planner/include/smac_planner/node_se2.hpp +++ b/smac_planner/include/smac_planner/node_se2.hpp @@ -320,6 +320,21 @@ class NodeSE2 return angle + x * angle_quantization + y * width * angle_quantization; } + /** + * @brief Get index at coordinates + * @param x X coordinate of point + * @param y Y coordinate of point + * @param angle Theta coordinate of point + * @return Index + */ + static inline unsigned int getIndex( + const unsigned int & x, const unsigned int & y, const unsigned int & angle) + { + return getIndex( + x, y, angle, motion_table.size_x, + motion_table.num_angle_quantization); + } + /** * @brief Get coordinates at index * @param index Index of point @@ -391,7 +406,7 @@ class NodeSE2 NodeSE2 * parent; Coordinates pose; static double neutral_cost; - static MotionTable _motion_model; + static MotionTable motion_table; private: float _cell_cost; diff --git a/smac_planner/include/smac_planner/types.hpp b/smac_planner/include/smac_planner/types.hpp index ff185b9e57..e39564d333 100644 --- a/smac_planner/include/smac_planner/types.hpp +++ b/smac_planner/include/smac_planner/types.hpp @@ -34,6 +34,7 @@ struct SearchInfo float change_penalty; float reverse_penalty; float cost_penalty; + float analytic_expansion_ratio; }; } // namespace smac_planner diff --git a/smac_planner/src/a_star.cpp b/smac_planner/src/a_star.cpp index c2ebdd9d4c..3464442b70 100644 --- a/smac_planner/src/a_star.cpp +++ b/smac_planner/src/a_star.cpp @@ -1,4 +1,5 @@ // Copyright (c) 2020, Samsung Research America +// Copyright (c) 2020, Applied Electric Vehicles Pty Ltd // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -14,6 +15,10 @@ #include +#include +#include +#include + #include #include #include @@ -23,6 +28,7 @@ #include #include #include +#include #include "smac_planner/a_star.hpp" using namespace std::chrono; // NOLINT @@ -240,10 +246,12 @@ bool AStarAlgorithm::createPath( NodeVector neighbors; int approach_iterations = 0; NeighborIterator neighbor_iterator; + int analytic_iterations = 0; + int closest_distance = std::numeric_limits::max(); // Given an index, return a node ptr reference if its collision-free and valid const unsigned int max_index = getSizeX() * getSizeY() * getSizeDim3(); - std::function neighborGetter = + NodeGetter neighborGetter = [&, this](const unsigned int & index, NodePtr & neighbor_rtn) -> bool { if (index < 0 || index >= max_index) { @@ -269,6 +277,15 @@ bool AStarAlgorithm::createPath( // 2) Mark Nbest as visited current_node->visited(); + // 2.a) Use an analytic expansion (if available) to generate a path + // to the goal. + NodePtr result = tryAnalyticExpansion( + current_node, neighborGetter, analytic_iterations, + closest_distance); + if (result != nullptr) { + current_node = result; + } + // 3) Check if we're at the goal, backtrace if required if (isGoal(current_node)) { return backtracePath(current_node, path); @@ -317,6 +334,98 @@ bool AStarAlgorithm::isGoal(NodePtr & node) return node == getGoal(); } +template<> +AStarAlgorithm::NodePtr AStarAlgorithm::getAnalyticPath( + const NodePtr & node, + const NodeGetter & node_getter) +{ + ompl::base::ScopedState<> from(node->motion_table.state_space), to( + node->motion_table.state_space), s(node->motion_table.state_space); + const NodeSE2::Coordinates & node_coords = node->pose; + from[0] = node_coords.x; + from[1] = node_coords.y; + from[2] = node_coords.theta * node->motion_table.bin_size; + to[0] = _goal_coordinates.x; + to[1] = _goal_coordinates.y; + to[2] = _goal_coordinates.theta * node->motion_table.bin_size; + + float d = node->motion_table.state_space->distance(from(), to()); + NodePtr prev(node); + // A move of sqrt(2) is guaranteed to be in a new cell + constexpr float sqrt_2 = std::sqrt(2.); + unsigned int num_intervals = std::floor(d / sqrt_2); + + using PossibleNode = std::pair; + std::vector possible_nodes; + possible_nodes.reserve(num_intervals - 1); // We won't store this node or the goal + std::vector reals; + // Pre-allocate + unsigned int index = 0; + NodePtr next(nullptr); + float angle = 0.0; + Coordinates proposed_coordinates; + // Don't generate the first point because we are already there! + // And the last point is the goal, so ignore it too! + for (float i = 1; i < num_intervals; i++) { + node->motion_table.state_space->interpolate(from(), to(), i / num_intervals, s()); + reals = s.reals(); + angle = reals[2] / node->motion_table.bin_size; + while (angle >= node->motion_table.num_angle_quantization_float) { + angle -= node->motion_table.num_angle_quantization_float; + } + while (angle < 0.0) { + angle += node->motion_table.num_angle_quantization_float; + } + // Turn the pose into a node, and check if it is valid + index = NodeSE2::getIndex( + static_cast(reals[0]), + static_cast(reals[1]), + static_cast(angle)); + // Get the node from the graph + if (node_getter(index, next)) { + Coordinates initial_node_coords = next->pose; + proposed_coordinates = {static_cast(reals[0]), static_cast(reals[1]), angle}; + next->setPose(proposed_coordinates); + if (next->isNodeValid(_traverse_unknown, _collision_checker) && next != prev) { + // Save the node, and its previous coordinates in case we need to abort + possible_nodes.emplace_back(next, initial_node_coords); + prev = next; + } else { + next->setPose(initial_node_coords); + for (const auto & node_pose : possible_nodes) { + const auto & n = node_pose.first; + n->setPose(node_pose.second); + } + return NodePtr(nullptr); + } + } else { + // Abort + for (const auto & node_pose : possible_nodes) { + const auto & n = node_pose.first; + n->setPose(node_pose.second); + } + return NodePtr(nullptr); + } + } + // Legitimate path - set the parent relationships - poses already set + prev = node; + for (const auto & node_pose : possible_nodes) { + const auto & n = node_pose.first; + n->parent = prev; + prev = n; + } + _goal->parent = prev; + return _goal; +} + +template +typename AStarAlgorithm::NodePtr AStarAlgorithm::getAnalyticPath( + const NodePtr & node, + const NodeGetter & node_getter) +{ + return NodePtr(nullptr); +} + template<> bool AStarAlgorithm::backtracePath(NodePtr & node, CoordinateVector & path) { @@ -483,6 +592,48 @@ unsigned int & AStarAlgorithm::getSizeDim3() return _dim3_size; } +template +typename AStarAlgorithm::NodePtr AStarAlgorithm::tryAnalyticExpansion( + const NodePtr & current_node, const NodeGetter & getter, int & analytic_iterations, + int & closest_distance) +{ + if (_motion_model == MotionModel::DUBIN || _motion_model == MotionModel::REEDS_SHEPP) { + // This must be a NodeSE2 node if we are using these motion models + + // See if we are closer and should be expanding more often + const Coordinates node_coords = + NodeT::getCoords(current_node->getIndex(), getSizeX(), getSizeDim3()); + closest_distance = + std::min( + closest_distance, + static_cast(NodeT::getHeuristicCost( + node_coords, + _goal_coordinates) / NodeT::neutral_cost) + ); + // We want to expand at a rate of d/expansion_ratio, + // but check to see if we are so close that we would be expanding every iteration + // If so, limit it to the expansion ratio (rounded up) + int desired_iterations = std::max( + static_cast(closest_distance / _search_info.analytic_expansion_ratio), + static_cast(std::ceil(_search_info.analytic_expansion_ratio)) + ); + // If we are closer now, we should update the target number of iterations to go + analytic_iterations = + std::min(analytic_iterations, desired_iterations); + + // Always run the expansion on the first run in case there is a + // trivial path to be found + if (analytic_iterations <= 0) { + // Reset the counter, and try the analytic path expansion + analytic_iterations = desired_iterations; + return getAnalyticPath(current_node, getter); + } + analytic_iterations--; + } + // No valid motion model - return nullptr + return NodePtr(nullptr); +} + // Instantiate algorithm for the supported template types template class AStarAlgorithm; template class AStarAlgorithm; diff --git a/smac_planner/src/node_2d.cpp b/smac_planner/src/node_2d.cpp index 6595ab4138..b6afd5b944 100644 --- a/smac_planner/src/node_2d.cpp +++ b/smac_planner/src/node_2d.cpp @@ -1,4 +1,5 @@ // Copyright (c) 2020, Samsung Research America +// Copyright (c) 2020, Applied Electric Vehicles Pty Ltd // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/smac_planner/src/node_se2.cpp b/smac_planner/src/node_se2.cpp index 7e488da901..b3b9b248fc 100644 --- a/smac_planner/src/node_se2.cpp +++ b/smac_planner/src/node_se2.cpp @@ -1,4 +1,5 @@ // Copyright (c) 2020, Samsung Research America +// Copyright (c) 2020, Applied Electric Vehicles Pty Ltd // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -32,9 +33,9 @@ namespace smac_planner { // defining static member for all instance to share -MotionTable NodeSE2::_motion_model; std::vector NodeSE2::_wavefront_heuristic; double NodeSE2::neutral_cost = sqrt(2); +MotionTable NodeSE2::motion_table; // Each of these tables are the projected motion models through // time and space applied to the search on the current node in @@ -77,11 +78,12 @@ void MotionTable::initDubin( float increments; if (angle < bin_size) { increments = 1.0f; - angle = bin_size; + } else { + // Search dimensions are clean multiples of quantization - this prevents + // paths with loops in them + increments = floor(angle / bin_size); } - - // Search dimensions not promised to be clean multiples of quantization - increments = angle / bin_size; + angle = increments * bin_size; // find deflections // If we make a right triangle out of the chord in circle of radius @@ -125,10 +127,11 @@ void MotionTable::initReedsShepp( float increments; if (angle < bin_size) { increments = 1.0f; - angle = bin_size; + } else { + increments = floor(angle / bin_size); } + angle = increments * bin_size; - increments = angle / bin_size; float delta_x = search_info.minimum_turning_radius * sin(angle); float delta_y = search_info.minimum_turning_radius - (search_info.minimum_turning_radius * cos(angle)); @@ -213,7 +216,7 @@ void NodeSE2::reset() bool NodeSE2::isNodeValid(const bool & traverse_unknown, GridCollisionChecker collision_checker) { if (collision_checker.inCollision( - this->pose.x, this->pose.y, this->pose.theta * _motion_model.bin_size, traverse_unknown)) + this->pose.x, this->pose.y, this->pose.theta * motion_table.bin_size, traverse_unknown)) { return false; } @@ -237,7 +240,7 @@ float NodeSE2::getTraversalCost(const NodePtr & child) } float travel_cost = 0.0; - float travel_cost_raw = NodeSE2::neutral_cost + _motion_model.cost_penalty * normalized_cost; + float travel_cost_raw = NodeSE2::neutral_cost + motion_table.cost_penalty * normalized_cost; if (child->getMotionPrimitiveIndex() == 0 || child->getMotionPrimitiveIndex() == 3) { // straight motion, no additional costs to be applied @@ -245,17 +248,17 @@ float NodeSE2::getTraversalCost(const NodePtr & child) } else { if (getMotionPrimitiveIndex() == child->getMotionPrimitiveIndex()) { // Turning motion but keeps in same direction: encourages to commit to turning if starting it - travel_cost = travel_cost_raw * _motion_model.non_straight_penalty; + travel_cost = travel_cost_raw * motion_table.non_straight_penalty; } else { // Turning motion and changing direction: penalizes wiggling - travel_cost = travel_cost_raw * _motion_model.change_penalty; - travel_cost += travel_cost_raw * _motion_model.non_straight_penalty; + travel_cost = travel_cost_raw * motion_table.change_penalty; + travel_cost += travel_cost_raw * motion_table.non_straight_penalty; } } if (getMotionPrimitiveIndex() > 2) { // reverse direction - travel_cost *= _motion_model.reverse_penalty; + travel_cost *= motion_table.reverse_penalty; } return travel_cost; @@ -266,18 +269,19 @@ float NodeSE2::getHeuristicCost( const Coordinates & goal_coords) { // Dubin or Reeds-Shepp shortest distances - ompl::base::ScopedState<> from(_motion_model.state_space), to(_motion_model.state_space); + // Create OMPL states for checking + ompl::base::ScopedState<> from(motion_table.state_space), to(motion_table.state_space); from[0] = node_coords.x; from[1] = node_coords.y; - from[2] = node_coords.theta * _motion_model.bin_size; + from[2] = node_coords.theta * motion_table.bin_size; to[0] = goal_coords.x; to[1] = goal_coords.y; - to[2] = goal_coords.theta * _motion_model.bin_size; + to[2] = goal_coords.theta * motion_table.bin_size; - const float motion_heuristic = _motion_model.state_space->distance(from(), to()); + const float motion_heuristic = motion_table.state_space->distance(from(), to()); const unsigned int & wavefront_idx = static_cast(node_coords.y) * - _motion_model.size_x + static_cast(node_coords.x); + motion_table.size_x + static_cast(node_coords.x); const unsigned int & wavefront_value = _wavefront_heuristic[wavefront_idx]; // if lethal or didn't visit, use the motion heuristic instead. @@ -301,10 +305,10 @@ void NodeSE2::initMotionModel( // find the motion model selected switch (motion_model) { case MotionModel::DUBIN: - _motion_model.initDubin(size_x, size_y, num_angle_quantization, search_info); + motion_table.initDubin(size_x, size_y, num_angle_quantization, search_info); break; case MotionModel::REEDS_SHEPP: - _motion_model.initReedsShepp(size_x, size_y, num_angle_quantization, search_info); + motion_table.initReedsShepp(size_x, size_y, num_angle_quantization, search_info); break; default: throw std::runtime_error( @@ -334,7 +338,7 @@ void NodeSE2::computeWavefrontHeuristic( } } - const unsigned int & size_x = _motion_model.size_x; + const unsigned int & size_x = motion_table.size_x; const int size_x_int = static_cast(size_x); const unsigned int size_y = costmap->getSizeInCellsY(); const unsigned int goal_index = goal_y * size_x + goal_x; @@ -396,15 +400,15 @@ void NodeSE2::getNeighbors( { unsigned int index = 0; NodePtr neighbor = nullptr; - const MotionPoses motion_projections = _motion_model.getProjections(node); Coordinates initial_node_coords; + const MotionPoses motion_projections = motion_table.getProjections(node); for (unsigned int i = 0; i != motion_projections.size(); i++) { index = NodeSE2::getIndex( static_cast(motion_projections[i]._x), static_cast(motion_projections[i]._y), static_cast(motion_projections[i]._theta), - _motion_model.size_x, _motion_model.num_angle_quantization); + motion_table.size_x, motion_table.num_angle_quantization); if (NeighborGetter(index, neighbor) && !neighbor->wasVisited()) { // Cache the initial pose in case it was visited but valid diff --git a/smac_planner/src/smac_planner.cpp b/smac_planner/src/smac_planner.cpp index 908f4e894b..ba3837356c 100644 --- a/smac_planner/src/smac_planner.cpp +++ b/smac_planner/src/smac_planner.cpp @@ -106,6 +106,9 @@ void SmacPlanner::configure( nav2_util::declare_parameter_if_not_declared( _node, name + ".cost_penalty", rclcpp::ParameterValue(1.2)); _node->get_parameter(name + ".cost_penalty", search_info.cost_penalty); + nav2_util::declare_parameter_if_not_declared( + _node, name + ".analytic_expansion_ratio", rclcpp::ParameterValue(2.0)); + _node->get_parameter(name + ".analytic_expansion_ratio", search_info.analytic_expansion_ratio); nav2_util::declare_parameter_if_not_declared( _node, name + ".max_planning_time_ms", rclcpp::ParameterValue(5000.0)); diff --git a/smac_planner/test/test_a_star.cpp b/smac_planner/test/test_a_star.cpp index 1ee6625b27..2e87e06877 100644 --- a/smac_planner/test/test_a_star.cpp +++ b/smac_planner/test/test_a_star.cpp @@ -145,7 +145,7 @@ TEST(AStarTest, test_a_star_se2) EXPECT_TRUE(a_star.createPath(path, num_it, tolerance)); // check path is the right size and collision free - EXPECT_EQ(num_it, 203); + EXPECT_EQ(num_it, 115); EXPECT_EQ(path.size(), 79u); for (unsigned int i = 0; i != path.size(); i++) { EXPECT_EQ(costmapA->getCost(path[i].x, path[i].y), 0); diff --git a/smac_planner/test/test_nodese2.cpp b/smac_planner/test/test_nodese2.cpp index c0cc4d75c2..7e0ad762d2 100644 --- a/smac_planner/test/test_nodese2.cpp +++ b/smac_planner/test/test_nodese2.cpp @@ -149,46 +149,46 @@ TEST(Node2DTest, test_node_2d_neighbors) // test neighborhood computation - EXPECT_EQ(smac_planner::NodeSE2::_motion_model.projections.size(), 3u); - EXPECT_NEAR(smac_planner::NodeSE2::_motion_model.projections[0]._x, 1.41421, 0.01); - EXPECT_NEAR(smac_planner::NodeSE2::_motion_model.projections[0]._y, 0, 0.01); - EXPECT_NEAR(smac_planner::NodeSE2::_motion_model.projections[0]._theta, 0, 0.01); + EXPECT_EQ(smac_planner::NodeSE2::motion_table.projections.size(), 3u); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[0]._x, 1.41421, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[0]._y, 0, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[0]._theta, 0, 0.01); - EXPECT_NEAR(smac_planner::NodeSE2::_motion_model.projections[1]._x, 1.39194, 0.01); - EXPECT_NEAR(smac_planner::NodeSE2::_motion_model.projections[1]._y, 0.25, 0.01); - EXPECT_NEAR(smac_planner::NodeSE2::_motion_model.projections[1]._theta, 4.07283, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[1]._x, 1.39194, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[1]._y, 0.25, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[1]._theta, 4.07283, 0.01); - EXPECT_NEAR(smac_planner::NodeSE2::_motion_model.projections[2]._x, 1.39194, 0.01); - EXPECT_NEAR(smac_planner::NodeSE2::_motion_model.projections[2]._y, -0.25, 0.01); - EXPECT_NEAR(smac_planner::NodeSE2::_motion_model.projections[2]._theta, -4.07283, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[2]._x, 1.39194, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[2]._y, -0.25, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[2]._theta, -4.07283, 0.01); smac_planner::NodeSE2::initMotionModel( smac_planner::MotionModel::REEDS_SHEPP, size_x, size_y, size_theta, info); - EXPECT_EQ(smac_planner::NodeSE2::_motion_model.projections.size(), 6u); - EXPECT_NEAR(smac_planner::NodeSE2::_motion_model.projections[0]._x, 1.41421, 0.01); - EXPECT_NEAR(smac_planner::NodeSE2::_motion_model.projections[0]._y, 0, 0.01); - EXPECT_NEAR(smac_planner::NodeSE2::_motion_model.projections[0]._theta, 0, 0.01); + EXPECT_EQ(smac_planner::NodeSE2::motion_table.projections.size(), 6u); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[0]._x, 1.41421, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[0]._y, 0, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[0]._theta, 0, 0.01); - EXPECT_NEAR(smac_planner::NodeSE2::_motion_model.projections[1]._x, 1.39194, 0.01); - EXPECT_NEAR(smac_planner::NodeSE2::_motion_model.projections[1]._y, 0.25, 0.01); - EXPECT_NEAR(smac_planner::NodeSE2::_motion_model.projections[1]._theta, 4.07283, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[1]._x, 1.39194, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[1]._y, 0.25, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[1]._theta, 4.07283, 0.01); - EXPECT_NEAR(smac_planner::NodeSE2::_motion_model.projections[2]._x, 1.39194, 0.01); - EXPECT_NEAR(smac_planner::NodeSE2::_motion_model.projections[2]._y, -0.25, 0.01); - EXPECT_NEAR(smac_planner::NodeSE2::_motion_model.projections[2]._theta, -4.07283, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[2]._x, 1.39194, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[2]._y, -0.25, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[2]._theta, -4.07283, 0.01); - EXPECT_NEAR(smac_planner::NodeSE2::_motion_model.projections[3]._x, -1.41421, 0.01); - EXPECT_NEAR(smac_planner::NodeSE2::_motion_model.projections[3]._y, 0, 0.01); - EXPECT_NEAR(smac_planner::NodeSE2::_motion_model.projections[3]._theta, 0, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[3]._x, -1.41421, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[3]._y, 0, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[3]._theta, 0, 0.01); - EXPECT_NEAR(smac_planner::NodeSE2::_motion_model.projections[4]._x, -1.39194, 0.01); - EXPECT_NEAR(smac_planner::NodeSE2::_motion_model.projections[4]._y, 0.25, 0.01); - EXPECT_NEAR(smac_planner::NodeSE2::_motion_model.projections[4]._theta, -4.07283, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[4]._x, -1.39194, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[4]._y, 0.25, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[4]._theta, -4.07283, 0.01); - EXPECT_NEAR(smac_planner::NodeSE2::_motion_model.projections[5]._x, -1.39194, 0.01); - EXPECT_NEAR(smac_planner::NodeSE2::_motion_model.projections[5]._y, -0.25, 0.01); - EXPECT_NEAR(smac_planner::NodeSE2::_motion_model.projections[5]._theta, 4.07283, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[5]._x, -1.39194, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[5]._y, -0.25, 0.01); + EXPECT_NEAR(smac_planner::NodeSE2::motion_table.projections[5]._theta, 4.07283, 0.01); nav2_costmap_2d::Costmap2D costmapA(100, 100, 0.05, 0.0, 0.0, 0); smac_planner::GridCollisionChecker checker(&costmapA);