From 722228711790e0350627ecf7dab6054931d8f767 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 2 Jun 2025 10:33:40 -0700 Subject: [PATCH 1/3] Revert "Prototype solving #5192 Issue 2: Reeds-Shepp reduce small reverse expansions (#5207)" This reverts commit c32873decf651b203d7da978bc307b7fbdc196ca. --- .../launch/tb3_loopback_simulation_launch.py | 2 - nav2_bringup/launch/tb3_simulation_launch.py | 2 - .../nav2_smac_planner/analytic_expansion.hpp | 39 +-------- nav2_smac_planner/src/analytic_expansion.cpp | 86 +++++-------------- nav2_smac_planner/test/test_a_star.cpp | 2 +- 5 files changed, 23 insertions(+), 108 deletions(-) diff --git a/nav2_bringup/launch/tb3_loopback_simulation_launch.py b/nav2_bringup/launch/tb3_loopback_simulation_launch.py index 9f51e66c04b..26c22a31d50 100644 --- a/nav2_bringup/launch/tb3_loopback_simulation_launch.py +++ b/nav2_bringup/launch/tb3_loopback_simulation_launch.py @@ -144,8 +144,6 @@ def generate_launch_description() -> LaunchDescription: 'use_composition': use_composition, 'use_respawn': use_respawn, 'use_localization': 'False', # Don't use SLAM, AMCL - 'use_keepout_zones': 'False', - 'use_speed_zones': 'False', }.items(), ) diff --git a/nav2_bringup/launch/tb3_simulation_launch.py b/nav2_bringup/launch/tb3_simulation_launch.py index 78bfa061383..808580af0d4 100644 --- a/nav2_bringup/launch/tb3_simulation_launch.py +++ b/nav2_bringup/launch/tb3_simulation_launch.py @@ -195,8 +195,6 @@ def generate_launch_description() -> LaunchDescription: 'autostart': autostart, 'use_composition': use_composition, 'use_respawn': use_respawn, - 'use_keepout_zones': 'False', - 'use_speed_zones': 'False', }.items(), ) # The SDF file for the world is a xacro file because we wanted to diff --git a/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp b/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp index 0c5cdede0bd..6c6f33490f4 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp @@ -15,10 +15,6 @@ #ifndef NAV2_SMAC_PLANNER__ANALYTIC_EXPANSION_HPP_ #define NAV2_SMAC_PLANNER__ANALYTIC_EXPANSION_HPP_ -#include -#include -#include - #include #include #include @@ -65,33 +61,7 @@ class AnalyticExpansion Coordinates proposed_coords; }; - /** - * @struct AnalyticExpansionNodes - * @brief Analytic expansion nodes and associated metadata - * - * This structure holds a collection of analytic expansion nodes and the number of direction - * changes encountered during the expansion. - */ - struct AnalyticExpansionNodes - { - AnalyticExpansionNodes() = default; - - void add( - NodePtr & node, - Coordinates & initial_coords, - Coordinates & proposed_coords) - { - nodes.emplace_back(node, initial_coords, proposed_coords); - } - - void setDirectionChanges(int changes) - { - direction_changes = changes; - } - - std::vector nodes; - int direction_changes{0}; - }; + typedef std::vector AnalyticExpansionNodes; /** * @brief Constructor for analytic expansion object @@ -166,13 +136,6 @@ class AnalyticExpansion const NodePtr & node, const NodePtr & goal, const AnalyticExpansionNodes & expanded_nodes); - /** - * @brief Counts the number of direction changes in a Reeds-Shepp path - * @param path The Reeds-Shepp path to count direction changes in - * @return The number of direction changes in the path - */ - int countDirectionChanges(const ompl::base::ReedsSheppStateSpace::ReedsSheppPath & path); - /** * @brief Takes an expanded nodes to clean up, if necessary, of any state * information that may be polluting it from a prior search iteration diff --git a/nav2_smac_planner/src/analytic_expansion.cpp b/nav2_smac_planner/src/analytic_expansion.cpp index 13df5feb6af..774454c216a 100644 --- a/nav2_smac_planner/src/analytic_expansion.cpp +++ b/nav2_smac_planner/src/analytic_expansion.cpp @@ -12,6 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. Reserved. +#include +#include +#include + #include #include #include @@ -60,7 +64,7 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalytic NodeT::getCoords( current_node->getIndex(), _collision_checker->getCostmap()->getSizeInCellsX(), _dim_3_size); - AnalyticExpansionNodes current_best_analytic_nodes; + AnalyticExpansionNodes current_best_analytic_nodes = {}; NodePtr current_best_goal = nullptr; NodePtr current_best_node = nullptr; float current_best_score = std::numeric_limits::max(); @@ -92,7 +96,7 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalytic getAnalyticPath( current_node, current_goal_node, getter, current_node->motion_table.state_space); - if (!analytic_nodes.nodes.empty()) { + if (!analytic_nodes.empty()) { found_valid_expansion = true; NodePtr node = current_node; float score = refineAnalyticPath( @@ -114,7 +118,7 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalytic getAnalyticPath( current_node, current_goal_node, getter, current_node->motion_table.state_space); - if (!analytic_nodes.nodes.empty()) { + if (!analytic_nodes.empty()) { NodePtr node = current_node; float score = refineAnalyticPath( node, current_goal_node, getter, analytic_nodes); @@ -130,7 +134,7 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalytic } } - if (!current_best_analytic_nodes.nodes.empty()) { + if (!current_best_analytic_nodes.empty()) { return setAnalyticPath( current_best_node, current_best_goal, current_best_analytic_nodes); @@ -142,28 +146,6 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalytic return NodePtr(nullptr); } -template -int AnalyticExpansion::countDirectionChanges( - const ompl::base::ReedsSheppStateSpace::ReedsSheppPath & path) -{ - const double * lengths = path.length_; - int changes = 0; - int last_dir = 0; - for (int i = 0; i < 5; ++i) { - if (lengths[i] == 0.0) { - continue; - } - - int currentDirection = (lengths[i] > 0.0) ? 1 : -1; - if (last_dir != 0 && currentDirection != last_dir) { - ++changes; - } - last_dir = currentDirection; - } - - return changes; -} - template typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansion::getAnalyticPath( const NodePtr & node, @@ -181,12 +163,6 @@ typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansiondistance(from(), to()); - auto rs_state_space = dynamic_cast(state_space.get()); - int direction_changes = 0; - if (rs_state_space) { - direction_changes = countDirectionChanges(rs_state_space->reedsShepp(from.get(), to.get())); - } - // A move of sqrt(2) is guaranteed to be in a new cell static const float sqrt_2 = sqrtf(2.0f); @@ -203,7 +179,7 @@ typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansion reals; double theta; @@ -238,7 +214,7 @@ typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansionsetPose(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.add(next, initial_node_coords, proposed_coordinates); + possible_nodes.emplace_back(next, initial_node_coords, proposed_coordinates); node_costs.emplace_back(next->getCost()); prev = next; } else { @@ -288,7 +264,7 @@ typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansionsetPose(node_pose.initial_coords); } @@ -297,7 +273,6 @@ typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansion::refineAnalyticPath( getAnalyticPath( test_node, goal_node, getter, test_node->motion_table.state_space); - if (refined_analytic_nodes.nodes.empty()) { + if (refined_analytic_nodes.empty()) { break; } - if (refined_analytic_nodes.direction_changes > analytic_nodes.direction_changes) { - // If the direction changes are worse, we don't want to use this path - continue; - } analytic_nodes = refined_analytic_nodes; node = test_node; } else { @@ -343,7 +314,7 @@ float AnalyticExpansion::refineAnalyticPath( // higher than the minimum turning radius and use the best solution based on // a scoring function similar to that used in traversal cost estimation. auto scoringFn = [&](const AnalyticExpansionNodes & expansion) { - if (expansion.nodes.size() < 2) { + if (expansion.size() < 2) { return std::numeric_limits::max(); } @@ -351,19 +322,18 @@ float AnalyticExpansion::refineAnalyticPath( float normalized_cost = 0.0; // Analytic expansions are consistently spaced const float distance = hypotf( - expansion.nodes[1].proposed_coords.x - expansion.nodes[0].proposed_coords.x, - expansion.nodes[1].proposed_coords.y - expansion.nodes[0].proposed_coords.y); - const float & weight = expansion.nodes[0].node->motion_table.cost_penalty; - for (auto iter = expansion.nodes.begin(); iter != expansion.nodes.end(); ++iter) { + expansion[1].proposed_coords.x - expansion[0].proposed_coords.x, + expansion[1].proposed_coords.y - expansion[0].proposed_coords.y); + const float & weight = expansion[0].node->motion_table.cost_penalty; + for (auto iter = expansion.begin(); iter != expansion.end(); ++iter) { normalized_cost = iter->node->getCost() / 252.0f; - // Search's Traversal Cost Function + // Search's Traversal Cost Function score += distance * (1.0 + weight * normalized_cost); } return score; }; - float original_score = scoringFn(analytic_nodes); - float best_score = original_score; + float best_score = scoringFn(analytic_nodes); float score = std::numeric_limits::max(); float min_turn_rad = node->motion_table.min_turning_radius; const float max_min_turn_rad = 4.0 * min_turn_rad; // Up to 4x the turning radius @@ -377,21 +347,7 @@ float AnalyticExpansion::refineAnalyticPath( } refined_analytic_nodes = getAnalyticPath(node, goal_node, getter, state_space); score = scoringFn(refined_analytic_nodes); - - // Normal scoring: prioritize lower cost as long as not more directional changes - if (score <= best_score && - refined_analytic_nodes.direction_changes <= analytic_nodes.direction_changes) - { - analytic_nodes = refined_analytic_nodes; - best_score = score; - continue; - } - - // Special case: If we have a better score than original (only) and less directional changes - // the path quality is still better than the original and is less operationally complex - if (score <= original_score && - refined_analytic_nodes.direction_changes < analytic_nodes.direction_changes) - { + if (score <= best_score) { analytic_nodes = refined_analytic_nodes; best_score = score; } @@ -409,7 +365,7 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::setAnalytic _detached_nodes.clear(); // Legitimate final path - set the parent relationships, states, and poses NodePtr prev = node; - for (const auto & node_pose : expanded_nodes.nodes) { + for (const auto & node_pose : expanded_nodes) { auto n = node_pose.node; cleanNode(n); if (n->getIndex() != goal_node->getIndex()) { diff --git a/nav2_smac_planner/test/test_a_star.cpp b/nav2_smac_planner/test/test_a_star.cpp index 3c9bd9269bd..e3f16da7efa 100644 --- a/nav2_smac_planner/test/test_a_star.cpp +++ b/nav2_smac_planner/test/test_a_star.cpp @@ -147,7 +147,7 @@ TEST(AStarTest, test_a_star_2d) analytic_expansion_nodes), std::numeric_limits::max()); nav2_smac_planner::AnalyticExpansion::AnalyticExpansionNodes expected_nodes = expander.getAnalyticPath(nullptr, nullptr, nullptr, nullptr); - EXPECT_EQ(expected_nodes.nodes.size(), 0); + EXPECT_EQ(expected_nodes.size(), 0); delete costmapA; } From 3349a6ff74293688020b1186772d9c616f1eda9d Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 2 Jun 2025 10:33:46 -0700 Subject: [PATCH 2/3] Revert "include bug fix for nav2_smac_planner (#5198)" This reverts commit 6a74ba61ca0ea117b2995dd0c8e266a5ec9741ba. --- .../nav2_smac_planner/analytic_expansion.hpp | 5 ++--- nav2_smac_planner/src/analytic_expansion.cpp | 20 ++++++++----------- nav2_smac_planner/test/test_a_star.cpp | 4 +--- 3 files changed, 11 insertions(+), 18 deletions(-) diff --git a/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp b/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp index 6c6f33490f4..0eecb78185e 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp @@ -112,15 +112,14 @@ class AnalyticExpansion /** * @brief Refined analytic path from the current node to the goal - * @param node The node to start the analytic path from. Node head may - * change as a result of refinement + * @param current_node The node to start the analytic path from * @param goal_node The goal node to plan to * @param getter The function object that gets valid nodes from the graph * @param analytic_nodes The set of analytic nodes to refine * @return The score of the refined path */ float refineAnalyticPath( - NodePtr & node, + const NodePtr & current_node, const NodePtr & goal_node, const NodeGetter & getter, AnalyticExpansionNodes & analytic_nodes); diff --git a/nav2_smac_planner/src/analytic_expansion.cpp b/nav2_smac_planner/src/analytic_expansion.cpp index 774454c216a..da46f781975 100644 --- a/nav2_smac_planner/src/analytic_expansion.cpp +++ b/nav2_smac_planner/src/analytic_expansion.cpp @@ -66,7 +66,6 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalytic AnalyticExpansionNodes current_best_analytic_nodes = {}; NodePtr current_best_goal = nullptr; - NodePtr current_best_node = nullptr; float current_best_score = std::numeric_limits::max(); closest_distance = std::min( @@ -98,15 +97,13 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalytic current_node->motion_table.state_space); if (!analytic_nodes.empty()) { found_valid_expansion = true; - NodePtr node = current_node; - float score = refineAnalyticPath( - node, current_goal_node, getter, analytic_nodes); + bool score = refineAnalyticPath( + current_node, current_goal_node, getter, analytic_nodes); // Update the best score if we found a better path if (score < current_best_score) { current_best_analytic_nodes = analytic_nodes; current_best_goal = current_goal_node; current_best_score = score; - current_best_node = node; } } } @@ -119,15 +116,13 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalytic current_node, current_goal_node, getter, current_node->motion_table.state_space); if (!analytic_nodes.empty()) { - NodePtr node = current_node; - float score = refineAnalyticPath( - node, current_goal_node, getter, analytic_nodes); + bool score = refineAnalyticPath( + current_node, current_goal_node, getter, analytic_nodes); // Update the best score if we found a better path if (score < current_best_score) { current_best_analytic_nodes = analytic_nodes; current_best_goal = current_goal_node; current_best_score = score; - current_best_node = node; } } } @@ -136,7 +131,7 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalytic if (!current_best_analytic_nodes.empty()) { return setAnalyticPath( - current_best_node, current_best_goal, + current_node, current_best_goal, current_best_analytic_nodes); } analytic_iterations--; @@ -278,11 +273,12 @@ typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansion float AnalyticExpansion::refineAnalyticPath( - NodePtr & node, + const NodePtr & current_node, const NodePtr & goal_node, const NodeGetter & getter, AnalyticExpansionNodes & analytic_nodes) { + NodePtr node = current_node; NodePtr test_node = node; AnalyticExpansionNodes refined_analytic_nodes; for (int i = 0; i < 8; i++) { @@ -411,7 +407,7 @@ getAnalyticPath( template<> float AnalyticExpansion::refineAnalyticPath( - NodePtr &, + const NodePtr &, const NodePtr &, const NodeGetter &, AnalyticExpansionNodes &) diff --git a/nav2_smac_planner/test/test_a_star.cpp b/nav2_smac_planner/test/test_a_star.cpp index e3f16da7efa..7338f4e43c0 100644 --- a/nav2_smac_planner/test/test_a_star.cpp +++ b/nav2_smac_planner/test/test_a_star.cpp @@ -141,9 +141,7 @@ TEST(AStarTest, test_a_star_2d) int dummy_int2 = 0; EXPECT_EQ(expander.tryAnalyticExpansion(nullptr, {}, {}, {}, nullptr, dummy_int1, dummy_int2), nullptr); - - nav2_smac_planner::Node2D * start = nullptr; - EXPECT_EQ(expander.refineAnalyticPath(start, nullptr, nullptr, + EXPECT_EQ(expander.refineAnalyticPath(nullptr, nullptr, nullptr, analytic_expansion_nodes), std::numeric_limits::max()); nav2_smac_planner::AnalyticExpansion::AnalyticExpansionNodes expected_nodes = expander.getAnalyticPath(nullptr, nullptr, nullptr, nullptr); From 0455b5737e73c8aef9dc79e68037f0102efc8862 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 2 Jun 2025 10:36:42 -0700 Subject: [PATCH 3/3] Revert "Feat/smac planner include orientation flexibility (#4127)" This reverts commit f5543c39aba5d3302208bc6e9696e7cbd41de8c9. --- .../include/nav2_smac_planner/a_star.hpp | 47 +- .../nav2_smac_planner/analytic_expansion.hpp | 33 +- .../include/nav2_smac_planner/constants.hpp | 35 - .../nav2_smac_planner/goal_manager.hpp | 190 - .../include/nav2_smac_planner/node_2d.hpp | 22 +- .../include/nav2_smac_planner/node_hybrid.hpp | 6 +- .../nav2_smac_planner/node_lattice.hpp | 6 +- .../nav2_smac_planner/smac_planner_hybrid.hpp | 2 - .../smac_planner_lattice.hpp | 2 - .../include/nav2_smac_planner/types.hpp | 12 - .../sample_primitives/test/output.json | 3931 ----------------- nav2_smac_planner/src/a_star.cpp | 142 +- nav2_smac_planner/src/analytic_expansion.cpp | 213 +- nav2_smac_planner/src/node_2d.cpp | 6 +- nav2_smac_planner/src/node_hybrid.cpp | 14 +- nav2_smac_planner/src/node_lattice.cpp | 15 +- nav2_smac_planner/src/smac_planner_hybrid.cpp | 76 +- .../src/smac_planner_lattice.cpp | 73 +- nav2_smac_planner/test/CMakeLists.txt | 10 - nav2_smac_planner/test/test_a_star.cpp | 145 +- nav2_smac_planner/test/test_goal_manager.cpp | 177 - nav2_smac_planner/test/test_node2d.cpp | 4 +- nav2_smac_planner/test/test_smac_hybrid.cpp | 108 +- nav2_smac_planner/test/test_smac_lattice.cpp | 84 +- 24 files changed, 160 insertions(+), 5193 deletions(-) delete mode 100644 nav2_smac_planner/include/nav2_smac_planner/goal_manager.hpp delete mode 100644 nav2_smac_planner/lattice_primitives/sample_primitives/test/output.json delete mode 100644 nav2_smac_planner/test/test_goal_manager.cpp diff --git a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp index b0e3bab0188..8b127960cc3 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp @@ -32,7 +32,6 @@ #include "nav2_smac_planner/node_hybrid.hpp" #include "nav2_smac_planner/node_lattice.hpp" #include "nav2_smac_planner/node_basic.hpp" -#include "nav2_smac_planner/goal_manager.hpp" #include "nav2_smac_planner/types.hpp" #include "nav2_smac_planner/constants.hpp" @@ -55,9 +54,6 @@ class AStarAlgorithm typedef typename NodeT::CoordinateVector CoordinateVector; typedef typename NodeVector::iterator NeighborIterator; typedef std::function NodeGetter; - typedef GoalManager GoalManagerT; - typedef std::vector> GoalStateVector; - /** * @struct nav2_smac_planner::NodeComparator @@ -94,8 +90,6 @@ class AStarAlgorithm * or planning time exceeded * @param max_planning_time Maximum time (in seconds) to wait for a plan, createPath returns * false after this timeout - * @param lookup_table_size Size of the lookup table to store heuristic values - * @param dim_3_size Number of quantization bins */ void initialize( const bool & allow_unknown, @@ -131,15 +125,11 @@ class AStarAlgorithm * @param mx The node X index of the goal * @param my The node Y index of the goal * @param dim_3 The node dim_3 index of the goal - * @param goal_heading_mode The goal heading mode to use - * @param coarse_search_resolution The resolution to search for goal heading */ void setGoal( const float & mx, const float & my, - const unsigned int & dim_3, - const GoalHeadingMode & goal_heading_mode = GoalHeadingMode::DEFAULT, - const int & coarse_search_resolution = 1); + const unsigned int & dim_3); /** * @brief Set the starting pose for planning, as a node index @@ -164,6 +154,12 @@ class AStarAlgorithm */ NodePtr & getStart(); + /** + * @brief Get pointer reference to goal node + * @return Node pointer reference to goal node + */ + NodePtr & getGoal(); + /** * @brief Get maximum number of on-approach iterations after within threshold * @return Reference to Maximum on-approach iterations parameter @@ -194,18 +190,6 @@ class AStarAlgorithm */ unsigned int & getSizeDim3(); - /** - * @brief Get the resolution of the coarse search - * @return Size of the goals to expand - */ - unsigned int getCoarseSearchResolution(); - - /** - * @brief Get the goals manager class - * @return Goal manager class - */ - GoalManagerT getGoalManager(); - protected: /** * @brief Get pointer to next goal in open set @@ -226,6 +210,13 @@ class AStarAlgorithm */ inline NodePtr addToGraph(const uint64_t & index); + /** + * @brief Check if this node is the goal node + * @param node Node pointer to check if its the goal node + * @return if node is goal + */ + inline bool isGoal(NodePtr & node); + /** * @brief Get cost of heuristic of node * @param node Node pointer to get heuristic for @@ -249,11 +240,6 @@ class AStarAlgorithm */ inline void clearGraph(); - /** - * @brief Check if node has been visited - * @param current_node Node to check if visited - * @return if node has been visited - */ inline bool onVisitationCheckNode(const NodePtr & node); /** @@ -274,11 +260,12 @@ class AStarAlgorithm unsigned int _x_size; unsigned int _y_size; unsigned int _dim3_size; - unsigned int _coarse_search_resolution; SearchInfo _search_info; + Coordinates _goal_coordinates; NodePtr _start; - GoalManagerT _goal_manager; + NodePtr _goal; + Graph _graph; NodeQueue _queue; diff --git a/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp b/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp index 0eecb78185e..6be84d1c109 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp @@ -35,10 +35,8 @@ class AnalyticExpansion { public: typedef NodeT * NodePtr; - typedef std::vector NodeVector; typedef typename NodeT::Coordinates Coordinates; typedef std::function NodeGetter; - typedef typename NodeT::CoordinateVector CoordinateVector; /** * @struct nav2_smac_planner::AnalyticExpansion::AnalyticExpansionNodes @@ -81,22 +79,17 @@ class AnalyticExpansion /** * @brief Attempt an analytic path completion * @param node The node to start the analytic path from - * @param coarse_check_goals Coarse list of goals nodes to plan to - * @param fine_check_goals Fine list of goals nodes to plan to - * @param goals_coords vector of goal coordinates to plan to + * @param goal The goal node to plan to * @param getter Gets a node at a set of coordinates * @param iterations Iterations to run over - * @param closest_distance Closest distance to goal - * @return Node pointer reference to goal node with the best score out of the goals node if - * successful, else return nullptr + * @param best_cost Best heuristic cost to propertionally expand more closer to the goal + * @return Node pointer reference to goal node if successful, else + * return nullptr */ NodePtr tryAnalyticExpansion( const NodePtr & current_node, - const NodeVector & coarse_check_goals, - const NodeVector & fine_check_goals, - const CoordinateVector & goals_coords, - const NodeGetter & getter, int & iterations, - int & closest_distance); + const NodePtr & goal_node, + const NodeGetter & getter, int & iterations, int & best_cost); /** * @brief Perform an analytic path expansion to the goal @@ -110,20 +103,6 @@ class AnalyticExpansion const NodePtr & node, const NodePtr & goal, const NodeGetter & getter, const ompl::base::StateSpacePtr & state_space); - /** - * @brief Refined analytic path from the current node to the goal - * @param current_node The node to start the analytic path from - * @param goal_node The goal node to plan to - * @param getter The function object that gets valid nodes from the graph - * @param analytic_nodes The set of analytic nodes to refine - * @return The score of the refined path - */ - float refineAnalyticPath( - const NodePtr & current_node, - const NodePtr & goal_node, - const NodeGetter & getter, - AnalyticExpansionNodes & analytic_nodes); - /** * @brief Takes final analytic expansion and appends to current expanded node * @param node The node to start the analytic path from diff --git a/nav2_smac_planner/include/nav2_smac_planner/constants.hpp b/nav2_smac_planner/include/nav2_smac_planner/constants.hpp index 6344c86fb89..af44ce53659 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/constants.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/constants.hpp @@ -28,14 +28,6 @@ enum class MotionModel STATE_LATTICE = 4, }; -enum class GoalHeadingMode -{ - UNKNOWN = 0, - DEFAULT = 1, - BIDIRECTIONAL = 2, - ALL_DIRECTION = 3, -}; - inline std::string toString(const MotionModel & n) { switch (n) { @@ -67,33 +59,6 @@ inline MotionModel fromString(const std::string & n) } } -inline std::string toString(const GoalHeadingMode & n) -{ - switch (n) { - case GoalHeadingMode::DEFAULT: - return "DEFAULT"; - case GoalHeadingMode::BIDIRECTIONAL: - return "BIDIRECTIONAL"; - case GoalHeadingMode::ALL_DIRECTION: - return "ALL_DIRECTION"; - default: - return "Unknown"; - } -} - -inline GoalHeadingMode fromStringToGH(const std::string & n) -{ - if (n == "DEFAULT") { - return GoalHeadingMode::DEFAULT; - } else if (n == "BIDIRECTIONAL") { - return GoalHeadingMode::BIDIRECTIONAL; - } else if (n == "ALL_DIRECTION") { - return GoalHeadingMode::ALL_DIRECTION; - } else { - return GoalHeadingMode::UNKNOWN; - } -} - const float UNKNOWN_COST = 255.0; const float OCCUPIED_COST = 254.0; const float INSCRIBED_COST = 253.0; diff --git a/nav2_smac_planner/include/nav2_smac_planner/goal_manager.hpp b/nav2_smac_planner/include/nav2_smac_planner/goal_manager.hpp deleted file mode 100644 index 826fe9d47e5..00000000000 --- a/nav2_smac_planner/include/nav2_smac_planner/goal_manager.hpp +++ /dev/null @@ -1,190 +0,0 @@ -// Copyright (c) 2020, Samsung Research America -// Copyright (c) 2020, Applied Electric Vehicles Pty Ltd -// Copyright (c) 2024, Stevedan Ogochukwu Omodolor Omodia -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. Reserved. - -#ifndef NAV2_SMAC_PLANNER__GOAL_MANAGER_HPP_ -#define NAV2_SMAC_PLANNER__GOAL_MANAGER_HPP_ - -#include -#include -#include - -#include "nav2_smac_planner/types.hpp" -#include "nav2_smac_planner/node_2d.hpp" -#include "nav2_smac_planner/node_hybrid.hpp" -#include "nav2_smac_planner/node_lattice.hpp" -#include "nav2_smac_planner/node_basic.hpp" -#include "nav2_smac_planner/collision_checker.hpp" - - -namespace nav2_smac_planner -{ - -/** -* @class nav2_smac_planner::GoalManager -* @brief Responsible for managing multiple variables storing information on the goal -*/ -template -class GoalManager -{ -public: - typedef NodeT * NodePtr; - typedef std::vector NodeVector; - typedef std::unordered_set NodeSet; - typedef std::vector> GoalStateVector; - typedef typename NodeT::Coordinates Coordinates; - typedef typename NodeT::CoordinateVector CoordinateVector; - - /** - * @brief Constructor: Initializes empty goal state. sets and coordinate lists. - */ - GoalManager() - : _goals_set(NodeSet()), - _goals_state(GoalStateVector()), - _goals_coordinate(CoordinateVector()) - { - } - - /** - * @brief Destructor for the GoalManager - */ - ~GoalManager() = default; - - /** - * @brief Checks if the goals set is empty - * @return true if the goals set is empty - */ - bool goalsIsEmpty() - { - return _goals_state.empty(); - } - - /** - * @brief Sets the internal goals' state list to the provided goals. - * @param goals Vector of goals state to set, - */ - - void setGoalStates( - GoalStateVector & goals_state) - { - clear(); - _goals_state = goals_state; - } - - /** - * @brief Clears all internal goal data, including goals, states, and coordinates. - */ - void clear() - { - _goals_set.clear(); - _goals_state.clear(); - _goals_coordinate.clear(); - } - - /** - * @brief Populates coarse and fine goal lists for analytic expansion. - * @param coarse_check_goals Output list of goals for coarse search expansion. - * @param fine_check_goals Output list of goals for fine search refinement. - * @param coarse_search_resolution Number of fine goals per coarse goal. - */ - void prepareGoalsForAnalyticExpansion( - NodeVector & coarse_check_goals, NodeVector & fine_check_goals, - int coarse_search_resolution) - { - for (unsigned int i = 0; i < _goals_state.size(); i++) { - if (_goals_state[i].is_valid) { - if (i % coarse_search_resolution == 0) { - coarse_check_goals.push_back(_goals_state[i].goal); - } else { - fine_check_goals.push_back(_goals_state[i].goal); - } - } - } - } - - /** - * @brief Filters and marks invalid goals based on collision checking and tolerance thresholds. - * - * Stores only valid (or tolerably infeasible) goals into internal goal sets and coordinates. - * - * @param tolerance Heuristic tolerance allowed for infeasible goals. - * @param collision_checker Collision checker to validate goal positions. - * @param traverse_unknown Flag whether traversal through unknown space is allowed. - */ - void removeInvalidGoals( - const float & tolerance, - GridCollisionChecker * collision_checker, - const bool & traverse_unknown) - { - for (unsigned int i = 0; i < _goals_state.size(); i++) { - if (_goals_state[i].goal->isNodeValid(traverse_unknown, collision_checker) || - tolerance > 0.001) - { - _goals_state[i].is_valid = true; - _goals_set.insert(_goals_state[i].goal); - _goals_coordinate.push_back(_goals_state[i].goal->pose); - } else { - _goals_state[i].is_valid = false; - } - } - } - - /** - * @brief Check if a given node is part of the goal set. - * @param node Node pointer to check. - * @return if node matches any goal in the goal set. - */ - inline bool isGoal(NodePtr & node) - { - return _goals_set.find(node) != _goals_set.end(); - } - - /** - * @brief Get pointer reference to goals set vector - * @return unordered_set of node pointers reference to the goals nodes - */ - inline NodeSet & getGoalsSet() - { - return _goals_set; - } - - /** - * @brief Get pointer reference to goals state - * @return vector of node pointers reference to the goals state - */ - inline GoalStateVector & getGoalsState() - { - return _goals_state; - } - - /** - * @brief Get pointer reference to goals coordinates - * @return vector of goals coordinates reference to the goals coordinates - */ - inline CoordinateVector & getGoalsCoordinates() - { - return _goals_coordinate; - } - -protected: - NodeSet _goals_set; - GoalStateVector _goals_state; - CoordinateVector _goals_coordinate; - NodePtr primary_goal; -}; - -} // namespace nav2_smac_planner - -#endif // NAV2_SMAC_PLANNER__GOAL_MANAGER_HPP_ diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp index 6d54e1696ab..1bd0993a8eb 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp @@ -50,16 +50,6 @@ class Node2D : x(x_in), y(y_in) {} - inline bool operator==(const Coordinates & rhs) const - { - return this->x == rhs.x && this->y == rhs.y; - } - - inline bool operator!=(const Coordinates & rhs) const - { - return !(*this == rhs); - } - float x, y; }; typedef std::vector CoordinateVector; @@ -85,15 +75,6 @@ class Node2D return this->_index == rhs._index; } - /** - * @brief setting continuous coordinate search poses (in partial-cells) - * @param Pose pose - */ - inline void setPose(const Coordinates & pose_in) - { - pose = pose_in; - } - /** * @brief Reset method for new search */ @@ -243,7 +224,7 @@ class Node2D */ static float getHeuristicCost( const Coordinates & node_coords, - const CoordinateVector & goals_coords); + const Coordinates & goal_coordinates); /** * @brief Initialize the neighborhood to be used in A* @@ -283,7 +264,6 @@ class Node2D bool backtracePath(CoordinateVector & path); Node2D * parent; - Coordinates pose; static float cost_travel_multiplier; static std::vector _neighbors_grid_offsets; diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp index cc3650563a8..ee3a4bf231c 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp @@ -167,12 +167,12 @@ class NodeHybrid : x(x_in), y(y_in), theta(theta_in) {} - inline bool operator==(const Coordinates & rhs) const + inline bool operator==(const Coordinates & rhs) { return this->x == rhs.x && this->y == rhs.y && this->theta == rhs.theta; } - inline bool operator!=(const Coordinates & rhs) const + inline bool operator!=(const Coordinates & rhs) { return !(*this == rhs); } @@ -374,7 +374,7 @@ class NodeHybrid */ static float getHeuristicCost( const Coordinates & node_coords, - const CoordinateVector & goals_coords); + const Coordinates & goal_coordinates); /** * @brief Initialize motion models diff --git a/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp b/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp index 5b07e5453bf..824b435447d 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp @@ -314,7 +314,7 @@ class NodeLattice */ static float getHeuristicCost( const Coordinates & node_coords, - const CoordinateVector & goals_coords); + const Coordinates & goal_coordinates); /** * @brief Initialize motion models @@ -408,8 +408,8 @@ class NodeLattice bool backtracePath(CoordinateVector & path); /** - * @brief add node to the path - * @param current_node + * \brief add node to the path + * \param current_node */ void addNodeToPath(NodePtr current_node, CoordinateVector & path); diff --git a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp index 0b50c0e15fd..ca88725cc23 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_hybrid.hpp @@ -123,8 +123,6 @@ class SmacPlannerHybrid : public nav2_core::GlobalPlanner bool _debug_visualizations; std::string _motion_model_for_search; MotionModel _motion_model; - GoalHeadingMode _goal_heading_mode; - int _coarse_search_resolution; rclcpp_lifecycle::LifecyclePublisher::SharedPtr _raw_plan_publisher; rclcpp_lifecycle::LifecyclePublisher::SharedPtr _planned_footprints_publisher; diff --git a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp index 8cead0a0145..42b30066b8a 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smac_planner_lattice.hpp @@ -122,8 +122,6 @@ class SmacPlannerLattice : public nav2_core::GlobalPlanner _smoothed_footprints_publisher; rclcpp_lifecycle::LifecyclePublisher::SharedPtr _expansions_publisher; - GoalHeadingMode _goal_heading_mode; - int _coarse_search_resolution; std::mutex _mutex; rclcpp_lifecycle::LifecycleNode::WeakPtr _node; diff --git a/nav2_smac_planner/include/nav2_smac_planner/types.hpp b/nav2_smac_planner/include/nav2_smac_planner/types.hpp index b0ccaa8bb29..cc6c9975c5f 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/types.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/types.hpp @@ -188,18 +188,6 @@ struct MotionPrimitive MotionPoses poses; }; - /** - * @struct nav2_smac_planner::GoalState - * @brief A struct to store the goal state - */ - -template -struct GoalState -{ - NodeT * goal = nullptr; - bool is_valid = true; -}; - typedef std::vector MotionPrimitives; typedef std::vector MotionPrimitivePtrs; diff --git a/nav2_smac_planner/lattice_primitives/sample_primitives/test/output.json b/nav2_smac_planner/lattice_primitives/sample_primitives/test/output.json deleted file mode 100644 index feae5b38326..00000000000 --- a/nav2_smac_planner/lattice_primitives/sample_primitives/test/output.json +++ /dev/null @@ -1,3931 +0,0 @@ -{ - "version": 1.0, - "date_generated": "2022-03-17", - "lattice_metadata": { - "motion_model": "ackermann", - "turning_radius": 0.5, - "grid_resolution": 0.05, - "stopping_threshold": 5, - "num_of_headings": 15, - "heading_angles": [ - 0.0, - 0.4636476090008061, - 0.7853981633974483, - 1.1071487177940904, - 1.5707963267948966, - 2.0344439357957027, - 2.356194490192345, - 2.677945044588987, - 3.141592653589793, - 3.6052402625905993, - 3.9269908169872414, - 4.2487413713838835, - 4.71238898038469, - 5.176036589385496, - 5.497787143782138 - ], - "number_of_trajectories": 80 - }, - "primitives": [ - { - "trajectory_id": 0, - "start_angle_index": 0, - "end_angle_index": 13, - "left_turn": false, - "trajectory_radius": 0.52586, - "trajectory_length": 0.64852, - "arc_length": 0.58221, - "straight_length": 0.06631, - "poses": [ - [ - 0.04981, - -0.00236, - 6.18831988221979 - ], - [ - 0.09917, - -0.00944, - 6.093454457259993 - ], - [ - 0.14765, - -0.02115, - 5.998589032300196 - ], - [ - 0.19479, - -0.03741, - 5.903723607340399 - ], - [ - 0.24018, - -0.05805, - 5.808858182380602 - ], - [ - 0.28341, - -0.08291, - 5.713992757420805 - ], - [ - 0.3241, - -0.11175, - 5.619127332461009 - ], - [ - 0.36187, - -0.14431, - 5.524261907501212 - ], - [ - 0.39638, - -0.1803, - 5.429396482541415 - ], - [ - 0.42733, - -0.2194, - 5.334531057581619 - ], - [ - 0.45444, - -0.26126, - 5.239665632621822 - ], - [ - 0.47769, - -0.30538, - 5.176036589385496 - ], - [ - 0.5, - -0.35, - 5.176036589385496 - ] - ] - }, - { - "trajectory_id": 1, - "start_angle_index": 0, - "end_angle_index": 15, - "left_turn": false, - "trajectory_radius": 0.63541, - "trajectory_length": 0.36821, - "arc_length": 0.29461, - "straight_length": 0.07361, - "poses": [ - [ - 0.05254, - -0.00218, - 6.200401159939406 - ], - [ - 0.10472, - -0.00869, - 6.117617012699226 - ], - [ - 0.15619, - -0.0195, - 6.034832865459045 - ], - [ - 0.20658, - -0.03452, - 5.952048718218864 - ], - [ - 0.25556, - -0.05366, - 5.869264570978684 - ], - [ - 0.30295, - -0.07648, - 5.81953769817878 - ], - [ - 0.35, - -0.1, - 5.81953769817878 - ] - ] - }, - { - "trajectory_id": 2, - "start_angle_index": 0, - "end_angle_index": 0, - "left_turn": true, - "trajectory_radius": 0.0, - "trajectory_length": 0.15, - "arc_length": 0.0, - "straight_length": 0.15, - "poses": [ - [ - 0.05, - 0.0, - 0.0 - ], - [ - 0.1, - 0.0, - 0.0 - ], - [ - 0.15, - 0.0, - 0.0 - ] - ] - }, - { - "trajectory_id": 3, - "start_angle_index": 0, - "end_angle_index": 1, - "left_turn": true, - "trajectory_radius": 0.63541, - "trajectory_length": 0.36821, - "arc_length": 0.29461, - "straight_length": 0.07361, - "poses": [ - [ - 0.05254, - 0.00218, - 0.0827841472401804 - ], - [ - 0.10472, - 0.00869, - 0.1655682944803608 - ], - [ - 0.15619, - 0.0195, - 0.2483524417205412 - ], - [ - 0.20658, - 0.03452, - 0.3311365889607216 - ], - [ - 0.25556, - 0.05366, - 0.413920736200902 - ], - [ - 0.30295, - 0.07648, - 0.4636476090008061 - ], - [ - 0.35, - 0.1, - 0.4636476090008061 - ] - ] - }, - { - "trajectory_id": 4, - "start_angle_index": 0, - "end_angle_index": 3, - "left_turn": true, - "trajectory_radius": 0.52586, - "trajectory_length": 0.64852, - "arc_length": 0.58221, - "straight_length": 0.06631, - "poses": [ - [ - 0.04981, - 0.00236, - 0.0948654249597968 - ], - [ - 0.09917, - 0.00944, - 0.1897308499195936 - ], - [ - 0.14765, - 0.02115, - 0.2845962748793904 - ], - [ - 0.19479, - 0.03741, - 0.3794616998391872 - ], - [ - 0.24018, - 0.05805, - 0.4743271247989839 - ], - [ - 0.28341, - 0.08291, - 0.5691925497587808 - ], - [ - 0.3241, - 0.11175, - 0.6640579747185776 - ], - [ - 0.36187, - 0.14431, - 0.7589233996783744 - ], - [ - 0.39638, - 0.1803, - 0.8537888246381711 - ], - [ - 0.42733, - 0.2194, - 0.9486542495979677 - ], - [ - 0.45444, - 0.26126, - 1.0435196745577644 - ], - [ - 0.47769, - 0.30538, - 1.1071487177940904 - ], - [ - 0.5, - 0.35, - 1.1071487177940904 - ] - ] - }, - { - "trajectory_id": 5, - "start_angle_index": 1, - "end_angle_index": 15, - "left_turn": false, - "trajectory_radius": 0.50312, - "trajectory_length": 0.46654, - "arc_length": 0.46654, - "straight_length": 0.0, - "poses": [ - [ - 0.04747, - 0.02076, - 0.360614807000627 - ], - [ - 0.09683, - 0.03652, - 0.2575820050004478 - ], - [ - 0.14755, - 0.04712, - 0.15454920300026875 - ], - [ - 0.19909, - 0.05245, - 0.051516401000089584 - ], - [ - 0.25091, - 0.05245, - 6.231668906179497 - ], - [ - 0.30245, - 0.04712, - 6.128636104179318 - ], - [ - 0.35317, - 0.03652, - 6.025603302179138 - ], - [ - 0.40253, - 0.02076, - 5.922570500178959 - ], - [ - 0.45, - 0.0, - 5.81953769817878 - ] - ] - }, - { - "trajectory_id": 6, - "start_angle_index": 1, - "end_angle_index": 15, - "left_turn": false, - "trajectory_radius": 0.61492, - "trajectory_length": 0.57021, - "arc_length": 0.57021, - "straight_length": 0.0, - "poses": [ - [ - 0.04729, - 0.0212, - 0.37934804372793224 - ], - [ - 0.09619, - 0.03835, - 0.29504847845505844 - ], - [ - 0.14636, - 0.05131, - 0.21074891318218458 - ], - [ - 0.19745, - 0.06001, - 0.12644934790931073 - ], - [ - 0.24909, - 0.06437, - 0.04214978263643687 - ], - [ - 0.30091, - 0.06437, - 6.2410355245431495 - ], - [ - 0.35255, - 0.06001, - 6.156735959270275 - ], - [ - 0.40364, - 0.05131, - 6.0724363939974015 - ], - [ - 0.45381, - 0.03835, - 5.988136828724528 - ], - [ - 0.50271, - 0.0212, - 5.903837263451654 - ], - [ - 0.55, - 0.0, - 5.81953769817878 - ] - ] - }, - { - "trajectory_id": 7, - "start_angle_index": 1, - "end_angle_index": 0, - "left_turn": false, - "trajectory_radius": 0.63541, - "trajectory_length": 0.36821, - "arc_length": 0.29461, - "straight_length": 0.07361, - "poses": [ - [ - 0.04705, - 0.02352, - 0.4636476090008061 - ], - [ - 0.09444, - 0.04634, - 0.413920736200902 - ], - [ - 0.14342, - 0.06548, - 0.3311365889607216 - ], - [ - 0.19381, - 0.0805, - 0.24835244172054122 - ], - [ - 0.24528, - 0.09131, - 0.16556829448036087 - ], - [ - 0.29746, - 0.09782, - 0.08278414724018052 - ], - [ - 0.35, - 0.1, - 0.0 - ] - ] - }, - { - "trajectory_id": 8, - "start_angle_index": 1, - "end_angle_index": 1, - "left_turn": true, - "trajectory_radius": 0.0, - "trajectory_length": 0.22361, - "arc_length": 0.0, - "straight_length": 0.22361, - "poses": [ - [ - 0.05, - 0.025, - 0.4636476090008061 - ], - [ - 0.1, - 0.05, - 0.4636476090008061 - ], - [ - 0.15, - 0.075, - 0.4636476090008061 - ], - [ - 0.2, - 0.1, - 0.4636476090008061 - ] - ] - }, - { - "trajectory_id": 9, - "start_angle_index": 1, - "end_angle_index": 2, - "left_turn": true, - "trajectory_radius": 0.68895, - "trajectory_length": 0.25129, - "arc_length": 0.22167, - "straight_length": 0.02962, - "poses": [ - [ - 0.04409, - 0.0241, - 0.536596705651906 - ], - [ - 0.08631, - 0.05134, - 0.6095458023030058 - ], - [ - 0.12643, - 0.08159, - 0.6824948989541056 - ], - [ - 0.16424, - 0.11469, - 0.7554439956052055 - ], - [ - 0.2, - 0.15, - 0.7853981633974483 - ] - ] - }, - { - "trajectory_id": 10, - "start_angle_index": 2, - "end_angle_index": 0, - "left_turn": false, - "trajectory_radius": 0.51213, - "trajectory_length": 0.4401, - "arc_length": 0.40223, - "straight_length": 0.03787, - "poses": [ - [ - 0.03617, - 0.03288, - 0.6899154709776123 - ], - [ - 0.07532, - 0.06215, - 0.5944327785577764 - ], - [ - 0.11707, - 0.08756, - 0.4989500861379405 - ], - [ - 0.16106, - 0.10888, - 0.4034673937181046 - ], - [ - 0.20688, - 0.1259, - 0.30798470129826866 - ], - [ - 0.25412, - 0.13848, - 0.21250200887843262 - ], - [ - 0.30234, - 0.1465, - 0.11701931645859664 - ], - [ - 0.3511, - 0.14988, - 0.021536624038760666 - ], - [ - 0.4, - 0.15, - 0.0 - ] - ] - }, - { - "trajectory_id": 11, - "start_angle_index": 2, - "end_angle_index": 1, - "left_turn": false, - "trajectory_radius": 0.68895, - "trajectory_length": 0.25129, - "arc_length": 0.22167, - "straight_length": 0.02962, - "poses": [ - [ - 0.03576, - 0.03531, - 0.7554439956052055 - ], - [ - 0.07357, - 0.06841, - 0.6824948989541056 - ], - [ - 0.11369, - 0.09866, - 0.6095458023030058 - ], - [ - 0.15591, - 0.1259, - 0.5365967056519059 - ], - [ - 0.2, - 0.15, - 0.4636476090008061 - ] - ] - }, - { - "trajectory_id": 12, - "start_angle_index": 2, - "end_angle_index": 2, - "left_turn": true, - "trajectory_radius": 0.0, - "trajectory_length": 0.21213, - "arc_length": 0.0, - "straight_length": 0.21213, - "poses": [ - [ - 0.0375, - 0.0375, - 0.7853981633974483 - ], - [ - 0.075, - 0.075, - 0.7853981633974483 - ], - [ - 0.1125, - 0.1125, - 0.7853981633974483 - ], - [ - 0.15, - 0.15, - 0.7853981633974483 - ] - ] - }, - { - "trajectory_id": 13, - "start_angle_index": 2, - "end_angle_index": 3, - "left_turn": true, - "trajectory_radius": 0.68895, - "trajectory_length": 0.25129, - "arc_length": 0.22167, - "straight_length": 0.02962, - "poses": [ - [ - 0.03531, - 0.03576, - 0.8153523311896911 - ], - [ - 0.06841, - 0.07357, - 0.8883014278407909 - ], - [ - 0.09866, - 0.11369, - 0.9612505244918907 - ], - [ - 0.1259, - 0.15591, - 1.0341996211429907 - ], - [ - 0.15, - 0.2, - 1.1071487177940904 - ] - ] - }, - { - "trajectory_id": 14, - "start_angle_index": 2, - "end_angle_index": 4, - "left_turn": true, - "trajectory_radius": 0.51213, - "trajectory_length": 0.4401, - "arc_length": 0.40223, - "straight_length": 0.03787, - "poses": [ - [ - 0.03288, - 0.03617, - 0.8808808558172843 - ], - [ - 0.06215, - 0.07532, - 0.9763635482371201 - ], - [ - 0.08756, - 0.11707, - 1.071846240656956 - ], - [ - 0.10888, - 0.16106, - 1.167328933076792 - ], - [ - 0.1259, - 0.20688, - 1.262811625496628 - ], - [ - 0.13848, - 0.25412, - 1.358294317916464 - ], - [ - 0.1465, - 0.30234, - 1.4537770103363 - ], - [ - 0.14988, - 0.3511, - 1.549259702756136 - ], - [ - 0.15, - 0.4, - 1.5707963267948966 - ] - ] - }, - { - "trajectory_id": 15, - "start_angle_index": 3, - "end_angle_index": 2, - "left_turn": false, - "trajectory_radius": 0.68895, - "trajectory_length": 0.25129, - "arc_length": 0.22167, - "straight_length": 0.02962, - "poses": [ - [ - 0.0241, - 0.04409, - 1.0341996211429905 - ], - [ - 0.05134, - 0.08631, - 0.9612505244918907 - ], - [ - 0.08159, - 0.12643, - 0.8883014278407909 - ], - [ - 0.11469, - 0.16424, - 0.8153523311896911 - ], - [ - 0.15, - 0.2, - 0.7853981633974483 - ] - ] - }, - { - "trajectory_id": 16, - "start_angle_index": 3, - "end_angle_index": 3, - "left_turn": true, - "trajectory_radius": 0.0, - "trajectory_length": 0.22361, - "arc_length": 0.0, - "straight_length": 0.22361, - "poses": [ - [ - 0.025, - 0.05, - 1.1071487177940904 - ], - [ - 0.05, - 0.1, - 1.1071487177940904 - ], - [ - 0.075, - 0.15, - 1.1071487177940904 - ], - [ - 0.1, - 0.2, - 1.1071487177940904 - ] - ] - }, - { - "trajectory_id": 17, - "start_angle_index": 3, - "end_angle_index": 4, - "left_turn": true, - "trajectory_radius": 0.63541, - "trajectory_length": 0.36821, - "arc_length": 0.29461, - "straight_length": 0.07361, - "poses": [ - [ - 0.02352, - 0.04705, - 1.1071487177940904 - ], - [ - 0.04634, - 0.09444, - 1.1568755905939945 - ], - [ - 0.06548, - 0.14342, - 1.239659737834175 - ], - [ - 0.0805, - 0.19381, - 1.3224438850743554 - ], - [ - 0.09131, - 0.24528, - 1.4052280323145356 - ], - [ - 0.09782, - 0.29746, - 1.488012179554716 - ], - [ - 0.1, - 0.35, - 1.5707963267948966 - ] - ] - }, - { - "trajectory_id": 18, - "start_angle_index": 3, - "end_angle_index": 5, - "left_turn": true, - "trajectory_radius": 0.50312, - "trajectory_length": 0.46654, - "arc_length": 0.46654, - "straight_length": 0.0, - "poses": [ - [ - 0.02076, - 0.04747, - 1.2101815197942696 - ], - [ - 0.03652, - 0.09683, - 1.3132143217944487 - ], - [ - 0.04712, - 0.14755, - 1.4162471237946277 - ], - [ - 0.05245, - 0.19909, - 1.519279925794807 - ], - [ - 0.05245, - 0.25091, - 1.622312727794986 - ], - [ - 0.04712, - 0.30245, - 1.7253455297951654 - ], - [ - 0.03652, - 0.35317, - 1.8283783317953444 - ], - [ - 0.02076, - 0.40253, - 1.9314111337955238 - ], - [ - 0.0, - 0.45, - 2.0344439357957027 - ] - ] - }, - { - "trajectory_id": 19, - "start_angle_index": 3, - "end_angle_index": 5, - "left_turn": true, - "trajectory_radius": 0.61492, - "trajectory_length": 0.57021, - "arc_length": 0.57021, - "straight_length": 0.0, - "poses": [ - [ - 0.0212, - 0.04729, - 1.1914482830669642 - ], - [ - 0.03835, - 0.09619, - 1.2757478483398381 - ], - [ - 0.05131, - 0.14636, - 1.3600474136127119 - ], - [ - 0.06001, - 0.19745, - 1.4443469788855858 - ], - [ - 0.06437, - 0.24909, - 1.5286465441584596 - ], - [ - 0.06437, - 0.30091, - 1.6129461094313333 - ], - [ - 0.06001, - 0.35255, - 1.6972456747042073 - ], - [ - 0.05131, - 0.40364, - 1.7815452399770813 - ], - [ - 0.03835, - 0.45381, - 1.8658448052499552 - ], - [ - 0.0212, - 0.50271, - 1.9501443705228287 - ], - [ - 0.0, - 0.55, - 2.0344439357957027 - ] - ] - }, - { - "trajectory_id": 20, - "start_angle_index": 4, - "end_angle_index": 1, - "left_turn": false, - "trajectory_radius": 0.52586, - "trajectory_length": 0.64852, - "arc_length": 0.58221, - "straight_length": 0.06631, - "poses": [ - [ - 0.00236, - 0.04981, - 1.4759309018350997 - ], - [ - 0.00944, - 0.09917, - 1.381065476875303 - ], - [ - 0.02115, - 0.14765, - 1.2862000519155061 - ], - [ - 0.03741, - 0.19479, - 1.1913346269557095 - ], - [ - 0.05805, - 0.24018, - 1.0964692019959126 - ], - [ - 0.08291, - 0.28341, - 1.0016037770361157 - ], - [ - 0.11175, - 0.3241, - 0.9067383520763189 - ], - [ - 0.14431, - 0.36187, - 0.8118729271165221 - ], - [ - 0.1803, - 0.39638, - 0.7170075021567255 - ], - [ - 0.2194, - 0.42733, - 0.6221420771969288 - ], - [ - 0.26126, - 0.45444, - 0.5272766522371322 - ], - [ - 0.30538, - 0.47769, - 0.4636476090008061 - ], - [ - 0.35, - 0.5, - 0.4636476090008061 - ] - ] - }, - { - "trajectory_id": 21, - "start_angle_index": 4, - "end_angle_index": 3, - "left_turn": false, - "trajectory_radius": 0.63541, - "trajectory_length": 0.36821, - "arc_length": 0.29461, - "straight_length": 0.07361, - "poses": [ - [ - 0.00218, - 0.05254, - 1.488012179554716 - ], - [ - 0.00869, - 0.10472, - 1.4052280323145356 - ], - [ - 0.0195, - 0.15619, - 1.3224438850743554 - ], - [ - 0.03452, - 0.20658, - 1.239659737834175 - ], - [ - 0.05366, - 0.25556, - 1.1568755905939945 - ], - [ - 0.07648, - 0.30295, - 1.1071487177940904 - ], - [ - 0.1, - 0.35, - 1.1071487177940904 - ] - ] - }, - { - "trajectory_id": 22, - "start_angle_index": 4, - "end_angle_index": 4, - "left_turn": true, - "trajectory_radius": 0.0, - "trajectory_length": 0.15, - "arc_length": 0.0, - "straight_length": 0.15, - "poses": [ - [ - 0.0, - 0.05, - 1.5707963267948966 - ], - [ - 0.0, - 0.1, - 1.5707963267948966 - ], - [ - 0.0, - 0.15, - 1.5707963267948966 - ] - ] - }, - { - "trajectory_id": 23, - "start_angle_index": 4, - "end_angle_index": 5, - "left_turn": true, - "trajectory_radius": 0.63541, - "trajectory_length": 0.36821, - "arc_length": 0.29461, - "straight_length": 0.07361, - "poses": [ - [ - -0.00218, - 0.05254, - 1.653580474035077 - ], - [ - -0.00869, - 0.10472, - 1.7363646212752575 - ], - [ - -0.0195, - 0.15619, - 1.8191487685154377 - ], - [ - -0.03452, - 0.20658, - 1.9019329157556182 - ], - [ - -0.05366, - 0.25556, - 1.9847170629957986 - ], - [ - -0.07648, - 0.30295, - 2.0344439357957027 - ], - [ - -0.1, - 0.35, - 2.0344439357957027 - ] - ] - }, - { - "trajectory_id": 24, - "start_angle_index": 4, - "end_angle_index": 7, - "left_turn": true, - "trajectory_radius": 0.52586, - "trajectory_length": 0.64852, - "arc_length": 0.58221, - "straight_length": 0.06631, - "poses": [ - [ - -0.00236, - 0.04981, - 1.6656617517546934 - ], - [ - -0.00944, - 0.09917, - 1.76052717671449 - ], - [ - -0.02115, - 0.14765, - 1.855392601674287 - ], - [ - -0.03741, - 0.19479, - 1.9502580266340837 - ], - [ - -0.05805, - 0.24018, - 2.0451234515938803 - ], - [ - -0.08291, - 0.28341, - 2.1399888765536774 - ], - [ - -0.11175, - 0.3241, - 2.234854301513474 - ], - [ - -0.14431, - 0.36187, - 2.3297197264732707 - ], - [ - -0.1803, - 0.39638, - 2.4245851514330674 - ], - [ - -0.2194, - 0.42733, - 2.519450576392864 - ], - [ - -0.26126, - 0.45444, - 2.6143160013526607 - ], - [ - -0.30538, - 0.47769, - 2.677945044588987 - ], - [ - -0.35, - 0.5, - 2.677945044588987 - ] - ] - }, - { - "trajectory_id": 25, - "start_angle_index": 5, - "end_angle_index": 3, - "left_turn": false, - "trajectory_radius": 0.50312, - "trajectory_length": 0.46654, - "arc_length": 0.46654, - "straight_length": 0.0, - "poses": [ - [ - -0.02076, - 0.04747, - 1.9314111337955235 - ], - [ - -0.03652, - 0.09683, - 1.8283783317953444 - ], - [ - -0.04712, - 0.14755, - 1.7253455297951654 - ], - [ - -0.05245, - 0.19909, - 1.622312727794986 - ], - [ - -0.05245, - 0.25091, - 1.519279925794807 - ], - [ - -0.04712, - 0.30245, - 1.4162471237946277 - ], - [ - -0.03652, - 0.35317, - 1.3132143217944487 - ], - [ - -0.02076, - 0.40253, - 1.2101815197942694 - ], - [ - 0.0, - 0.45, - 1.1071487177940904 - ] - ] - }, - { - "trajectory_id": 26, - "start_angle_index": 5, - "end_angle_index": 3, - "left_turn": false, - "trajectory_radius": 0.61492, - "trajectory_length": 0.57021, - "arc_length": 0.57021, - "straight_length": 0.0, - "poses": [ - [ - -0.0212, - 0.04729, - 1.950144370522829 - ], - [ - -0.03835, - 0.09619, - 1.865844805249955 - ], - [ - -0.05131, - 0.14636, - 1.7815452399770813 - ], - [ - -0.06001, - 0.19745, - 1.6972456747042073 - ], - [ - -0.06437, - 0.24909, - 1.6129461094313335 - ], - [ - -0.06437, - 0.30091, - 1.5286465441584598 - ], - [ - -0.06001, - 0.35255, - 1.4443469788855858 - ], - [ - -0.05131, - 0.40364, - 1.3600474136127119 - ], - [ - -0.03835, - 0.45381, - 1.275747848339838 - ], - [ - -0.0212, - 0.50271, - 1.1914482830669644 - ], - [ - 0.0, - 0.55, - 1.1071487177940904 - ] - ] - }, - { - "trajectory_id": 27, - "start_angle_index": 5, - "end_angle_index": 4, - "left_turn": false, - "trajectory_radius": 0.63541, - "trajectory_length": 0.36821, - "arc_length": 0.29461, - "straight_length": 0.07361, - "poses": [ - [ - -0.02352, - 0.04705, - 2.0344439357957027 - ], - [ - -0.04634, - 0.09444, - 1.9847170629957986 - ], - [ - -0.06548, - 0.14342, - 1.9019329157556182 - ], - [ - -0.0805, - 0.19381, - 1.8191487685154377 - ], - [ - -0.09131, - 0.24528, - 1.7363646212752575 - ], - [ - -0.09782, - 0.29746, - 1.653580474035077 - ], - [ - -0.1, - 0.35, - 1.5707963267948966 - ] - ] - }, - { - "trajectory_id": 28, - "start_angle_index": 5, - "end_angle_index": 5, - "left_turn": true, - "trajectory_radius": 0.0, - "trajectory_length": 0.22361, - "arc_length": 0.0, - "straight_length": 0.22361, - "poses": [ - [ - -0.025, - 0.05, - 2.0344439357957027 - ], - [ - -0.05, - 0.1, - 2.0344439357957027 - ], - [ - -0.075, - 0.15, - 2.0344439357957027 - ], - [ - -0.1, - 0.2, - 2.0344439357957027 - ] - ] - }, - { - "trajectory_id": 29, - "start_angle_index": 5, - "end_angle_index": 6, - "left_turn": true, - "trajectory_radius": 0.68895, - "trajectory_length": 0.25129, - "arc_length": 0.22167, - "straight_length": 0.02962, - "poses": [ - [ - -0.0241, - 0.04409, - 2.1073930324468026 - ], - [ - -0.05134, - 0.08631, - 2.1803421290979026 - ], - [ - -0.08159, - 0.12643, - 2.253291225749002 - ], - [ - -0.11469, - 0.16424, - 2.326240322400102 - ], - [ - -0.15, - 0.2, - 2.356194490192345 - ] - ] - }, - { - "trajectory_id": 30, - "start_angle_index": 6, - "end_angle_index": 4, - "left_turn": false, - "trajectory_radius": 0.51213, - "trajectory_length": 0.4401, - "arc_length": 0.40223, - "straight_length": 0.03787, - "poses": [ - [ - -0.03288, - 0.03617, - 2.260711797772509 - ], - [ - -0.06215, - 0.07532, - 2.165229105352673 - ], - [ - -0.08756, - 0.11707, - 2.069746412932837 - ], - [ - -0.10888, - 0.16106, - 1.9742637205130011 - ], - [ - -0.1259, - 0.20688, - 1.8787810280931652 - ], - [ - -0.13848, - 0.25412, - 1.7832983356733292 - ], - [ - -0.1465, - 0.30234, - 1.6878156432534932 - ], - [ - -0.14988, - 0.3511, - 1.5923329508336572 - ], - [ - -0.15, - 0.4, - 1.5707963267948966 - ] - ] - }, - { - "trajectory_id": 31, - "start_angle_index": 6, - "end_angle_index": 5, - "left_turn": false, - "trajectory_radius": 0.68895, - "trajectory_length": 0.25129, - "arc_length": 0.22167, - "straight_length": 0.02962, - "poses": [ - [ - -0.03531, - 0.03576, - 2.326240322400102 - ], - [ - -0.06841, - 0.07357, - 2.253291225749002 - ], - [ - -0.09866, - 0.11369, - 2.1803421290979026 - ], - [ - -0.1259, - 0.15591, - 2.1073930324468026 - ], - [ - -0.15, - 0.2, - 2.0344439357957027 - ] - ] - }, - { - "trajectory_id": 32, - "start_angle_index": 6, - "end_angle_index": 6, - "left_turn": true, - "trajectory_radius": 0.0, - "trajectory_length": 0.21213, - "arc_length": 0.0, - "straight_length": 0.21213, - "poses": [ - [ - -0.0375, - 0.0375, - 2.356194490192345 - ], - [ - -0.075, - 0.075, - 2.356194490192345 - ], - [ - -0.1125, - 0.1125, - 2.356194490192345 - ], - [ - -0.15, - 0.15, - 2.356194490192345 - ] - ] - }, - { - "trajectory_id": 33, - "start_angle_index": 6, - "end_angle_index": 7, - "left_turn": true, - "trajectory_radius": 0.68895, - "trajectory_length": 0.25129, - "arc_length": 0.22167, - "straight_length": 0.02962, - "poses": [ - [ - -0.03576, - 0.03531, - 2.3861486579845876 - ], - [ - -0.07357, - 0.06841, - 2.4590977546356876 - ], - [ - -0.11369, - 0.09866, - 2.532046851286787 - ], - [ - -0.15591, - 0.1259, - 2.604995947937887 - ], - [ - -0.2, - 0.15, - 2.677945044588987 - ] - ] - }, - { - "trajectory_id": 34, - "start_angle_index": 6, - "end_angle_index": 8, - "left_turn": true, - "trajectory_radius": 0.51213, - "trajectory_length": 0.4401, - "arc_length": 0.40223, - "straight_length": 0.03787, - "poses": [ - [ - -0.03617, - 0.03288, - 2.4516771826121806 - ], - [ - -0.07532, - 0.06215, - 2.547159875032017 - ], - [ - -0.11707, - 0.08756, - 2.6426425674518526 - ], - [ - -0.16106, - 0.10888, - 2.7381252598716888 - ], - [ - -0.20688, - 0.1259, - 2.8336079522915245 - ], - [ - -0.25412, - 0.13848, - 2.9290906447113603 - ], - [ - -0.30234, - 0.1465, - 3.0245733371311965 - ], - [ - -0.3511, - 0.14988, - 3.1200560295510327 - ], - [ - -0.4, - 0.15, - 3.141592653589793 - ] - ] - }, - { - "trajectory_id": 35, - "start_angle_index": 7, - "end_angle_index": 9, - "left_turn": true, - "trajectory_radius": 0.50312, - "trajectory_length": 0.46654, - "arc_length": 0.46654, - "straight_length": 0.0, - "poses": [ - [ - -0.04747, - 0.02076, - 2.780977846589166 - ], - [ - -0.09683, - 0.03652, - 2.8840106485893453 - ], - [ - -0.14755, - 0.04712, - 2.9870434505895243 - ], - [ - -0.19909, - 0.05245, - 3.0900762525897036 - ], - [ - -0.25091, - 0.05245, - 3.1931090545898826 - ], - [ - -0.30245, - 0.04712, - 3.296141856590062 - ], - [ - -0.35317, - 0.03652, - 3.399174658590241 - ], - [ - -0.40253, - 0.02076, - 3.5022074605904203 - ], - [ - -0.45, - 0.0, - 3.6052402625905993 - ] - ] - }, - { - "trajectory_id": 36, - "start_angle_index": 7, - "end_angle_index": 9, - "left_turn": true, - "trajectory_radius": 0.61492, - "trajectory_length": 0.57021, - "arc_length": 0.57021, - "straight_length": 0.0, - "poses": [ - [ - -0.04729, - 0.0212, - 2.762244609861861 - ], - [ - -0.09619, - 0.03835, - 2.8465441751347345 - ], - [ - -0.14636, - 0.05131, - 2.9308437404076084 - ], - [ - -0.19745, - 0.06001, - 3.0151433056804824 - ], - [ - -0.24909, - 0.06437, - 3.0994428709533564 - ], - [ - -0.30091, - 0.06437, - 3.18374243622623 - ], - [ - -0.35255, - 0.06001, - 3.268042001499104 - ], - [ - -0.40364, - 0.05131, - 3.352341566771978 - ], - [ - -0.45381, - 0.03835, - 3.4366411320448518 - ], - [ - -0.50271, - 0.0212, - 3.5209406973177253 - ], - [ - -0.55, - 0.0, - 3.6052402625905993 - ] - ] - }, - { - "trajectory_id": 37, - "start_angle_index": 7, - "end_angle_index": 6, - "left_turn": false, - "trajectory_radius": 0.68895, - "trajectory_length": 0.25129, - "arc_length": 0.22167, - "straight_length": 0.02962, - "poses": [ - [ - -0.04409, - 0.0241, - 2.604995947937887 - ], - [ - -0.08631, - 0.05134, - 2.532046851286787 - ], - [ - -0.12643, - 0.08159, - 2.4590977546356876 - ], - [ - -0.16424, - 0.11469, - 2.3861486579845876 - ], - [ - -0.2, - 0.15, - 2.356194490192345 - ] - ] - }, - { - "trajectory_id": 38, - "start_angle_index": 7, - "end_angle_index": 7, - "left_turn": true, - "trajectory_radius": 0.0, - "trajectory_length": 0.22361, - "arc_length": 0.0, - "straight_length": 0.22361, - "poses": [ - [ - -0.05, - 0.025, - 2.677945044588987 - ], - [ - -0.1, - 0.05, - 2.677945044588987 - ], - [ - -0.15, - 0.075, - 2.677945044588987 - ], - [ - -0.2, - 0.1, - 2.677945044588987 - ] - ] - }, - { - "trajectory_id": 39, - "start_angle_index": 7, - "end_angle_index": 8, - "left_turn": true, - "trajectory_radius": 0.63541, - "trajectory_length": 0.36821, - "arc_length": 0.29461, - "straight_length": 0.07361, - "poses": [ - [ - -0.04705, - 0.02352, - 2.677945044588987 - ], - [ - -0.09444, - 0.04634, - 2.727671917388891 - ], - [ - -0.14342, - 0.06548, - 2.8104560646290713 - ], - [ - -0.19381, - 0.0805, - 2.893240211869252 - ], - [ - -0.24528, - 0.09131, - 2.976024359109432 - ], - [ - -0.29746, - 0.09782, - 3.058808506349613 - ], - [ - -0.35, - 0.1, - 3.141592653589793 - ] - ] - }, - { - "trajectory_id": 40, - "start_angle_index": 8, - "end_angle_index": 9, - "left_turn": true, - "trajectory_radius": 0.63541, - "trajectory_length": 0.36821, - "arc_length": 0.29461, - "straight_length": 0.07361, - "poses": [ - [ - -0.05254, - -0.00218, - 3.2243768008299734 - ], - [ - -0.10472, - -0.00869, - 3.307160948070154 - ], - [ - -0.15619, - -0.0195, - 3.3899450953103343 - ], - [ - -0.20658, - -0.03452, - 3.472729242550515 - ], - [ - -0.25556, - -0.05366, - 3.555513389790695 - ], - [ - -0.30295, - -0.07648, - 3.6052402625905993 - ], - [ - -0.35, - -0.1, - 3.6052402625905993 - ] - ] - }, - { - "trajectory_id": 41, - "start_angle_index": 8, - "end_angle_index": 11, - "left_turn": true, - "trajectory_radius": 0.52586, - "trajectory_length": 0.64852, - "arc_length": 0.58221, - "straight_length": 0.06631, - "poses": [ - [ - -0.04981, - -0.00236, - 3.23645807854959 - ], - [ - -0.09917, - -0.00944, - 3.331323503509387 - ], - [ - -0.14765, - -0.02115, - 3.4261889284691835 - ], - [ - -0.19479, - -0.03741, - 3.52105435342898 - ], - [ - -0.24018, - -0.05805, - 3.615919778388777 - ], - [ - -0.28341, - -0.08291, - 3.710785203348574 - ], - [ - -0.3241, - -0.11175, - 3.8056506283083706 - ], - [ - -0.36187, - -0.14431, - 3.9005160532681673 - ], - [ - -0.39638, - -0.1803, - 3.995381478227964 - ], - [ - -0.42733, - -0.2194, - 4.090246903187761 - ], - [ - -0.45444, - -0.26126, - 4.185112328147557 - ], - [ - -0.47769, - -0.30538, - 4.2487413713838835 - ], - [ - -0.5, - -0.35, - 4.2487413713838835 - ] - ] - }, - { - "trajectory_id": 42, - "start_angle_index": 8, - "end_angle_index": 5, - "left_turn": false, - "trajectory_radius": 0.52586, - "trajectory_length": 0.64852, - "arc_length": 0.58221, - "straight_length": 0.06631, - "poses": [ - [ - -0.04981, - 0.00236, - 3.0467272286299965 - ], - [ - -0.09917, - 0.00944, - 2.9518618036701993 - ], - [ - -0.14765, - 0.02115, - 2.8569963787104027 - ], - [ - -0.19479, - 0.03741, - 2.762130953750606 - ], - [ - -0.24018, - 0.05805, - 2.6672655287908094 - ], - [ - -0.28341, - 0.08291, - 2.5724001038310123 - ], - [ - -0.3241, - 0.11175, - 2.4775346788712156 - ], - [ - -0.36187, - 0.14431, - 2.382669253911419 - ], - [ - -0.39638, - 0.1803, - 2.2878038289516223 - ], - [ - -0.42733, - 0.2194, - 2.1929384039918256 - ], - [ - -0.45444, - 0.26126, - 2.098072979032029 - ], - [ - -0.47769, - 0.30538, - 2.0344439357957027 - ], - [ - -0.5, - 0.35, - 2.0344439357957027 - ] - ] - }, - { - "trajectory_id": 43, - "start_angle_index": 8, - "end_angle_index": 7, - "left_turn": false, - "trajectory_radius": 0.63541, - "trajectory_length": 0.36821, - "arc_length": 0.29461, - "straight_length": 0.07361, - "poses": [ - [ - -0.05254, - 0.00218, - 3.058808506349613 - ], - [ - -0.10472, - 0.00869, - 2.976024359109432 - ], - [ - -0.15619, - 0.0195, - 2.893240211869252 - ], - [ - -0.20658, - 0.03452, - 2.8104560646290713 - ], - [ - -0.25556, - 0.05366, - 2.727671917388891 - ], - [ - -0.30295, - 0.07648, - 2.677945044588987 - ], - [ - -0.35, - 0.1, - 2.677945044588987 - ] - ] - }, - { - "trajectory_id": 44, - "start_angle_index": 8, - "end_angle_index": 8, - "left_turn": true, - "trajectory_radius": 0.0, - "trajectory_length": 0.15, - "arc_length": 0.0, - "straight_length": 0.15, - "poses": [ - [ - -0.05, - 0.0, - 3.141592653589793 - ], - [ - -0.1, - 0.0, - 3.141592653589793 - ], - [ - -0.15, - 0.0, - 3.141592653589793 - ] - ] - }, - { - "trajectory_id": 45, - "start_angle_index": 9, - "end_angle_index": 9, - "left_turn": true, - "trajectory_radius": 0.0, - "trajectory_length": 0.22361, - "arc_length": 0.0, - "straight_length": 0.22361, - "poses": [ - [ - -0.05, - -0.025, - 3.6052402625905993 - ], - [ - -0.1, - -0.05, - 3.6052402625905993 - ], - [ - -0.15, - -0.075, - 3.6052402625905993 - ], - [ - -0.2, - -0.1, - 3.6052402625905993 - ] - ] - }, - { - "trajectory_id": 46, - "start_angle_index": 9, - "end_angle_index": 10, - "left_turn": true, - "trajectory_radius": 0.68895, - "trajectory_length": 0.25129, - "arc_length": 0.22167, - "straight_length": 0.02962, - "poses": [ - [ - -0.04409, - -0.0241, - 3.678189359241699 - ], - [ - -0.08631, - -0.05134, - 3.751138455892799 - ], - [ - -0.12643, - -0.08159, - 3.8240875525438986 - ], - [ - -0.16424, - -0.11469, - 3.8970366491949986 - ], - [ - -0.2, - -0.15, - 3.9269908169872414 - ] - ] - }, - { - "trajectory_id": 47, - "start_angle_index": 9, - "end_angle_index": 7, - "left_turn": false, - "trajectory_radius": 0.50312, - "trajectory_length": 0.46654, - "arc_length": 0.46654, - "straight_length": 0.0, - "poses": [ - [ - -0.04747, - -0.02076, - 3.5022074605904203 - ], - [ - -0.09683, - -0.03652, - 3.399174658590241 - ], - [ - -0.14755, - -0.04712, - 3.296141856590062 - ], - [ - -0.19909, - -0.05245, - 3.1931090545898826 - ], - [ - -0.25091, - -0.05245, - 3.0900762525897036 - ], - [ - -0.30245, - -0.04712, - 2.9870434505895243 - ], - [ - -0.35317, - -0.03652, - 2.8840106485893453 - ], - [ - -0.40253, - -0.02076, - 2.780977846589166 - ], - [ - -0.45, - 0.0, - 2.677945044588987 - ] - ] - }, - { - "trajectory_id": 48, - "start_angle_index": 9, - "end_angle_index": 7, - "left_turn": false, - "trajectory_radius": 0.61492, - "trajectory_length": 0.57021, - "arc_length": 0.57021, - "straight_length": 0.0, - "poses": [ - [ - -0.04729, - -0.0212, - 3.5209406973177253 - ], - [ - -0.09619, - -0.03835, - 3.4366411320448518 - ], - [ - -0.14636, - -0.05131, - 3.352341566771978 - ], - [ - -0.19745, - -0.06001, - 3.268042001499104 - ], - [ - -0.24909, - -0.06437, - 3.18374243622623 - ], - [ - -0.30091, - -0.06437, - 3.0994428709533564 - ], - [ - -0.35255, - -0.06001, - 3.0151433056804824 - ], - [ - -0.40364, - -0.05131, - 2.9308437404076084 - ], - [ - -0.45381, - -0.03835, - 2.8465441751347345 - ], - [ - -0.50271, - -0.0212, - 2.762244609861861 - ], - [ - -0.55, - 0.0, - 2.677945044588987 - ] - ] - }, - { - "trajectory_id": 49, - "start_angle_index": 9, - "end_angle_index": 8, - "left_turn": false, - "trajectory_radius": 0.63541, - "trajectory_length": 0.36821, - "arc_length": 0.29461, - "straight_length": 0.07361, - "poses": [ - [ - -0.04705, - -0.02352, - 3.6052402625905993 - ], - [ - -0.09444, - -0.04634, - 3.555513389790695 - ], - [ - -0.14342, - -0.06548, - 3.472729242550515 - ], - [ - -0.19381, - -0.0805, - 3.3899450953103343 - ], - [ - -0.24528, - -0.09131, - 3.307160948070154 - ], - [ - -0.29746, - -0.09782, - 3.2243768008299734 - ], - [ - -0.35, - -0.1, - 3.141592653589793 - ] - ] - }, - { - "trajectory_id": 50, - "start_angle_index": 10, - "end_angle_index": 9, - "left_turn": false, - "trajectory_radius": 0.68895, - "trajectory_length": 0.25129, - "arc_length": 0.22167, - "straight_length": 0.02962, - "poses": [ - [ - -0.03576, - -0.03531, - 3.8970366491949986 - ], - [ - -0.07357, - -0.06841, - 3.8240875525438986 - ], - [ - -0.11369, - -0.09866, - 3.751138455892799 - ], - [ - -0.15591, - -0.1259, - 3.678189359241699 - ], - [ - -0.2, - -0.15, - 3.6052402625905993 - ] - ] - }, - { - "trajectory_id": 51, - "start_angle_index": 10, - "end_angle_index": 10, - "left_turn": true, - "trajectory_radius": 0.0, - "trajectory_length": 0.21213, - "arc_length": 0.0, - "straight_length": 0.21213, - "poses": [ - [ - -0.0375, - -0.0375, - 3.9269908169872414 - ], - [ - -0.075, - -0.075, - 3.9269908169872414 - ], - [ - -0.1125, - -0.1125, - 3.9269908169872414 - ], - [ - -0.15, - -0.15, - 3.9269908169872414 - ] - ] - }, - { - "trajectory_id": 52, - "start_angle_index": 10, - "end_angle_index": 11, - "left_turn": true, - "trajectory_radius": 0.68895, - "trajectory_length": 0.25129, - "arc_length": 0.22167, - "straight_length": 0.02962, - "poses": [ - [ - -0.03531, - -0.03576, - 3.956944984779484 - ], - [ - -0.06841, - -0.07357, - 4.029894081430584 - ], - [ - -0.09866, - -0.11369, - 4.102843178081684 - ], - [ - -0.1259, - -0.15591, - 4.175792274732784 - ], - [ - -0.15, - -0.2, - 4.2487413713838835 - ] - ] - }, - { - "trajectory_id": 53, - "start_angle_index": 10, - "end_angle_index": 12, - "left_turn": true, - "trajectory_radius": 0.51213, - "trajectory_length": 0.4401, - "arc_length": 0.40223, - "straight_length": 0.03787, - "poses": [ - [ - -0.03288, - -0.03617, - 4.022473509407077 - ], - [ - -0.06215, - -0.07532, - 4.117956201826914 - ], - [ - -0.08756, - -0.11707, - 4.2134388942467496 - ], - [ - -0.10888, - -0.16106, - 4.308921586666585 - ], - [ - -0.1259, - -0.20688, - 4.404404279086421 - ], - [ - -0.13848, - -0.25412, - 4.499886971506257 - ], - [ - -0.1465, - -0.30234, - 4.595369663926093 - ], - [ - -0.14988, - -0.3511, - 4.690852356345929 - ], - [ - -0.15, - -0.4, - 4.71238898038469 - ] - ] - }, - { - "trajectory_id": 54, - "start_angle_index": 10, - "end_angle_index": 8, - "left_turn": false, - "trajectory_radius": 0.51213, - "trajectory_length": 0.4401, - "arc_length": 0.40223, - "straight_length": 0.03787, - "poses": [ - [ - -0.03617, - -0.03288, - 3.8315081245674056 - ], - [ - -0.07532, - -0.06215, - 3.7360254321475694 - ], - [ - -0.11707, - -0.08756, - 3.6405427397277337 - ], - [ - -0.16106, - -0.10888, - 3.5450600473078975 - ], - [ - -0.20688, - -0.1259, - 3.4495773548880617 - ], - [ - -0.25412, - -0.13848, - 3.354094662468226 - ], - [ - -0.30234, - -0.1465, - 3.2586119700483898 - ], - [ - -0.3511, - -0.14988, - 3.1631292776285536 - ], - [ - -0.4, - -0.15, - 3.141592653589793 - ] - ] - }, - { - "trajectory_id": 55, - "start_angle_index": 11, - "end_angle_index": 10, - "left_turn": false, - "trajectory_radius": 0.68895, - "trajectory_length": 0.25129, - "arc_length": 0.22167, - "straight_length": 0.02962, - "poses": [ - [ - -0.0241, - -0.04409, - 4.175792274732784 - ], - [ - -0.05134, - -0.08631, - 4.102843178081684 - ], - [ - -0.08159, - -0.12643, - 4.029894081430584 - ], - [ - -0.11469, - -0.16424, - 3.956944984779484 - ], - [ - -0.15, - -0.2, - 3.9269908169872414 - ] - ] - }, - { - "trajectory_id": 56, - "start_angle_index": 11, - "end_angle_index": 11, - "left_turn": true, - "trajectory_radius": 0.0, - "trajectory_length": 0.22361, - "arc_length": 0.0, - "straight_length": 0.22361, - "poses": [ - [ - -0.025, - -0.05, - 4.2487413713838835 - ], - [ - -0.05, - -0.1, - 4.2487413713838835 - ], - [ - -0.075, - -0.15, - 4.2487413713838835 - ], - [ - -0.1, - -0.2, - 4.2487413713838835 - ] - ] - }, - { - "trajectory_id": 57, - "start_angle_index": 11, - "end_angle_index": 12, - "left_turn": true, - "trajectory_radius": 0.63541, - "trajectory_length": 0.36821, - "arc_length": 0.29461, - "straight_length": 0.07361, - "poses": [ - [ - -0.02352, - -0.04705, - 4.2487413713838835 - ], - [ - -0.04634, - -0.09444, - 4.298468244183788 - ], - [ - -0.06548, - -0.14342, - 4.381252391423968 - ], - [ - -0.0805, - -0.19381, - 4.464036538664148 - ], - [ - -0.09131, - -0.24528, - 4.546820685904329 - ], - [ - -0.09782, - -0.29746, - 4.629604833144509 - ], - [ - -0.1, - -0.35, - 4.71238898038469 - ] - ] - }, - { - "trajectory_id": 58, - "start_angle_index": 11, - "end_angle_index": 13, - "left_turn": true, - "trajectory_radius": 0.50312, - "trajectory_length": 0.46654, - "arc_length": 0.46654, - "straight_length": 0.0, - "poses": [ - [ - -0.02076, - -0.04747, - 4.3517741733840625 - ], - [ - -0.03652, - -0.09683, - 4.454806975384242 - ], - [ - -0.04712, - -0.14755, - 4.557839777384421 - ], - [ - -0.05245, - -0.19909, - 4.6608725793846 - ], - [ - -0.05245, - -0.25091, - 4.763905381384779 - ], - [ - -0.04712, - -0.30245, - 4.866938183384958 - ], - [ - -0.03652, - -0.35317, - 4.969970985385137 - ], - [ - -0.02076, - -0.40253, - 5.073003787385317 - ], - [ - 0.0, - -0.45, - 5.176036589385496 - ] - ] - }, - { - "trajectory_id": 59, - "start_angle_index": 11, - "end_angle_index": 13, - "left_turn": true, - "trajectory_radius": 0.61492, - "trajectory_length": 0.57021, - "arc_length": 0.57021, - "straight_length": 0.0, - "poses": [ - [ - -0.0212, - -0.04729, - 4.333040936656757 - ], - [ - -0.03835, - -0.09619, - 4.4173405019296315 - ], - [ - -0.05131, - -0.14636, - 4.501640067202505 - ], - [ - -0.06001, - -0.19745, - 4.5859396324753785 - ], - [ - -0.06437, - -0.24909, - 4.670239197748253 - ], - [ - -0.06437, - -0.30091, - 4.754538763021126 - ], - [ - -0.06001, - -0.35255, - 4.838838328294001 - ], - [ - -0.05131, - -0.40364, - 4.923137893566874 - ], - [ - -0.03835, - -0.45381, - 5.007437458839748 - ], - [ - -0.0212, - -0.50271, - 5.091737024112621 - ], - [ - 0.0, - -0.55, - 5.176036589385496 - ] - ] - }, - { - "trajectory_id": 60, - "start_angle_index": 12, - "end_angle_index": 9, - "left_turn": false, - "trajectory_radius": 0.52586, - "trajectory_length": 0.64852, - "arc_length": 0.58221, - "straight_length": 0.06631, - "poses": [ - [ - -0.00236, - -0.04981, - 4.617523555424893 - ], - [ - -0.00944, - -0.09917, - 4.522658130465096 - ], - [ - -0.02115, - -0.14765, - 4.427792705505299 - ], - [ - -0.03741, - -0.19479, - 4.332927280545503 - ], - [ - -0.05805, - -0.24018, - 4.2380618555857055 - ], - [ - -0.08291, - -0.28341, - 4.143196430625909 - ], - [ - -0.11175, - -0.3241, - 4.048331005666112 - ], - [ - -0.14431, - -0.36187, - 3.9534655807063155 - ], - [ - -0.1803, - -0.39638, - 3.858600155746519 - ], - [ - -0.2194, - -0.42733, - 3.763734730786722 - ], - [ - -0.26126, - -0.45444, - 3.6688693058269255 - ], - [ - -0.30538, - -0.47769, - 3.6052402625905993 - ], - [ - -0.35, - -0.5, - 3.6052402625905993 - ] - ] - }, - { - "trajectory_id": 61, - "start_angle_index": 12, - "end_angle_index": 11, - "left_turn": false, - "trajectory_radius": 0.63541, - "trajectory_length": 0.36821, - "arc_length": 0.29461, - "straight_length": 0.07361, - "poses": [ - [ - -0.00218, - -0.05254, - 4.629604833144509 - ], - [ - -0.00869, - -0.10472, - 4.546820685904329 - ], - [ - -0.0195, - -0.15619, - 4.464036538664148 - ], - [ - -0.03452, - -0.20658, - 4.381252391423968 - ], - [ - -0.05366, - -0.25556, - 4.298468244183788 - ], - [ - -0.07648, - -0.30295, - 4.2487413713838835 - ], - [ - -0.1, - -0.35, - 4.2487413713838835 - ] - ] - }, - { - "trajectory_id": 62, - "start_angle_index": 12, - "end_angle_index": 12, - "left_turn": true, - "trajectory_radius": 0.0, - "trajectory_length": 0.15, - "arc_length": 0.0, - "straight_length": 0.15, - "poses": [ - [ - 0.0, - -0.05, - 4.71238898038469 - ], - [ - 0.0, - -0.1, - 4.71238898038469 - ], - [ - 0.0, - -0.15, - 4.71238898038469 - ] - ] - }, - { - "trajectory_id": 63, - "start_angle_index": 12, - "end_angle_index": 13, - "left_turn": true, - "trajectory_radius": 0.63541, - "trajectory_length": 0.36821, - "arc_length": 0.29461, - "straight_length": 0.07361, - "poses": [ - [ - 0.00218, - -0.05254, - 4.79517312762487 - ], - [ - 0.00869, - -0.10472, - 4.87795727486505 - ], - [ - 0.0195, - -0.15619, - 4.960741422105231 - ], - [ - 0.03452, - -0.20658, - 5.0435255693454115 - ], - [ - 0.05366, - -0.25556, - 5.126309716585592 - ], - [ - 0.07648, - -0.30295, - 5.176036589385496 - ], - [ - 0.1, - -0.35, - 5.176036589385496 - ] - ] - }, - { - "trajectory_id": 64, - "start_angle_index": 12, - "end_angle_index": 15, - "left_turn": true, - "trajectory_radius": 0.52586, - "trajectory_length": 0.64852, - "arc_length": 0.58221, - "straight_length": 0.06631, - "poses": [ - [ - 0.00236, - -0.04981, - 4.807254405344486 - ], - [ - 0.00944, - -0.09917, - 4.902119830304283 - ], - [ - 0.02115, - -0.14765, - 4.9969852552640805 - ], - [ - 0.03741, - -0.19479, - 5.091850680223876 - ], - [ - 0.05805, - -0.24018, - 5.186716105183674 - ], - [ - 0.08291, - -0.28341, - 5.2815815301434705 - ], - [ - 0.11175, - -0.3241, - 5.376446955103267 - ], - [ - 0.14431, - -0.36187, - 5.471312380063064 - ], - [ - 0.1803, - -0.39638, - 5.5661778050228605 - ], - [ - 0.2194, - -0.42733, - 5.661043229982657 - ], - [ - 0.26126, - -0.45444, - 5.755908654942454 - ], - [ - 0.30538, - -0.47769, - 5.81953769817878 - ], - [ - 0.35, - -0.5, - 5.81953769817878 - ] - ] - }, - { - "trajectory_id": 65, - "start_angle_index": 13, - "end_angle_index": 11, - "left_turn": false, - "trajectory_radius": 0.50312, - "trajectory_length": 0.46654, - "arc_length": 0.46654, - "straight_length": 0.0, - "poses": [ - [ - 0.02076, - -0.04747, - 5.073003787385317 - ], - [ - 0.03652, - -0.09683, - 4.969970985385137 - ], - [ - 0.04712, - -0.14755, - 4.866938183384958 - ], - [ - 0.05245, - -0.19909, - 4.763905381384779 - ], - [ - 0.05245, - -0.25091, - 4.6608725793846 - ], - [ - 0.04712, - -0.30245, - 4.557839777384421 - ], - [ - 0.03652, - -0.35317, - 4.454806975384242 - ], - [ - 0.02076, - -0.40253, - 4.3517741733840625 - ], - [ - 0.0, - -0.45, - 4.2487413713838835 - ] - ] - }, - { - "trajectory_id": 66, - "start_angle_index": 13, - "end_angle_index": 11, - "left_turn": false, - "trajectory_radius": 0.61492, - "trajectory_length": 0.57021, - "arc_length": 0.57021, - "straight_length": 0.0, - "poses": [ - [ - 0.0212, - -0.04729, - 5.091737024112622 - ], - [ - 0.03835, - -0.09619, - 5.007437458839748 - ], - [ - 0.05131, - -0.14636, - 4.923137893566874 - ], - [ - 0.06001, - -0.19745, - 4.838838328294001 - ], - [ - 0.06437, - -0.24909, - 4.754538763021126 - ], - [ - 0.06437, - -0.30091, - 4.670239197748253 - ], - [ - 0.06001, - -0.35255, - 4.5859396324753785 - ], - [ - 0.05131, - -0.40364, - 4.501640067202505 - ], - [ - 0.03835, - -0.45381, - 4.4173405019296315 - ], - [ - 0.0212, - -0.50271, - 4.333040936656758 - ], - [ - 0.0, - -0.55, - 4.2487413713838835 - ] - ] - }, - { - "trajectory_id": 67, - "start_angle_index": 13, - "end_angle_index": 12, - "left_turn": false, - "trajectory_radius": 0.63541, - "trajectory_length": 0.36821, - "arc_length": 0.29461, - "straight_length": 0.07361, - "poses": [ - [ - 0.02352, - -0.04705, - 5.176036589385496 - ], - [ - 0.04634, - -0.09444, - 5.126309716585592 - ], - [ - 0.06548, - -0.14342, - 5.0435255693454115 - ], - [ - 0.0805, - -0.19381, - 4.960741422105231 - ], - [ - 0.09131, - -0.24528, - 4.87795727486505 - ], - [ - 0.09782, - -0.29746, - 4.79517312762487 - ], - [ - 0.1, - -0.35, - 4.71238898038469 - ] - ] - }, - { - "trajectory_id": 68, - "start_angle_index": 13, - "end_angle_index": 13, - "left_turn": true, - "trajectory_radius": 0.0, - "trajectory_length": 0.22361, - "arc_length": 0.0, - "straight_length": 0.22361, - "poses": [ - [ - 0.025, - -0.05, - 5.176036589385496 - ], - [ - 0.05, - -0.1, - 5.176036589385496 - ], - [ - 0.075, - -0.15, - 5.176036589385496 - ], - [ - 0.1, - -0.2, - 5.176036589385496 - ] - ] - }, - { - "trajectory_id": 69, - "start_angle_index": 13, - "end_angle_index": 14, - "left_turn": true, - "trajectory_radius": 0.68895, - "trajectory_length": 0.25129, - "arc_length": 0.22167, - "straight_length": 0.02962, - "poses": [ - [ - 0.0241, - -0.04409, - 5.248985686036596 - ], - [ - 0.05134, - -0.08631, - 5.321934782687696 - ], - [ - 0.08159, - -0.12643, - 5.394883879338796 - ], - [ - 0.11469, - -0.16424, - 5.467832975989895 - ], - [ - 0.15, - -0.2, - 5.497787143782138 - ] - ] - }, - { - "trajectory_id": 70, - "start_angle_index": 14, - "end_angle_index": 12, - "left_turn": false, - "trajectory_radius": 0.51213, - "trajectory_length": 0.4401, - "arc_length": 0.40223, - "straight_length": 0.03787, - "poses": [ - [ - 0.03288, - -0.03617, - 5.402304451362302 - ], - [ - 0.06215, - -0.07532, - 5.306821758942466 - ], - [ - 0.08756, - -0.11707, - 5.21133906652263 - ], - [ - 0.10888, - -0.16106, - 5.115856374102794 - ], - [ - 0.1259, - -0.20688, - 5.020373681682958 - ], - [ - 0.13848, - -0.25412, - 4.9248909892631225 - ], - [ - 0.1465, - -0.30234, - 4.829408296843287 - ], - [ - 0.14988, - -0.3511, - 4.73392560442345 - ], - [ - 0.15, - -0.4, - 4.71238898038469 - ] - ] - }, - { - "trajectory_id": 71, - "start_angle_index": 14, - "end_angle_index": 13, - "left_turn": false, - "trajectory_radius": 0.68895, - "trajectory_length": 0.25129, - "arc_length": 0.22167, - "straight_length": 0.02962, - "poses": [ - [ - 0.03531, - -0.03576, - 5.467832975989895 - ], - [ - 0.06841, - -0.07357, - 5.394883879338796 - ], - [ - 0.09866, - -0.11369, - 5.321934782687696 - ], - [ - 0.1259, - -0.15591, - 5.248985686036596 - ], - [ - 0.15, - -0.2, - 5.176036589385496 - ] - ] - }, - { - "trajectory_id": 72, - "start_angle_index": 14, - "end_angle_index": 14, - "left_turn": true, - "trajectory_radius": 0.0, - "trajectory_length": 0.21213, - "arc_length": 0.0, - "straight_length": 0.21213, - "poses": [ - [ - 0.0375, - -0.0375, - 5.497787143782138 - ], - [ - 0.075, - -0.075, - 5.497787143782138 - ], - [ - 0.1125, - -0.1125, - 5.497787143782138 - ], - [ - 0.15, - -0.15, - 5.497787143782138 - ] - ] - }, - { - "trajectory_id": 73, - "start_angle_index": 14, - "end_angle_index": 15, - "left_turn": true, - "trajectory_radius": 0.68895, - "trajectory_length": 0.25129, - "arc_length": 0.22167, - "straight_length": 0.02962, - "poses": [ - [ - 0.03576, - -0.03531, - 5.527741311574381 - ], - [ - 0.07357, - -0.06841, - 5.60069040822548 - ], - [ - 0.11369, - -0.09866, - 5.67363950487658 - ], - [ - 0.15591, - -0.1259, - 5.74658860152768 - ], - [ - 0.2, - -0.15, - 5.81953769817878 - ] - ] - }, - { - "trajectory_id": 74, - "start_angle_index": 14, - "end_angle_index": 0, - "left_turn": true, - "trajectory_radius": 0.51213, - "trajectory_length": 0.4401, - "arc_length": 0.40223, - "straight_length": 0.03787, - "poses": [ - [ - 0.03617, - -0.03288, - 5.593269836201974 - ], - [ - 0.07532, - -0.06215, - 5.6887525286218095 - ], - [ - 0.11707, - -0.08756, - 5.784235221041646 - ], - [ - 0.16106, - -0.10888, - 5.879717913461482 - ], - [ - 0.20688, - -0.1259, - 5.975200605881318 - ], - [ - 0.25412, - -0.13848, - 6.070683298301153 - ], - [ - 0.30234, - -0.1465, - 6.166165990720989 - ], - [ - 0.3511, - -0.14988, - 6.261648683140826 - ], - [ - 0.4, - -0.15, - 0.0 - ] - ] - }, - { - "trajectory_id": 75, - "start_angle_index": 15, - "end_angle_index": 14, - "left_turn": false, - "trajectory_radius": 0.68895, - "trajectory_length": 0.25129, - "arc_length": 0.22167, - "straight_length": 0.02962, - "poses": [ - [ - 0.04409, - -0.0241, - 5.74658860152768 - ], - [ - 0.08631, - -0.05134, - 5.67363950487658 - ], - [ - 0.12643, - -0.08159, - 5.60069040822548 - ], - [ - 0.16424, - -0.11469, - 5.527741311574381 - ], - [ - 0.2, - -0.15, - 5.497787143782138 - ] - ] - }, - { - "trajectory_id": 76, - "start_angle_index": 15, - "end_angle_index": 15, - "left_turn": true, - "trajectory_radius": 0.0, - "trajectory_length": 0.22361, - "arc_length": 0.0, - "straight_length": 0.22361, - "poses": [ - [ - 0.05, - -0.025, - 5.81953769817878 - ], - [ - 0.1, - -0.05, - 5.81953769817878 - ], - [ - 0.15, - -0.075, - 5.81953769817878 - ], - [ - 0.2, - -0.1, - 5.81953769817878 - ] - ] - }, - { - "trajectory_id": 77, - "start_angle_index": 15, - "end_angle_index": 0, - "left_turn": true, - "trajectory_radius": 0.63541, - "trajectory_length": 0.36821, - "arc_length": 0.29461, - "straight_length": 0.07361, - "poses": [ - [ - 0.04705, - -0.02352, - 5.81953769817878 - ], - [ - 0.09444, - -0.04634, - 5.869264570978684 - ], - [ - 0.14342, - -0.06548, - 5.952048718218864 - ], - [ - 0.19381, - -0.0805, - 6.034832865459045 - ], - [ - 0.24528, - -0.09131, - 6.117617012699226 - ], - [ - 0.29746, - -0.09782, - 6.200401159939406 - ], - [ - 0.35, - -0.1, - 0.0 - ] - ] - }, - { - "trajectory_id": 78, - "start_angle_index": 15, - "end_angle_index": 1, - "left_turn": true, - "trajectory_radius": 0.50312, - "trajectory_length": 0.46654, - "arc_length": 0.46654, - "straight_length": 0.0, - "poses": [ - [ - 0.04747, - -0.02076, - 5.922570500178959 - ], - [ - 0.09683, - -0.03652, - 6.025603302179139 - ], - [ - 0.14755, - -0.04712, - 6.128636104179318 - ], - [ - 0.19909, - -0.05245, - 6.231668906179497 - ], - [ - 0.25091, - -0.05245, - 0.05151640100008953 - ], - [ - 0.30245, - -0.04712, - 0.1545492030002687 - ], - [ - 0.35317, - -0.03652, - 0.25758200500044787 - ], - [ - 0.40253, - -0.02076, - 0.36061480700062715 - ], - [ - 0.45, - 0.0, - 0.4636476090008061 - ] - ] - }, - { - "trajectory_id": 79, - "start_angle_index": 15, - "end_angle_index": 1, - "left_turn": true, - "trajectory_radius": 0.61492, - "trajectory_length": 0.57021, - "arc_length": 0.57021, - "straight_length": 0.0, - "poses": [ - [ - 0.04729, - -0.0212, - 5.903837263451654 - ], - [ - 0.09619, - -0.03835, - 5.988136828724528 - ], - [ - 0.14636, - -0.05131, - 6.0724363939974015 - ], - [ - 0.19745, - -0.06001, - 6.156735959270275 - ], - [ - 0.24909, - -0.06437, - 6.2410355245431495 - ], - [ - 0.30091, - -0.06437, - 0.04214978263643693 - ], - [ - 0.35255, - -0.06001, - 0.1264493479093109 - ], - [ - 0.40364, - -0.05131, - 0.21074891318218475 - ], - [ - 0.45381, - -0.03835, - 0.2950484784550586 - ], - [ - 0.50271, - -0.0212, - 0.37934804372793235 - ], - [ - 0.55, - 0.0, - 0.4636476090008061 - ] - ] - } - ] -} diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index 2762ae37c8a..52b94db14af 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -43,8 +43,9 @@ AStarAlgorithm::AStarAlgorithm( _x_size(0), _y_size(0), _search_info(search_info), + _goal_coordinates(Coordinates()), _start(nullptr), - _goal_manager(GoalManagerT()), + _goal(nullptr), _motion_model(motion_model) { _graph.reserve(100000); @@ -191,102 +192,35 @@ template<> void AStarAlgorithm::setGoal( const float & mx, const float & my, - const unsigned int & dim_3, - const GoalHeadingMode & /*goal_heading_mode*/, - const int & /*coarse_search_resolution*/) + const unsigned int & dim_3) { if (dim_3 != 0) { throw std::runtime_error("Node type Node2D cannot be given non-zero goal dim 3."); } - NodePtr goal = addToGraph( + _goal = addToGraph( Node2D::getIndex( static_cast(mx), static_cast(my), getSizeX())); - - goal->setPose(Node2D::Coordinates(mx, my)); - GoalStateVector goals_state; - goals_state.push_back({goal, true}); - _goal_manager.setGoalStates(goals_state); - _coarse_search_resolution = 1; + _goal_coordinates = Node2D::Coordinates(mx, my); } template void AStarAlgorithm::setGoal( const float & mx, const float & my, - const unsigned int & dim_3, - const GoalHeadingMode & goal_heading_mode, - const int & coarse_search_resolution) + const unsigned int & dim_3) { - // Default to minimal resolution unless overridden for ALL_DIRECTION - _coarse_search_resolution = 1; - - unsigned int num_bins = NodeT::motion_table.num_angle_quantization; - GoalStateVector goals_state; - - // set goal based on heading mode - switch (goal_heading_mode) { - case GoalHeadingMode::DEFAULT: { - // add a single goal node with single heading - auto goal = addToGraph(NodeT::getIndex(mx, my, dim_3)); - goal->setPose(typename NodeT::Coordinates( - static_cast(mx), static_cast(my), static_cast(dim_3))); - goals_state.push_back({goal, true}); - break; - } - - case GoalHeadingMode::BIDIRECTIONAL: { - // Add two goals, one for each direction - // add goal in original direction - auto goal = addToGraph(NodeT::getIndex(mx, my, dim_3)); - goal->setPose(typename NodeT::Coordinates( - static_cast(mx), static_cast(my), static_cast(dim_3))); - goals_state.push_back({goal, true}); - - // Add goal node in opposite (180°) direction - unsigned int opposite_heading = (dim_3 + (num_bins / 2)) % num_bins; - auto opposite_goal = addToGraph(NodeT::getIndex(mx, my, opposite_heading)); - opposite_goal->setPose(typename NodeT::Coordinates( - static_cast(mx), static_cast(my), static_cast(opposite_heading))); - goals_state.push_back({opposite_goal, true}); - break; - } - - case GoalHeadingMode::ALL_DIRECTION: { - // Set the coarse search resolution only for all direction - _coarse_search_resolution = coarse_search_resolution; - - // Add goal nodes for all headings - for (unsigned int i = 0; i < num_bins; ++i) { - auto goal = addToGraph(NodeT::getIndex(mx, my, i)); - goal->setPose(typename NodeT::Coordinates( - static_cast(mx), static_cast(my), static_cast(i))); - goals_state.push_back({goal, true}); - } - break; - } - case GoalHeadingMode::UNKNOWN: - throw std::runtime_error("Goal heading is UNKNOWN."); - } - - // check if we need to reset the obstacle heuristic, we only need to - // check that the x and y component has changed - const auto & previous_goals = _goal_manager.getGoalsState(); - - // Lambda to check if goal has changed - auto goalHasChanged = [&]() -> bool { - if (previous_goals.empty()) { - return true; - } + _goal = addToGraph( + NodeT::getIndex( + static_cast(mx), + static_cast(my), + dim_3)); - const auto & prev = previous_goals.front().goal; - const auto & curr = goals_state.front().goal; - return (prev->pose.x != curr->pose.x) || (prev->pose.y != curr->pose.y); - }; + typename NodeT::Coordinates goal_coords(mx, my, dim_3); - if (!_search_info.cache_obstacle_heuristic || goalHasChanged()) { + if (!_search_info.cache_obstacle_heuristic || goal_coords != _goal_coordinates) { if (!_start) { throw std::runtime_error("Start must be set before goal."); } @@ -295,8 +229,8 @@ void AStarAlgorithm::setGoal( _collision_checker->getCostmapROS(), _start->pose.x, _start->pose.y, mx, my); } - // assign the goals state - _goal_manager.setGoalStates(goals_state); + _goal_coordinates = goal_coords; + _goal->setPose(_goal_coordinates); } template @@ -308,15 +242,14 @@ bool AStarAlgorithm::areInputsValid() } // Check if points were filled in - if (!_start || _goal_manager.goalsIsEmpty()) { + if (!_start || !_goal) { throw std::runtime_error("Failed to compute path, no valid start or goal given."); } - // remove invalid goals - _goal_manager.removeInvalidGoals(getToleranceHeuristic(), _collision_checker, _traverse_unknown); - // Check if ending point is valid - if (_goal_manager.getGoalsSet().empty()) { + if (getToleranceHeuristic() < 0.001 && + !_goal->isNodeValid(_traverse_unknown, _collision_checker)) + { throw nav2_core::GoalOccupied("Goal was in lethal cost"); } @@ -340,10 +273,6 @@ bool AStarAlgorithm::createPath( return false; } - NodeVector coarse_check_goals, fine_check_goals; - _goal_manager.prepareGoalsForAnalyticExpansion(coarse_check_goals, fine_check_goals, - _coarse_search_resolution); - // 0) Add starting point to the open set addNode(0.0, getStart()); getStart()->setAccumulatedCost(0.0); @@ -410,14 +339,13 @@ bool AStarAlgorithm::createPath( // 2.1) Use an analytic expansion (if available) to generate a path expansion_result = nullptr; expansion_result = _expander->tryAnalyticExpansion( - current_node, coarse_check_goals, fine_check_goals, - _goal_manager.getGoalsCoordinates(), neighborGetter, analytic_iterations, closest_distance); + current_node, getGoal(), neighborGetter, analytic_iterations, closest_distance); if (expansion_result != nullptr) { current_node = expansion_result; } // 3) Check if we're at the goal, backtrace if required - if (_goal_manager.isGoal(current_node)) { + if (isGoal(current_node)) { return current_node->backtracePath(path); } else if (_best_heuristic_node.first < getToleranceHeuristic()) { // Optimization: Let us find when in tolerance and refine within reason @@ -458,12 +386,24 @@ bool AStarAlgorithm::createPath( return false; } +template +bool AStarAlgorithm::isGoal(NodePtr & node) +{ + return node == getGoal(); +} + template typename AStarAlgorithm::NodePtr & AStarAlgorithm::getStart() { return _start; } +template +typename AStarAlgorithm::NodePtr & AStarAlgorithm::getGoal() +{ + return _goal; +} + template typename AStarAlgorithm::NodePtr AStarAlgorithm::getNextNode() { @@ -486,7 +426,9 @@ float AStarAlgorithm::getHeuristicCost(const NodePtr & node) { const Coordinates node_coords = NodeT::getCoords(node->getIndex(), getSizeX(), getSizeDim3()); - float heuristic = NodeT::getHeuristicCost(node_coords, _goal_manager.getGoalsCoordinates()); + float heuristic = NodeT::getHeuristicCost( + node_coords, _goal_coordinates); + if (heuristic < _best_heuristic_node.first) { _best_heuristic_node = {heuristic, node->getIndex()}; } @@ -551,18 +493,6 @@ unsigned int & AStarAlgorithm::getSizeDim3() return _dim3_size; } -template -unsigned int AStarAlgorithm::getCoarseSearchResolution() -{ - return _coarse_search_resolution; -} - -template -typename AStarAlgorithm::GoalManagerT AStarAlgorithm::getGoalManager() -{ - return _goal_manager; -} - // Instantiate algorithm for the supported template types template class AStarAlgorithm; template class AStarAlgorithm; diff --git a/nav2_smac_planner/src/analytic_expansion.cpp b/nav2_smac_planner/src/analytic_expansion.cpp index da46f781975..ec0ccc31f02 100644 --- a/nav2_smac_planner/src/analytic_expansion.cpp +++ b/nav2_smac_planner/src/analytic_expansion.cpp @@ -48,10 +48,7 @@ void AnalyticExpansion::setCollisionChecker( template typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalyticExpansion( - const NodePtr & current_node, - const NodeVector & coarse_check_goals, - const NodeVector & fine_check_goals, - const CoordinateVector & goals_coords, + const NodePtr & current_node, const NodePtr & goal_node, const NodeGetter & getter, int & analytic_iterations, int & closest_distance) { @@ -63,14 +60,10 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalytic const Coordinates node_coords = NodeT::getCoords( current_node->getIndex(), _collision_checker->getCostmap()->getSizeInCellsX(), _dim_3_size); - - AnalyticExpansionNodes current_best_analytic_nodes = {}; - NodePtr current_best_goal = nullptr; - float current_best_score = std::numeric_limits::max(); - closest_distance = std::min( closest_distance, - static_cast(NodeT::getHeuristicCost(node_coords, goals_coords))); + static_cast(NodeT::getHeuristicCost(node_coords, goal_node->pose))); + // 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) @@ -87,53 +80,81 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalytic if (analytic_iterations <= 0) { // Reset the counter and try the analytic path expansion analytic_iterations = desired_iterations; - bool found_valid_expansion = false; - - // First check the coarse search resolution goals - for (auto & current_goal_node : coarse_check_goals) { - AnalyticExpansionNodes analytic_nodes = - getAnalyticPath( - current_node, current_goal_node, getter, - current_node->motion_table.state_space); - if (!analytic_nodes.empty()) { - found_valid_expansion = true; - bool score = refineAnalyticPath( - current_node, current_goal_node, getter, analytic_nodes); - // Update the best score if we found a better path - if (score < current_best_score) { - current_best_analytic_nodes = analytic_nodes; - current_best_goal = current_goal_node; - current_best_score = score; + AnalyticExpansionNodes analytic_nodes = + getAnalyticPath(current_node, goal_node, getter, current_node->motion_table.state_space); + if (!analytic_nodes.empty()) { + // If we have a valid path, attempt to refine it + NodePtr node = current_node; + NodePtr test_node = current_node; + AnalyticExpansionNodes refined_analytic_nodes; + for (int i = 0; i < 8; i++) { + // Attempt to create better paths in 5 node increments, need to make sure + // they exist for each in order to do so (maximum of 40 points back). + if (test_node->parent && test_node->parent->parent && test_node->parent->parent->parent && + test_node->parent->parent->parent->parent && + test_node->parent->parent->parent->parent->parent) + { + test_node = test_node->parent->parent->parent->parent->parent; + refined_analytic_nodes = + getAnalyticPath(test_node, goal_node, getter, test_node->motion_table.state_space); + if (refined_analytic_nodes.empty()) { + break; + } + analytic_nodes = refined_analytic_nodes; + node = test_node; + } else { + break; } } - } - // perform a final search if we found a goal - if (found_valid_expansion) { - for (auto & current_goal_node : fine_check_goals) { - AnalyticExpansionNodes analytic_nodes = - getAnalyticPath( - current_node, current_goal_node, getter, - current_node->motion_table.state_space); - if (!analytic_nodes.empty()) { - bool score = refineAnalyticPath( - current_node, current_goal_node, getter, analytic_nodes); - // Update the best score if we found a better path - if (score < current_best_score) { - current_best_analytic_nodes = analytic_nodes; - current_best_goal = current_goal_node; - current_best_score = score; + // The analytic expansion can short-cut near obstacles when closer to a goal + // So, we can attempt to refine it more by increasing the possible radius + // higher than the minimum turning radius and use the best solution based on + // a scoring function similar to that used in traversal cost estimation. + auto scoringFn = [&](const AnalyticExpansionNodes & expansion) { + if (expansion.size() < 2) { + return std::numeric_limits::max(); } + + float score = 0.0; + float normalized_cost = 0.0; + // Analytic expansions are consistently spaced + const float distance = hypotf( + expansion[1].proposed_coords.x - expansion[0].proposed_coords.x, + expansion[1].proposed_coords.y - expansion[0].proposed_coords.y); + const float & weight = expansion[0].node->motion_table.cost_penalty; + for (auto iter = expansion.begin(); iter != expansion.end(); ++iter) { + normalized_cost = iter->node->getCost() / 252.0f; + // Search's Traversal Cost Function + score += distance * (1.0 + weight * normalized_cost); + } + return score; + }; + + float best_score = scoringFn(analytic_nodes); + float score = std::numeric_limits::max(); + float min_turn_rad = node->motion_table.min_turning_radius; + const float max_min_turn_rad = 4.0 * min_turn_rad; // Up to 4x the turning radius + while (min_turn_rad < max_min_turn_rad) { + min_turn_rad += 0.5; // In Grid Coords, 1/2 cell steps + ompl::base::StateSpacePtr state_space; + if (node->motion_table.motion_model == MotionModel::DUBIN) { + state_space = std::make_shared(min_turn_rad); + } else { + state_space = std::make_shared(min_turn_rad); + } + refined_analytic_nodes = getAnalyticPath(node, goal_node, getter, state_space); + score = scoringFn(refined_analytic_nodes); + if (score <= best_score) { + analytic_nodes = refined_analytic_nodes; + best_score = score; } } + + return setAnalyticPath(node, goal_node, analytic_nodes); } } - if (!current_best_analytic_nodes.empty()) { - return setAnalyticPath( - current_node, current_best_goal, - current_best_analytic_nodes); - } analytic_iterations--; } @@ -271,87 +292,6 @@ typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansion -float AnalyticExpansion::refineAnalyticPath( - const NodePtr & current_node, - const NodePtr & goal_node, - const NodeGetter & getter, - AnalyticExpansionNodes & analytic_nodes) -{ - NodePtr node = current_node; - NodePtr test_node = node; - AnalyticExpansionNodes refined_analytic_nodes; - for (int i = 0; i < 8; i++) { - // Attempt to create better paths in 5 node increments, need to make sure - // they exist for each in order to do so (maximum of 40 points back). - if (test_node->parent && test_node->parent->parent && - test_node->parent->parent->parent && - test_node->parent->parent->parent->parent && - test_node->parent->parent->parent->parent->parent) - { - test_node = test_node->parent->parent->parent->parent->parent; - // print the goals pose - refined_analytic_nodes = - getAnalyticPath( - test_node, goal_node, getter, - test_node->motion_table.state_space); - if (refined_analytic_nodes.empty()) { - break; - } - analytic_nodes = refined_analytic_nodes; - node = test_node; - } else { - break; - } - } - - // The analytic expansion can short-cut near obstacles when closer to a goal - // So, we can attempt to refine it more by increasing the possible radius - // higher than the minimum turning radius and use the best solution based on - // a scoring function similar to that used in traversal cost estimation. - auto scoringFn = [&](const AnalyticExpansionNodes & expansion) { - if (expansion.size() < 2) { - return std::numeric_limits::max(); - } - - float score = 0.0; - float normalized_cost = 0.0; - // Analytic expansions are consistently spaced - const float distance = hypotf( - expansion[1].proposed_coords.x - expansion[0].proposed_coords.x, - expansion[1].proposed_coords.y - expansion[0].proposed_coords.y); - const float & weight = expansion[0].node->motion_table.cost_penalty; - for (auto iter = expansion.begin(); iter != expansion.end(); ++iter) { - normalized_cost = iter->node->getCost() / 252.0f; - // Search's Traversal Cost Function - score += distance * (1.0 + weight * normalized_cost); - } - return score; - }; - - float best_score = scoringFn(analytic_nodes); - float score = std::numeric_limits::max(); - float min_turn_rad = node->motion_table.min_turning_radius; - const float max_min_turn_rad = 4.0 * min_turn_rad; // Up to 4x the turning radius - while (min_turn_rad < max_min_turn_rad) { - min_turn_rad += 0.5; // In Grid Coords, 1/2 cell steps - ompl::base::StateSpacePtr state_space; - if (node->motion_table.motion_model == MotionModel::DUBIN) { - state_space = std::make_shared(min_turn_rad); - } else { - state_space = std::make_shared(min_turn_rad); - } - refined_analytic_nodes = getAnalyticPath(node, goal_node, getter, state_space); - score = scoringFn(refined_analytic_nodes); - if (score <= best_score) { - analytic_nodes = refined_analytic_nodes; - best_score = score; - } - } - - return best_score; -} - template typename AnalyticExpansion::NodePtr AnalyticExpansion::setAnalyticPath( const NodePtr & node, @@ -405,16 +345,6 @@ getAnalyticPath( return AnalyticExpansionNodes(); } -template<> -float AnalyticExpansion::refineAnalyticPath( - const NodePtr &, - const NodePtr &, - const NodeGetter &, - AnalyticExpansionNodes &) -{ - return std::numeric_limits::max(); -} - template<> typename AnalyticExpansion::NodePtr AnalyticExpansion::setAnalyticPath( const NodePtr &, @@ -426,10 +356,7 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::setAnalyt template<> typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalyticExpansion( - const NodePtr &, - const NodeVector &, - const NodeVector &, - const CoordinateVector &, + const NodePtr &, const NodePtr &, const NodeGetter &, int &, int &) { diff --git a/nav2_smac_planner/src/node_2d.cpp b/nav2_smac_planner/src/node_2d.cpp index 11fdf8b3b41..bf64ae29e3e 100644 --- a/nav2_smac_planner/src/node_2d.cpp +++ b/nav2_smac_planner/src/node_2d.cpp @@ -85,12 +85,12 @@ float Node2D::getTraversalCost(const NodePtr & child) float Node2D::getHeuristicCost( const Coordinates & node_coords, - const CoordinateVector & goals_coords) + const Coordinates & goal_coordinates) { // Using Moore distance as it more accurately represents the distances // even a Van Neumann neighborhood robot can navigate. - auto dx = goals_coords[0].x - node_coords.x; - auto dy = goals_coords[0].y - node_coords.y; + auto dx = goal_coordinates.x - node_coords.x; + auto dy = goal_coordinates.y - node_coords.y; return std::sqrt(dx * dx + dy * dy); } diff --git a/nav2_smac_planner/src/node_hybrid.cpp b/nav2_smac_planner/src/node_hybrid.cpp index bbbae87ae7c..9d46ab0f0ff 100644 --- a/nav2_smac_planner/src/node_hybrid.cpp +++ b/nav2_smac_planner/src/node_hybrid.cpp @@ -442,18 +442,12 @@ float NodeHybrid::getTraversalCost(const NodePtr & child) float NodeHybrid::getHeuristicCost( const Coordinates & node_coords, - const CoordinateVector & goals_coords) + const Coordinates & goal_coords) { - // obstacle heuristic does not depend on goal heading const float obstacle_heuristic = - getObstacleHeuristic(node_coords, goals_coords[0], motion_table.cost_penalty); - float distance_heuristic = std::numeric_limits::max(); - for (unsigned int i = 0; i < goals_coords.size(); i++) { - distance_heuristic = std::min( - distance_heuristic, - getDistanceHeuristic(node_coords, goals_coords[i], obstacle_heuristic)); - } - return std::max(obstacle_heuristic, distance_heuristic); + getObstacleHeuristic(node_coords, goal_coords, motion_table.cost_penalty); + const float dist_heuristic = getDistanceHeuristic(node_coords, goal_coords, obstacle_heuristic); + return std::max(obstacle_heuristic, dist_heuristic); } void NodeHybrid::initMotionModel( diff --git a/nav2_smac_planner/src/node_lattice.cpp b/nav2_smac_planner/src/node_lattice.cpp index d9a7c8ebfa5..fc03969e978 100644 --- a/nav2_smac_planner/src/node_lattice.cpp +++ b/nav2_smac_planner/src/node_lattice.cpp @@ -345,18 +345,13 @@ float NodeLattice::getTraversalCost(const NodePtr & child) float NodeLattice::getHeuristicCost( const Coordinates & node_coords, - const CoordinateVector & goals_coords) + const Coordinates & goal_coords) { // get obstacle heuristic value - // obstacle heuristic does not depend on goal heading const float obstacle_heuristic = getObstacleHeuristic( - node_coords, goals_coords[0], motion_table.cost_penalty); - float distance_heuristic = std::numeric_limits::max(); - for (unsigned int i = 0; i < goals_coords.size(); i++) { - distance_heuristic = std::min( - distance_heuristic, - getDistanceHeuristic(node_coords, goals_coords[i], obstacle_heuristic)); - } + node_coords, goal_coords, motion_table.cost_penalty); + const float distance_heuristic = + getDistanceHeuristic(node_coords, goal_coords, obstacle_heuristic); return std::max(obstacle_heuristic, distance_heuristic); } @@ -472,7 +467,7 @@ void NodeLattice::precomputeDistanceHeuristic( int dim_3_size_int = static_cast(dim_3_size); // Create a lookup table of Dubin/Reeds-Shepp distances in a window around the goal - // to help drive the search towards admissible approaches. Due to symmetries in the + // to help drive the search towards admissible approaches. Deu to symmetries in the // Heuristic space, we need to only store 2 of the 4 quadrants and simply mirror // around the X axis any relative node lookup. This reduces memory overhead and increases // the size of a window a platform can store in memory. diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index ffe69aab305..8830f021d1d 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -171,24 +171,7 @@ void SmacPlannerHybrid::configure( nav2_util::declare_parameter_if_not_declared( node, "service_introspection_mode", rclcpp::ParameterValue("disabled")); - std::string goal_heading_type; - nav2_util::declare_parameter_if_not_declared( - node, name + ".goal_heading_mode", rclcpp::ParameterValue("DEFAULT")); - node->get_parameter(name + ".goal_heading_mode", goal_heading_type); - _goal_heading_mode = fromStringToGH(goal_heading_type); - - nav2_util::declare_parameter_if_not_declared( - node, name + ".coarse_search_resolution", rclcpp::ParameterValue(1)); - node->get_parameter(name + ".coarse_search_resolution", _coarse_search_resolution); - - if (_goal_heading_mode == GoalHeadingMode::UNKNOWN) { - std::string error_msg = "Unable to get GoalHeader type. Given '" + goal_heading_type + "' " - "Valid options are DEFAULT, BIDIRECTIONAL, ALL_DIRECTION. "; - throw nav2_core::PlannerException(error_msg); - } - _motion_model = fromString(_motion_model_for_search); - if (_motion_model == MotionModel::UNKNOWN) { RCLCPP_WARN( _logger, @@ -211,21 +194,6 @@ void SmacPlannerHybrid::configure( _max_iterations = std::numeric_limits::max(); } - if (_coarse_search_resolution <= 0) { - RCLCPP_WARN( - _logger, "coarse iteration resolution selected as <= 0, " - "disabling coarse iteration resolution search for goal heading" - ); - - _coarse_search_resolution = 1; - } - - if (_angle_quantizations % _coarse_search_resolution != 0) { - std::string error_msg = "coarse iteration should be an increment" - " of the number of angular bins configured"; - throw nav2_core::PlannerException(error_msg); - } - if (_minimum_turning_radius_global_coords < _costmap->getResolution() * _downsampling_factor) { RCLCPP_WARN( _logger, "Min turning radius cannot be less than the search grid cell resolution!"); @@ -445,8 +413,7 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( if (orientation_bin >= static_cast(_angle_quantizations)) { orientation_bin -= static_cast(_angle_quantizations); } - _a_star->setGoal(mx_goal, my_goal, static_cast(orientation_bin), - _goal_heading_mode, _coarse_search_resolution); + _a_star->setGoal(mx_goal, my_goal, static_cast(orientation_bin)); // Setup message nav_msgs::msg::Path plan; @@ -733,31 +700,6 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para int angle_quantizations = parameter.as_int(); _angle_bin_size = 2.0 * M_PI / angle_quantizations; _angle_quantizations = static_cast(angle_quantizations); - - if (_angle_quantizations % _coarse_search_resolution != 0) { - RCLCPP_WARN( - _logger, "coarse iteration should be an increment of the " - "number of angular bins configured. Disabling course research!" - ); - _coarse_search_resolution = 1; - } - } else if (param_name == _name + ".coarse_search_resolution") { - _coarse_search_resolution = parameter.as_int(); - if (_coarse_search_resolution <= 0) { - RCLCPP_WARN( - _logger, "coarse iteration resolution selected as <= 0. " - "Disabling course research!" - ); - _coarse_search_resolution = 1; - } - if (_angle_quantizations % _coarse_search_resolution != 0) { - RCLCPP_WARN( - _logger, - "coarse iteration should be an increment of the " - "number of angular bins configured. Disabling course research!" - ); - _coarse_search_resolution = 1; - } } } else if (param_type == ParameterType::PARAMETER_STRING) { if (param_name == _name + ".motion_model_for_search") { @@ -770,22 +712,6 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para "valid options are MOORE, VON_NEUMANN, DUBIN, REEDS_SHEPP.", _motion_model_for_search.c_str()); } - } else if (param_name == _name + ".goal_heading_mode") { - std::string goal_heading_type = parameter.as_string(); - GoalHeadingMode goal_heading_mode = fromStringToGH(goal_heading_type); - RCLCPP_INFO( - _logger, - "GoalHeadingMode type set to '%s'.", - goal_heading_type.c_str()); - if (goal_heading_mode == GoalHeadingMode::UNKNOWN) { - RCLCPP_WARN( - _logger, - "Unable to get GoalHeader type. Given '%s', " - "Valid options are DEFAULT, BIDIRECTIONAL, ALL_DIRECTION. ", - goal_heading_type.c_str()); - } else { - _goal_heading_mode = goal_heading_mode; - } } } } diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp index 759d6b34768..22723d4eda1 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -140,22 +140,6 @@ void SmacPlannerLattice::configure( node, name + ".debug_visualizations", rclcpp::ParameterValue(false)); node->get_parameter(name + ".debug_visualizations", _debug_visualizations); - std::string goal_heading_type; - nav2_util::declare_parameter_if_not_declared( - node, name + ".goal_heading_mode", rclcpp::ParameterValue("DEFAULT")); - node->get_parameter(name + ".goal_heading_mode", goal_heading_type); - _goal_heading_mode = fromStringToGH(goal_heading_type); - - nav2_util::declare_parameter_if_not_declared( - node, name + ".coarse_search_resolution", rclcpp::ParameterValue(1)); - node->get_parameter(name + ".coarse_search_resolution", _coarse_search_resolution); - - if (_goal_heading_mode == GoalHeadingMode::UNKNOWN) { - std::string error_msg = "Unable to get GoalHeader type. Given '" + goal_heading_type + "' " - "Valid options are DEFAULT, BIDIRECTIONAL, ALL_DIRECTION. "; - throw nav2_core::PlannerException(error_msg); - } - _metadata = LatticeMotionTable::getLatticeMetadata(_search_info.lattice_filepath); _search_info.minimum_turning_radius = _metadata.min_turning_radius / (_costmap->getResolution()); @@ -175,20 +159,6 @@ void SmacPlannerLattice::configure( _max_iterations = std::numeric_limits::max(); } - if (_coarse_search_resolution <= 0) { - RCLCPP_WARN( - _logger, "coarse iteration resolution selected as <= 0, " - "disabling coarse iteration resolution search for goal heading" - ); - _coarse_search_resolution = 1; - } - - if (_metadata.number_of_headings % _coarse_search_resolution != 0) { - std::string error_msg = "coarse iteration should be an increment of" - " the number of angular bins configured"; - throw nav2_core::PlannerException(error_msg); - } - float lookup_table_dim = static_cast(_lookup_table_size) / static_cast(_costmap->getResolution()); @@ -355,8 +325,7 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( } _a_star->setGoal( mx_goal, my_goal, - NodeLattice::motion_table.getClosestAngularBin(tf2::getYaw(goal.pose.orientation)), - _goal_heading_mode, _coarse_search_resolution); + NodeLattice::motion_table.getClosestAngularBin(tf2::getYaw(goal.pose.orientation))); // Setup message nav_msgs::msg::Path plan; @@ -624,23 +593,6 @@ SmacPlannerLattice::dynamicParametersCallback(std::vector par } else if (param_name == _name + ".terminal_checking_interval") { reinit_a_star = true; _terminal_checking_interval = parameter.as_int(); - } else if (param_name == _name + ".coarse_search_resolution") { - _coarse_search_resolution = parameter.as_int(); - if (_coarse_search_resolution <= 0) { - RCLCPP_WARN( - _logger, "coarse iteration resolution selected as <= 0. " - "Disabling course research!" - ); - _coarse_search_resolution = 1; - } - if (_metadata.number_of_headings % _coarse_search_resolution != 0) { - RCLCPP_WARN( - _logger, - "coarse iteration should be an increment of the number<" - " of angular bins configured. Disabling course research!" - ); - _coarse_search_resolution = 1; - } } } else if (param_type == ParameterType::PARAMETER_STRING) { if (param_name == _name + ".lattice_filepath") { @@ -652,29 +604,6 @@ SmacPlannerLattice::dynamicParametersCallback(std::vector par _metadata = LatticeMotionTable::getLatticeMetadata(_search_info.lattice_filepath); _search_info.minimum_turning_radius = _metadata.min_turning_radius / (_costmap->getResolution()); - if (_metadata.number_of_headings % _coarse_search_resolution != 0) { - RCLCPP_WARN( - _logger, "coarse iteration should be an increment of the number " - "of angular bins configured. Disabling course research!" - ); - _coarse_search_resolution = 1; - } - } else if (param_name == _name + ".goal_heading_mode") { - std::string goal_heading_type = parameter.as_string(); - GoalHeadingMode goal_heading_mode = fromStringToGH(goal_heading_type); - RCLCPP_INFO( - _logger, - "GoalHeadingMode type set to '%s'.", - goal_heading_type.c_str()); - if (goal_heading_mode == GoalHeadingMode::UNKNOWN) { - RCLCPP_WARN( - _logger, - "Unable to get GoalHeader type. Given '%s', " - "Valid options are DEFAULT, BIDIRECTIONAL, ALL_DIRECTION. ", - goal_heading_type.c_str()); - } else { - _goal_heading_mode = goal_heading_mode; - } } } } diff --git a/nav2_smac_planner/test/CMakeLists.txt b/nav2_smac_planner/test/CMakeLists.txt index 24c86d9c24f..fc4abf20c75 100644 --- a/nav2_smac_planner/test/CMakeLists.txt +++ b/nav2_smac_planner/test/CMakeLists.txt @@ -135,13 +135,3 @@ target_link_libraries(test_lattice_node rclcpp::rclcpp rclcpp_lifecycle::rclcpp_lifecycle ) - - -# Test Goal Manager -ament_add_gtest(test_goal_manager test_goal_manager.cpp) -target_link_libraries(test_goal_manager - ${library_name} - nav2_costmap_2d::nav2_costmap_2d_core - rclcpp::rclcpp - rclcpp_lifecycle::rclcpp_lifecycle -) diff --git a/nav2_smac_planner/test/test_a_star.cpp b/nav2_smac_planner/test/test_a_star.cpp index 7338f4e43c0..53f9302b6f4 100644 --- a/nav2_smac_planner/test/test_a_star.cpp +++ b/nav2_smac_planner/test/test_a_star.cpp @@ -123,30 +123,13 @@ TEST(AStarTest, test_a_star_2d) } EXPECT_TRUE(a_star_2.getStart() != nullptr); - EXPECT_NE(a_star_2.getGoalManager().getGoalsState().size(), 0); + EXPECT_TRUE(a_star_2.getGoal() != nullptr); EXPECT_EQ(a_star_2.getSizeX(), 100u); EXPECT_EQ(a_star_2.getSizeY(), 100u); EXPECT_EQ(a_star_2.getSizeDim3(), 1u); EXPECT_EQ(a_star_2.getToleranceHeuristic(), 20.0); EXPECT_EQ(a_star_2.getOnApproachMaxIterations(), 10); - // test unused functions - nav2_smac_planner::AnalyticExpansion expander( - nav2_smac_planner::MotionModel::TWOD, info, false, 1); - - auto analytic_expansion_nodes = - nav2_smac_planner::AnalyticExpansion::AnalyticExpansionNodes(); - EXPECT_EQ(expander.setAnalyticPath(nullptr, nullptr, analytic_expansion_nodes), nullptr); - int dummy_int1 = 0; - int dummy_int2 = 0; - EXPECT_EQ(expander.tryAnalyticExpansion(nullptr, {}, {}, {}, - nullptr, dummy_int1, dummy_int2), nullptr); - EXPECT_EQ(expander.refineAnalyticPath(nullptr, nullptr, nullptr, - analytic_expansion_nodes), std::numeric_limits::max()); - nav2_smac_planner::AnalyticExpansion::AnalyticExpansionNodes - expected_nodes = expander.getAnalyticPath(nullptr, nullptr, nullptr, nullptr); - EXPECT_EQ(expected_nodes.size(), 0); - delete costmapA; } @@ -408,17 +391,10 @@ TEST(AStarTest, test_se2_single_pose_path) }; // functional case testing a_star.setCollisionChecker(checker.get()); - nav2_smac_planner::NodeHybrid::CoordinateVector path; - - // test with no goals set nor start - EXPECT_THROW( - a_star.createPath( - path, num_it, tolerance, - dummy_cancel_checker), std::runtime_error); - a_star.setStart(10u, 10u, 0u); // Goal is one costmap cell away a_star.setGoal(12u, 10u, 0u); + nav2_smac_planner::NodeHybrid::CoordinateVector path; EXPECT_TRUE(a_star.createPath(path, num_it, tolerance, dummy_cancel_checker)); // Check that the path is length one @@ -430,102 +406,6 @@ TEST(AStarTest, test_se2_single_pose_path) nav2_smac_planner::NodeHybrid::destroyStaticAssets(); } -TEST(AStarTest, test_goal_heading_mode) -{ - auto lnode = std::make_shared("test"); - nav2_smac_planner::SearchInfo info; - info.change_penalty = 0.1; - info.non_straight_penalty = 1.1; - info.reverse_penalty = 2.0; - info.minimum_turning_radius = 8; // in grid coordinates - info.retrospective_penalty = 0.015; - info.analytic_expansion_max_length = 20.0; // in grid coordinates - info.analytic_expansion_ratio = 3.5; - unsigned int size_theta = 72; - info.cost_penalty = 1.7; - nav2_smac_planner::AStarAlgorithm a_star( - nav2_smac_planner::MotionModel::DUBIN, info); - int max_iterations = 10000; - float tolerance = 10.0; - int it_on_approach = 10; - int terminal_checking_interval = 5000; - double max_planning_time = 120.0; - int num_it = 0; - - // BIDIRECTIONAL goal heading mode - a_star.initialize( - false, max_iterations, it_on_approach, terminal_checking_interval, - max_planning_time, 401, size_theta); - - nav2_costmap_2d::Costmap2D * costmapA = - new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0.0, 0.0, 0); - - // Convert raw costmap into a costmap ros object - auto costmap_ros = std::make_shared(); - costmap_ros->on_configure(rclcpp_lifecycle::State()); - auto costmap = costmap_ros->getCostmap(); - *costmap = *costmapA; - - std::unique_ptr checker = - std::make_unique(costmap_ros, size_theta, lnode); - checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); - - a_star.setCollisionChecker(checker.get()); - - EXPECT_THROW( - a_star.setGoal( - 80u, 80u, 40u, - nav2_smac_planner::GoalHeadingMode::BIDIRECTIONAL), - std::runtime_error); - a_star.setStart(10u, 10u, 0u); - a_star.setGoal(80u, 80u, 40u, nav2_smac_planner::GoalHeadingMode::BIDIRECTIONAL); - nav2_smac_planner::NodeHybrid::CoordinateVector path; - std::unique_ptr>> expansions = nullptr; - expansions = std::make_unique>>(); - - auto dummy_cancel_checker = []() { - return false; - }; - EXPECT_TRUE(a_star.createPath(path, num_it, tolerance, dummy_cancel_checker, expansions.get())); - EXPECT_EQ(a_star.getGoalManager().getGoalsState().size(), 2); - EXPECT_EQ(a_star.getGoalManager().getGoalsState().size(), - a_star.getGoalManager().getGoalsCoordinates().size()); - - - // ALL_DIRECTION goal heading mode - unsigned int coarse_search_resolution = 16; - a_star.setCollisionChecker(checker.get()); - a_star.setStart(10u, 10u, 0u); - a_star.setGoal(80u, 80u, 40u, nav2_smac_planner::GoalHeadingMode::ALL_DIRECTION, - coarse_search_resolution); - EXPECT_TRUE(a_star.getCoarseSearchResolution() == coarse_search_resolution); - - unsigned int num_bins = nav2_smac_planner::NodeHybrid::motion_table.num_angle_quantization; - - // get number of valid goal states - unsigned int num_valid_goals = 0; - auto goals_state = a_star.getGoalManager().getGoalsState(); - for (unsigned int i = 0; i < goals_state.size(); i++) { - if(goals_state[i].is_valid) { - num_valid_goals++; - } - } - EXPECT_TRUE(a_star.getGoalManager().getGoalsState().size() == num_bins); - EXPECT_TRUE(a_star.getGoalManager().getGoalsState().size() == num_valid_goals); - EXPECT_TRUE(a_star.createPath(path, num_it, tolerance, dummy_cancel_checker, expansions.get())); - EXPECT_TRUE(a_star.getGoalManager().getGoalsState().size() == - a_star.getGoalManager().getGoalsCoordinates().size()); - - // UNKNOWN goal heading mode - a_star.setCollisionChecker(checker.get()); - a_star.setStart(10u, 10u, 0u); - - EXPECT_THROW( - a_star.setGoal( - 80u, 80u, 10u, - nav2_smac_planner::GoalHeadingMode::UNKNOWN), std::runtime_error); -} - TEST(AStarTest, test_constants) { nav2_smac_planner::MotionModel mm = nav2_smac_planner::MotionModel::UNKNOWN; // unknown @@ -537,15 +417,6 @@ TEST(AStarTest, test_constants) mm = nav2_smac_planner::MotionModel::REEDS_SHEPP; // reeds-shepp EXPECT_EQ(nav2_smac_planner::toString(mm), std::string("Reeds-Shepp")); - nav2_smac_planner::GoalHeadingMode gh = nav2_smac_planner::GoalHeadingMode::UNKNOWN; - EXPECT_EQ(nav2_smac_planner::toString(gh), std::string("Unknown")); - gh = nav2_smac_planner::GoalHeadingMode::DEFAULT; // default - EXPECT_EQ(nav2_smac_planner::toString(gh), std::string("DEFAULT")); - gh = nav2_smac_planner::GoalHeadingMode::BIDIRECTIONAL; // bidirectional - EXPECT_EQ(nav2_smac_planner::toString(gh), std::string("BIDIRECTIONAL")); - gh = nav2_smac_planner::GoalHeadingMode::ALL_DIRECTION; // all_direction - EXPECT_EQ(nav2_smac_planner::toString(gh), std::string("ALL_DIRECTION")); - EXPECT_EQ( nav2_smac_planner::fromString( "2D"), nav2_smac_planner::MotionModel::TWOD); @@ -554,18 +425,6 @@ TEST(AStarTest, test_constants) nav2_smac_planner::fromString( "REEDS_SHEPP"), nav2_smac_planner::MotionModel::REEDS_SHEPP); EXPECT_EQ(nav2_smac_planner::fromString("NONE"), nav2_smac_planner::MotionModel::UNKNOWN); - - EXPECT_EQ( - nav2_smac_planner::fromStringToGH( - "DEFAULT"), nav2_smac_planner::GoalHeadingMode::DEFAULT); - EXPECT_EQ( - nav2_smac_planner::fromStringToGH( - "BIDIRECTIONAL"), nav2_smac_planner::GoalHeadingMode::BIDIRECTIONAL); - EXPECT_EQ( - nav2_smac_planner::fromStringToGH( - "ALL_DIRECTION"), nav2_smac_planner::GoalHeadingMode::ALL_DIRECTION); - EXPECT_EQ( - nav2_smac_planner::fromStringToGH("NONE"), nav2_smac_planner::GoalHeadingMode::UNKNOWN); } int main(int argc, char **argv) diff --git a/nav2_smac_planner/test/test_goal_manager.cpp b/nav2_smac_planner/test/test_goal_manager.cpp deleted file mode 100644 index cbd17b87895..00000000000 --- a/nav2_smac_planner/test/test_goal_manager.cpp +++ /dev/null @@ -1,177 +0,0 @@ -// Copyright (c) 2023 Open Navigation LLC -// Copyright (c) 2024 Stevedan Ogochukwu Omodolor Omodia -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include - -#include "gtest/gtest.h" -#include "rclcpp/rclcpp.hpp" -#include "nav2_smac_planner/goal_manager.hpp" -#include "nav2_costmap_2d/costmap_2d.hpp" -#include "nav2_costmap_2d/costmap_subscriber.hpp" -#include "nav2_util/lifecycle_node.hpp" -#include "nav2_smac_planner/collision_checker.hpp" - -using namespace nav2_smac_planner; // NOLINT - -using GoalManagerHybrid = GoalManager; -using NodePtr = NodeHybrid *; -using NodeVector = GoalManagerHybrid::NodeVector; -using CoordinateVector = GoalManagerHybrid::CoordinateVector; -using GoalStateVector = GoalManagerHybrid::GoalStateVector; - -TEST(GoalManagerTest, test_goal_manager) -{ - auto node = std::make_shared("test_node"); - - auto costmapA = new nav2_costmap_2d::Costmap2D(100, 100, 0.1, 0.0, 0.0, 0); - - // Create an island of lethal cost in the middle - for (unsigned int i = 40; i <= 60; ++i) { - for (unsigned int j = 40; j <= 60; ++j) { - costmapA->setCost(i, j, 254); - } - } - - // Convert raw costmap into a costmap ros object - auto costmap_ros = std::make_shared(); - costmap_ros->on_configure(rclcpp_lifecycle::State()); - auto costmap = costmap_ros->getCostmap(); - *costmap = *costmapA; - - auto checker = std::make_unique( - costmap_ros, 72, node); - checker->setFootprint(nav2_costmap_2d::Footprint(), true, 0.0); - - GoalManagerHybrid goal_manager; - float tolerance = 20.0f; - bool allow_unknow = false; - - EXPECT_TRUE(goal_manager.goalsIsEmpty()); - - // Create two valid goals - NodePtr pose_a = new NodeHybrid(48); - NodePtr pose_b = new NodeHybrid(49); - pose_a->setPose(NodeHybrid::Coordinates(0, 0, 0)); - pose_b->setPose(NodeHybrid::Coordinates(0, 0, 10)); - - GoalStateVector goals_state = { - {pose_a, true}, - {pose_b, true} - }; - - goal_manager.setGoalStates(goals_state); - EXPECT_FALSE(goal_manager.goalsIsEmpty()); - EXPECT_EQ(goal_manager.getGoalsState().size(), 2u); - EXPECT_TRUE(goal_manager.getGoalsSet().empty()); - EXPECT_TRUE(goal_manager.getGoalsCoordinates().empty()); - - goal_manager.removeInvalidGoals(tolerance, checker.get(), allow_unknow); - - EXPECT_EQ(goal_manager.getGoalsSet().size(), 2); - EXPECT_EQ(goal_manager.getGoalsCoordinates().size(), 2); - for (const auto & goal_state : goal_manager.getGoalsState()) { - EXPECT_TRUE(goal_state.is_valid); - } - - // Test is goal - EXPECT_TRUE(goal_manager.isGoal(goals_state[0].goal)); - EXPECT_TRUE(goal_manager.isGoal(goals_state[1].goal)); - - // Re-populate and validate reset state - goal_manager.setGoalStates(goals_state); - EXPECT_EQ(goal_manager.getGoalsSet().size(), 0); - EXPECT_EQ(goal_manager.getGoalsCoordinates().size(), 0); - - // Add invalid goal - NodeHybrid pose_c(50); - pose_c.setPose(NodeHybrid::Coordinates(50, 50, 0)); // inside lethal zone - goals_state.push_back({&pose_c, true}); - - goal_manager.setGoalStates(goals_state); - EXPECT_EQ(goal_manager.getGoalsState().size(), 3); - - // Tolerance is high, one goal is invalid - // all goals are valid - goal_manager.removeInvalidGoals(tolerance, checker.get(), allow_unknow); - EXPECT_EQ(goal_manager.getGoalsSet().size(), 3); - EXPECT_EQ(goal_manager.getGoalsCoordinates().size(), 3); - - for (const auto & goal_state : goal_manager.getGoalsState()) { - EXPECT_TRUE(goal_state.is_valid); - } - - // Test with low tolerance, expect invalid goal to be filtered out - goal_manager.setGoalStates(goals_state); - int low_tolerance = 0.0f; - goal_manager.removeInvalidGoals(low_tolerance, checker.get(), allow_unknow); - - EXPECT_EQ(goal_manager.getGoalsSet().size(), 2); - EXPECT_EQ(goal_manager.getGoalsCoordinates().size(), 2); - - // expect last goal to be invalid - for (const auto & goal_state : goal_manager.getGoalsState()) { - if (goal_state.goal == goals_state[2].goal) { - EXPECT_FALSE(goal_state.is_valid); - } else { - EXPECT_TRUE(goal_state.is_valid); - } - } - - // Prepare goals for expansion - GoalStateVector expansion_goals; - unsigned int test_goal_size = 16; - - for (unsigned int i = 0; i < test_goal_size; ++i) { - auto goal = new NodeHybrid(i); - goal->setPose(NodeHybrid::Coordinates(i, i, 0)); - expansion_goals.push_back({goal, true}); - } - - goal_manager.setGoalStates(expansion_goals); - goal_manager.removeInvalidGoals(tolerance, checker.get(), allow_unknow); - - NodeVector coarse_check_goals; - NodeVector fine_check_goals; - - // Resolution 1: everything coarse - goal_manager.prepareGoalsForAnalyticExpansion(coarse_check_goals, fine_check_goals, 1); - EXPECT_EQ(coarse_check_goals.size(), test_goal_size); - EXPECT_TRUE(fine_check_goals.empty()); - - // Resolution 4: every 4th coarse - coarse_check_goals.clear(); - fine_check_goals.clear(); - - goal_manager.prepareGoalsForAnalyticExpansion(coarse_check_goals, fine_check_goals, 4); - EXPECT_EQ(coarse_check_goals.size(), 4u); // indices 0, 4, 8, 12 - EXPECT_EQ(fine_check_goals.size(), 12u); - - delete costmapA; - nav2_smac_planner::NodeHybrid::destroyStaticAssets(); -} - - -int main(int argc, char **argv) -{ - ::testing::InitGoogleTest(&argc, argv); - - rclcpp::init(0, nullptr); - - int result = RUN_ALL_TESTS(); - - rclcpp::shutdown(); - - return result; -} diff --git a/nav2_smac_planner/test/test_node2d.cpp b/nav2_smac_planner/test/test_node2d.cpp index 526a263a526..4e9b39be67b 100644 --- a/nav2_smac_planner/test/test_node2d.cpp +++ b/nav2_smac_planner/test/test_node2d.cpp @@ -67,10 +67,8 @@ TEST(Node2DTest, test_node_2d) // check heuristic cost computation nav2_smac_planner::Node2D::Coordinates A(0.0, 0.0); - nav2_smac_planner::Node2D::CoordinateVector B_vec; nav2_smac_planner::Node2D::Coordinates B(10.0, 5.0); - B_vec.push_back(B); - EXPECT_NEAR(testB.getHeuristicCost(A, B_vec), 11.18, 0.02); + EXPECT_NEAR(testB.getHeuristicCost(A, B), 11.18, 0.02); // check operator== works on index unsigned char costC = '2'; diff --git a/nav2_smac_planner/test/test_smac_hybrid.cpp b/nav2_smac_planner/test/test_smac_hybrid.cpp index 28c7719355a..f269e9599c5 100644 --- a/nav2_smac_planner/test/test_smac_hybrid.cpp +++ b/nav2_smac_planner/test/test_smac_hybrid.cpp @@ -29,36 +29,6 @@ #include "nav2_smac_planner/smac_planner_hybrid.hpp" #include "nav2_smac_planner/smac_planner_2d.hpp" -// Simple wrapper to be able to call a private member -class HybridWrap : public nav2_smac_planner::SmacPlannerHybrid -{ -public: - void callDynamicParams(std::vector parameters) - { - dynamicParametersCallback(parameters); - } - - int getCoarseSearchResolution() - { - return _coarse_search_resolution; - } - - int getMaxIterations() - { - return _max_iterations; - } - - int getMaxOnApproachIterations() - { - return _max_on_approach_iterations; - } - - nav2_smac_planner::GoalHeadingMode getGoalHeadingMode() - { - return _goal_heading_mode; - } -}; - // SMAC smoke tests for plugin-level issues rather than algorithms // (covered by more extensively testing in other files) // System tests in nav2_system_tests will actually plan with this work @@ -89,46 +59,7 @@ TEST(SmacTest, test_smac_se2) goal.pose.position.x = 1.0; goal.pose.position.y = 1.0; goal.pose.orientation.w = 1.0; - auto planner = std::make_unique(); - - // invalid goal heading mode - nodeSE2->declare_parameter("test.goal_heading_mode", std::string("UNKNOWN")); - nodeSE2->set_parameter(rclcpp::Parameter("test.goal_heading_mode", std::string("UNKNOWN"))); - EXPECT_THROW(planner->configure(nodeSE2, "test", nullptr, costmap_ros), std::runtime_error); - nodeSE2->set_parameter(rclcpp::Parameter("test.goal_heading_mode", std::string("DEFAULT"))); - - // Invalid motion model should not change the default value - nodeSE2->set_parameter(rclcpp::Parameter("test.motion_model_for_search", std::string("invalid"))); - EXPECT_THROW(planner->configure(nodeSE2, "test", nullptr, costmap_ros), std::runtime_error); - nodeSE2->set_parameter(rclcpp::Parameter("test.motion_model_for_search", std::string("DUBIN"))); - - // invalid coarse search resolution - nodeSE2->set_parameter(rclcpp::Parameter("test.coarse_search_resolution", -1)); - nodeSE2->set_parameter(rclcpp::Parameter("test.max_on_approach_iterations", -1)); - nodeSE2->set_parameter(rclcpp::Parameter("test.max_iterations", -1)); - EXPECT_NO_THROW(planner->configure(nodeSE2, "test", nullptr, costmap_ros)); - - EXPECT_EQ(planner->getCoarseSearchResolution(), 1); - EXPECT_EQ(planner->getMaxIterations(), std::numeric_limits::max()); - EXPECT_EQ(planner->getMaxOnApproachIterations(), std::numeric_limits::max()); - - - // valid Configuration - nodeSE2->set_parameter(rclcpp::Parameter("test.coarse_search_resolution", 1)); - nodeSE2->set_parameter(rclcpp::Parameter("test.max_on_approach_iterations", 1000)); - nodeSE2->set_parameter(rclcpp::Parameter("test.max_iterations", 1000000)); - EXPECT_NO_THROW(planner->configure(nodeSE2, "test", nullptr, costmap_ros)); - - - // angle_quantizations not multiple of coarse search resolution would trigger a throw - nodeSE2->set_parameter(rclcpp::Parameter("test.angle_quantization_bins", 72)); - nodeSE2->set_parameter(rclcpp::Parameter("test.coarse_search_resolution", 5)); - EXPECT_THROW(planner->configure(nodeSE2, "test", nullptr, costmap_ros), std::runtime_error); - - // valid configuration - nodeSE2->set_parameter(rclcpp::Parameter("test.coarse_search_resolution", 4)); - EXPECT_NO_THROW(planner->configure(nodeSE2, "test", nullptr, costmap_ros)); - + auto planner = std::make_unique(); planner->configure(nodeSE2, "test", nullptr, costmap_ros); planner->activate(); @@ -162,7 +93,7 @@ TEST(SmacTest, test_smac_se2_reconfigure) std::make_shared("global_costmap"); costmap_ros->on_configure(rclcpp_lifecycle::State()); - auto planner = std::make_unique(); + auto planner = std::make_unique(); planner->configure(nodeSE2, "test", nullptr, costmap_ros); planner->activate(); @@ -176,7 +107,7 @@ TEST(SmacTest, test_smac_se2_reconfigure) auto results = rec_param->set_parameters_atomically( {rclcpp::Parameter("test.downsample_costmap", true), rclcpp::Parameter("test.downsampling_factor", 2), - rclcpp::Parameter("test.angle_quantization_bins", 72), + rclcpp::Parameter("test.angle_quantization_bins", 100), rclcpp::Parameter("test.allow_unknown", false), rclcpp::Parameter("test.max_iterations", -1), rclcpp::Parameter("test.minimum_turning_radius", 1.0), @@ -194,9 +125,7 @@ TEST(SmacTest, test_smac_se2_reconfigure) rclcpp::Parameter("test.analytic_expansion_max_length", 42.0), rclcpp::Parameter("test.max_on_approach_iterations", 42), rclcpp::Parameter("test.terminal_checking_interval", 42), - rclcpp::Parameter("test.motion_model_for_search", std::string("REEDS_SHEPP")), - rclcpp::Parameter("test.goal_heading_mode", std::string("BIDIRECTIONAL")), - rclcpp::Parameter("test.coarse_search_resolution", -1)}); + rclcpp::Parameter("test.motion_model_for_search", std::string("REEDS_SHEPP"))}); rclcpp::spin_until_future_complete( nodeSE2->get_node_base_interface(), @@ -204,7 +133,7 @@ TEST(SmacTest, test_smac_se2_reconfigure) EXPECT_EQ(nodeSE2->get_parameter("test.downsample_costmap").as_bool(), true); EXPECT_EQ(nodeSE2->get_parameter("test.downsampling_factor").as_int(), 2); - EXPECT_EQ(nodeSE2->get_parameter("test.angle_quantization_bins").as_int(), 72); + EXPECT_EQ(nodeSE2->get_parameter("test.angle_quantization_bins").as_int(), 100); EXPECT_EQ(nodeSE2->get_parameter("test.allow_unknown").as_bool(), false); EXPECT_EQ(nodeSE2->get_parameter("test.max_iterations").as_int(), -1); EXPECT_EQ(nodeSE2->get_parameter("test.minimum_turning_radius").as_double(), 1.0); @@ -225,9 +154,6 @@ TEST(SmacTest, test_smac_se2_reconfigure) EXPECT_EQ( nodeSE2->get_parameter("test.motion_model_for_search").as_string(), std::string("REEDS_SHEPP")); - EXPECT_EQ( - nodeSE2->get_parameter("test.goal_heading_mode").as_string(), - std::string("BIDIRECTIONAL")); auto results2 = rec_param->set_parameters_atomically( {rclcpp::Parameter("resolution", 0.2)}); @@ -235,30 +161,6 @@ TEST(SmacTest, test_smac_se2_reconfigure) nodeSE2->get_node_base_interface(), results2); EXPECT_EQ(nodeSE2->get_parameter("resolution").as_double(), 0.2); - EXPECT_EQ(nodeSE2->get_parameter("test.coarse_search_resolution").as_int(), -1); - EXPECT_EQ(planner->getCoarseSearchResolution(), 1); - - // test coarse resolution edge cases. Consider when coarse resolution - // is not multiple of angle bin quantization(72) - std::vector parameters; - parameters.push_back(rclcpp::Parameter("test.coarse_search_resolution", 7)); - EXPECT_NO_THROW(planner->callDynamicParams(parameters)); - EXPECT_EQ(planner->getCoarseSearchResolution(), 1); - - // same test as before but the error comes from the angular bin - parameters.clear(); - parameters.push_back(rclcpp::Parameter("test.coarse_search_resolution", 4)); - parameters.push_back(rclcpp::Parameter("test.angle_quantization_bins", 87)); - EXPECT_NO_THROW(planner->callDynamicParams(parameters)); - EXPECT_EQ(planner->getCoarseSearchResolution(), 1); - - // test invalid goal heading mode does not modify current - // goal heading mode - parameters.clear(); - parameters.push_back(rclcpp::Parameter("test.goal_heading_mode", std::string("BIDIRECTIONAL"))); - parameters.push_back(rclcpp::Parameter("test.goal_heading_mode", std::string("invalid"))); - EXPECT_NO_THROW(planner->callDynamicParams(parameters)); - EXPECT_EQ(planner->getGoalHeadingMode(), nav2_smac_planner::GoalHeadingMode::BIDIRECTIONAL); } int main(int argc, char **argv) diff --git a/nav2_smac_planner/test/test_smac_lattice.cpp b/nav2_smac_planner/test/test_smac_lattice.cpp index a1eb918502f..949b48a5305 100644 --- a/nav2_smac_planner/test/test_smac_lattice.cpp +++ b/nav2_smac_planner/test/test_smac_lattice.cpp @@ -36,26 +36,6 @@ class LatticeWrap : public nav2_smac_planner::SmacPlannerLattice { dynamicParametersCallback(parameters); } - - int getCoarseSearchResolution() - { - return _coarse_search_resolution; - } - - int getMaxIterations() - { - return _max_iterations; - } - - int getMaxOnApproachIterations() - { - return _max_on_approach_iterations; - } - - nav2_smac_planner::GoalHeadingMode getGoalHeadingMode() - { - return _goal_heading_mode; - } }; // SMAC smoke tests for plugin-level issues rather than algorithms @@ -83,38 +63,10 @@ TEST(SmacTest, test_smac_lattice) goal.pose.position.x = 1.0; goal.pose.position.y = 1.0; goal.pose.orientation.w = 1.0; - auto planner = std::make_unique(); + auto planner = std::make_unique(); try { - // invalid goal heading mode - nodeLattice->declare_parameter("test.goal_heading_mode", std::string("UNKNOWN")); - nodeLattice->set_parameter(rclcpp::Parameter("test.goal_heading_mode", std::string("UNKNOWN"))); - EXPECT_THROW(planner->configure(nodeLattice, "test", nullptr, costmap_ros), std::runtime_error); - nodeLattice->set_parameter(rclcpp::Parameter("test.goal_heading_mode", std::string("DEFAULT"))); - - // invalid Configuration resolution - nodeLattice->set_parameter(rclcpp::Parameter("test.coarse_search_resolution", -1)); - nodeLattice->set_parameter(rclcpp::Parameter("test.max_iterations", -1)); - nodeLattice->set_parameter(rclcpp::Parameter("test.max_on_approach_iterations", -1)); - - EXPECT_NO_THROW(planner->configure(nodeLattice, "test", nullptr, costmap_ros)); - EXPECT_EQ(planner->getCoarseSearchResolution(), 1); - EXPECT_EQ(planner->getMaxIterations(), std::numeric_limits::max()); - EXPECT_EQ(planner->getMaxOnApproachIterations(), std::numeric_limits::max()); - - - // Valid configuration - nodeLattice->set_parameter(rclcpp::Parameter("test.max_iterations", 1000000)); - nodeLattice->set_parameter(rclcpp::Parameter("test.max_on_approach_iterations", 1000)); - - // Coarse search resolution will throw, not multiple of number of heading(16 default) - nodeLattice->set_parameter(rclcpp::Parameter("test.coarse_search_resolution", 3)); - EXPECT_THROW(planner->configure(nodeLattice, "test", nullptr, costmap_ros), std::runtime_error); - - // Valid configuration - nodeLattice->set_parameter(rclcpp::Parameter("test.coarse_search_resolution", 4)); // Expect to throw due to invalid prims file in param planner->configure(nodeLattice, "test", nullptr, costmap_ros); - EXPECT_EQ(planner->getCoarseSearchResolution(), 4); } catch (...) { } planner->activate(); @@ -191,41 +143,9 @@ TEST(SmacTest, test_smac_lattice_reconfigure) results); } catch (...) { } - // test edge cases Goal heading mode, make sure we don't reset the goal when invalid - std::vector parameters; - parameters.push_back(rclcpp::Parameter("test.goal_heading_mode", std::string("BIDIRECTIONAL"))); - parameters.push_back(rclcpp::Parameter("test.goal_heading_mode", std::string("invalid"))); - EXPECT_NO_THROW(planner->callDynamicParams(parameters)); - EXPECT_EQ(planner->getGoalHeadingMode(), nav2_smac_planner::GoalHeadingMode::BIDIRECTIONAL); - - // test coarse resolution edge cases. - // Negative coarse search resolution - parameters.clear(); - parameters.push_back(rclcpp::Parameter("test.coarse_search_resolution", -1)); - EXPECT_NO_THROW(planner->callDynamicParams(parameters)); - EXPECT_EQ(planner->getCoarseSearchResolution(), 1); - - // test value when coarse resolution - // is not multiple number_of_headings - parameters.clear(); - parameters.push_back(rclcpp::Parameter("test.coarse_search_resolution", 5)); - EXPECT_NO_THROW(planner->callDynamicParams(parameters)); - EXPECT_EQ(planner->getCoarseSearchResolution(), 1); - - // Similar modulous test but when the issue is from the number - // of heading, test output includes number of heading 15 - parameters.clear(); - - parameters.push_back(rclcpp::Parameter("test.coarse_search_resolution", 4)); - parameters.push_back(rclcpp::Parameter("test.lattice_filepath", - ament_index_cpp::get_package_share_directory("nav2_smac_planner") + - "/sample_primitives/test/output.json")); - EXPECT_NO_THROW(planner->callDynamicParams(parameters)); - EXPECT_EQ(planner->getCoarseSearchResolution(), 1); - // So instead, let's call manually on a change - parameters.clear(); + std::vector parameters; parameters.push_back(rclcpp::Parameter("test.lattice_filepath", std::string("HI"))); EXPECT_THROW(planner->callDynamicParams(parameters), std::runtime_error); }