From a8ed8e0f3905213a0aa1ae144124adeb9282628f Mon Sep 17 00:00:00 2001 From: Stevedan Ogochukwu Omodolor <61468301+stevedanomodolor@users.noreply.github.com> Date: Sun, 15 Jun 2025 23:54:34 +0200 Subject: [PATCH] Fix/smac planner orientation goals (#5235) * cherry pick Signed-off-by: Stevedan Omodolor * cherry pick 6a74ba61ca0ea117b2995dd0c8e266a5ec9741ba Signed-off-by: Stevedan Omodolor * cherrpy pick Signed-off-by: Stevedan Omodolor * include x11 forwarding Signed-off-by: Stevedan Omodolor * kind of working version Signed-off-by: Stevedan Omodolor * cleanup Signed-off-by: Stevedan Omodolor * formatting Signed-off-by: Stevedan Omodolor * minor format change Signed-off-by: Stevedan Omodolor * change naming Signed-off-by: Stevedan Omodolor * minor changes Signed-off-by: Stevedan Omodolor * working with new changes Signed-off-by: Stevedan Omodolor * Revert "Fix Ci from key signing (#5220)" (#5237) * Revert "Fix Ci from key signing (#5220)" This reverts the changes to the Dockerfile done in 1345c226bcf73068127b2e2efaee91a637d2f351. Signed-off-by: Nils-Christian Iseke * Update Cache Version Signed-off-by: Nils-Christian Iseke --------- Signed-off-by: Nils-Christian Iseke Signed-off-by: Stevedan Omodolor * Revert back Signed-off-by: Stevedan Omodolor * enable_groot_monitoring_ false (#5246) Signed-off-by: Guillaume Doisy Co-authored-by: Guillaume Doisy Signed-off-by: Stevedan Omodolor * Updating readme table for kilted release (#5249) * updating readme table for kilted release Signed-off-by: Steve Macenski * Updating table lint Signed-off-by: Steve Macenski --------- Signed-off-by: Steve Macenski Signed-off-by: Stevedan Omodolor * Add min_distance_to_obstacle parameter to RPP (#4543) * min_distance_to_obstacle Signed-off-by: Guillaume Doisy * suggestion to time base and combine Signed-off-by: Guillaume Doisy * typo Signed-off-by: Guillaume Doisy * use min_approach_linear_velocity Signed-off-by: Guillaume Doisy --------- Signed-off-by: Guillaume Doisy Co-authored-by: Guillaume Doisy Signed-off-by: Stevedan Omodolor * Fixing builds for message filters API change while retaining Jazzy, Kilted, and Rolling support (#5251) * Update amcl_node.hpp Signed-off-by: Steve Macenski * Update amcl_node.cpp Signed-off-by: Steve Macenski * Working for Kilted, Jazzy Signed-off-by: Steve Macenski * Update amcl_node.cpp Signed-off-by: Steve Macenski * Update amcl_node.cpp Signed-off-by: Steve Macenski * Update amcl_node.cpp Signed-off-by: Steve Macenski --------- Signed-off-by: Steve Macenski Signed-off-by: Stevedan Omodolor * Change max_cost default to 254 (#5256) Signed-off-by: Tony Najjar Signed-off-by: Stevedan Omodolor * linter Signed-off-by: Stevedan Omodolor * remove const Signed-off-by: Stevedan Omodolor * pass const pointer by value Signed-off-by: Stevedan Omodolor * pass const pointer by value Signed-off-by: Stevedan Omodolor * remove unused param Signed-off-by: Stevedan Omodolor --------- Signed-off-by: Stevedan Omodolor Signed-off-by: Nils-Christian Iseke Signed-off-by: Guillaume Doisy Signed-off-by: Steve Macenski Signed-off-by: Tony Najjar Co-authored-by: Steve Macenski Co-authored-by: Nils-Christian Iseke <48475933+Nils-ChristianIseke@users.noreply.github.com> Co-authored-by: Guillaume Doisy Co-authored-by: Guillaume Doisy Co-authored-by: Tony Najjar (cherry picked from commit afb9e7d4625cbedf1a0428385f69611581555597) --- .devcontainer/devcontainer.json | 4 + .../include/nav2_smac_planner/a_star.hpp | 46 +- .../nav2_smac_planner/analytic_expansion.hpp | 73 +- .../include/nav2_smac_planner/constants.hpp | 35 + .../nav2_smac_planner/goal_manager.hpp | 218 + .../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 | 11 + .../sample_primitives/test/output.json | 3931 +++++++++++++++++ nav2_smac_planner/src/a_star.cpp | 143 +- nav2_smac_planner/src/analytic_expansion.cpp | 277 +- 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 | 147 +- nav2_smac_planner/test/test_goal_manager.cpp | 186 + 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 +- 25 files changed, 5329 insertions(+), 170 deletions(-) create mode 100644 nav2_smac_planner/include/nav2_smac_planner/goal_manager.hpp create mode 100644 nav2_smac_planner/lattice_primitives/sample_primitives/test/output.json create mode 100644 nav2_smac_planner/test/test_goal_manager.cpp diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index bda8153b7f5..94d35e64055 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -14,6 +14,8 @@ // "--pid=host", // DDS discovery with host, without --network=host // "--privileged", // device access to host peripherals, e.g. USB // "--security-opt=seccomp=unconfined", // enable debugging, e.g. gdb + // "--volume=/tmp/.X11-unix:/tmp/.X11-unix", // X11 socket for GUI applications + // "--gpus=all" // access to all GPUs, e.g. for GPU-accelerated applications ], "workspaceFolder": "/opt/overlay_ws/src/navigation2", "workspaceMount": "source=${localWorkspaceFolder},target=${containerWorkspaceFolder},type=bind", @@ -23,6 +25,8 @@ "remoteEnv": { "OVERLAY_MIXINS": "release ccache lld", "CCACHE_DIR": "/tmp/.ccache" + // "QT_X11_NO_MITSHM": "1", // disable MIT-SHM for X11 forwarding + // "DISPLAY": "${localEnv:DISPLAY}", // X11 forwarding }, "mounts": [ { 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 8b127960cc3..a4720b3c5b2 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp @@ -32,6 +32,7 @@ #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" @@ -54,6 +55,8 @@ class AStarAlgorithm typedef typename NodeT::CoordinateVector CoordinateVector; typedef typename NodeVector::iterator NeighborIterator; typedef std::function NodeGetter; + typedef GoalManager GoalManagerT; + /** * @struct nav2_smac_planner::NodeComparator @@ -90,6 +93,8 @@ 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, @@ -125,11 +130,15 @@ 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 unsigned int & dim_3, + const GoalHeadingMode & goal_heading_mode = GoalHeadingMode::DEFAULT, + const int & coarse_search_resolution = 1); /** * @brief Set the starting pose for planning, as a node index @@ -154,12 +163,6 @@ 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 @@ -190,6 +193,18 @@ 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 @@ -210,13 +225,6 @@ 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 @@ -240,6 +248,11 @@ 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); /** @@ -260,12 +273,11 @@ 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; - NodePtr _goal; - + GoalManagerT _goal_manager; 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 6be84d1c109..0c5cdede0bd 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp @@ -15,6 +15,10 @@ #ifndef NAV2_SMAC_PLANNER__ANALYTIC_EXPANSION_HPP_ #define NAV2_SMAC_PLANNER__ANALYTIC_EXPANSION_HPP_ +#include +#include +#include + #include #include #include @@ -35,8 +39,10 @@ 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 @@ -59,7 +65,33 @@ class AnalyticExpansion Coordinates proposed_coords; }; - typedef std::vector AnalyticExpansionNodes; + /** + * @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}; + }; /** * @brief Constructor for analytic expansion object @@ -79,17 +111,22 @@ class AnalyticExpansion /** * @brief Attempt an analytic path completion * @param node The node to start the analytic path from - * @param goal The goal node to plan to + * @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 getter Gets a node at a set of coordinates * @param iterations Iterations to run over - * @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 + * @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 */ NodePtr tryAnalyticExpansion( const NodePtr & current_node, - const NodePtr & goal_node, - const NodeGetter & getter, int & iterations, int & best_cost); + const NodeVector & coarse_check_goals, + const NodeVector & fine_check_goals, + const CoordinateVector & goals_coords, + const NodeGetter & getter, int & iterations, + int & closest_distance); /** * @brief Perform an analytic path expansion to the goal @@ -103,6 +140,21 @@ 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 node The node to start the analytic path from. Node head may + * change as a result of refinement + * @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 & 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 @@ -114,6 +166,13 @@ 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/include/nav2_smac_planner/constants.hpp b/nav2_smac_planner/include/nav2_smac_planner/constants.hpp index af44ce53659..6344c86fb89 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/constants.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/constants.hpp @@ -28,6 +28,14 @@ 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) { @@ -59,6 +67,33 @@ 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 new file mode 100644 index 00000000000..317a67de933 --- /dev/null +++ b/nav2_smac_planner/include/nav2_smac_planner/goal_manager.hpp @@ -0,0 +1,218 @@ +// 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()), + _ref_goal_coord(Coordinates()) + { + } + + /** + * @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 Adds goal to the goal vector + *@param goal Reference to the NodePtr + */ + void addGoal(NodePtr & goal) + { + _goals_state.push_back({goal, true}); + } + + /** + * @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) + { + // Make sure that there was a goal clear before this was run + if (!_goals_set.empty() || !_goals_coordinate.empty()) { + throw std::runtime_error("Goal set should be cleared before calling " + "removeinvalidgoals"); + } + 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(const 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; + } + + /** + * @brief Set the Reference goal coordinate + * @param coord Coordinates to set as Reference goal + */ + inline void setRefGoalCoordinates(const Coordinates & coord) + { + _ref_goal_coord = coord; + } + + /** + * @brief Checks whether the Reference goal coordinate has changed. + * @param coord Coordinates to compare with the current Reference goal coordinate. + * @return true if the Reference goal coordinate has changed, false otherwise. + */ + inline bool hasGoalChanged(const Coordinates & coord) + { + /** + * Note: This function checks if the goal has changed. This has to be done with + * the coordinates not the Node pointer because the Node pointer + * can be reused for different goals, but the coordinates will always + * be unique for each goal. + */ + return _ref_goal_coord != coord; + } + +protected: + NodeSet _goals_set; + GoalStateVector _goals_state; + CoordinateVector _goals_coordinate; + Coordinates _ref_goal_coord; +}; + +} // 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 1bd0993a8eb..6d54e1696ab 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp @@ -50,6 +50,16 @@ 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; @@ -75,6 +85,15 @@ 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 */ @@ -224,7 +243,7 @@ class Node2D */ static float getHeuristicCost( const Coordinates & node_coords, - const Coordinates & goal_coordinates); + const CoordinateVector & goals_coords); /** * @brief Initialize the neighborhood to be used in A* @@ -264,6 +283,7 @@ 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 ee3a4bf231c..cc3650563a8 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) + inline bool operator==(const Coordinates & rhs) const { return this->x == rhs.x && this->y == rhs.y && this->theta == rhs.theta; } - inline bool operator!=(const Coordinates & rhs) + inline bool operator!=(const Coordinates & rhs) const { return !(*this == rhs); } @@ -374,7 +374,7 @@ class NodeHybrid */ static float getHeuristicCost( const Coordinates & node_coords, - const Coordinates & goal_coordinates); + const CoordinateVector & goals_coords); /** * @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 824b435447d..5b07e5453bf 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 Coordinates & goal_coordinates); + const CoordinateVector & goals_coords); /** * @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 ca88725cc23..0b50c0e15fd 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,6 +123,8 @@ 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 42b30066b8a..8cead0a0145 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,6 +122,8 @@ 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 cc6c9975c5f..f71b4a932f6 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/types.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/types.hpp @@ -188,6 +188,17 @@ 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 new file mode 100644 index 00000000000..feae5b38326 --- /dev/null +++ b/nav2_smac_planner/lattice_primitives/sample_primitives/test/output.json @@ -0,0 +1,3931 @@ +{ + "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 52b94db14af..98d040bd9cc 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -43,9 +43,8 @@ AStarAlgorithm::AStarAlgorithm( _x_size(0), _y_size(0), _search_info(search_info), - _goal_coordinates(Coordinates()), _start(nullptr), - _goal(nullptr), + _goal_manager(GoalManagerT()), _motion_model(motion_model) { _graph.reserve(100000); @@ -192,35 +191,43 @@ template<> void AStarAlgorithm::setGoal( const float & mx, const float & my, - const unsigned int & dim_3) + const unsigned int & dim_3, + const GoalHeadingMode & /*goal_heading_mode*/, + const int & /*coarse_search_resolution*/) { if (dim_3 != 0) { throw std::runtime_error("Node type Node2D cannot be given non-zero goal dim 3."); } - - _goal = addToGraph( + _goal_manager.clear(); + auto goal = addToGraph( Node2D::getIndex( static_cast(mx), static_cast(my), getSizeX())); - _goal_coordinates = Node2D::Coordinates(mx, my); + + goal->setPose(Node2D::Coordinates(mx, my)); + _goal_manager.addGoal(goal); + + _coarse_search_resolution = 1; } template void AStarAlgorithm::setGoal( const float & mx, const float & my, - const unsigned int & dim_3) + const unsigned int & dim_3, + const GoalHeadingMode & goal_heading_mode, + const int & coarse_search_resolution) { - _goal = addToGraph( - NodeT::getIndex( - static_cast(mx), - static_cast(my), - dim_3)); + // Default to minimal resolution unless overridden for ALL_DIRECTION + _coarse_search_resolution = 1; - typename NodeT::Coordinates goal_coords(mx, my, dim_3); + _goal_manager.clear(); + Coordinates ref_goal_coord(mx, my, static_cast(dim_3)); - if (!_search_info.cache_obstacle_heuristic || goal_coords != _goal_coordinates) { + if (!_search_info.cache_obstacle_heuristic || + _goal_manager.hasGoalChanged(ref_goal_coord)) + { if (!_start) { throw std::runtime_error("Start must be set before goal."); } @@ -229,8 +236,66 @@ void AStarAlgorithm::setGoal( _collision_checker->getCostmapROS(), _start->pose.x, _start->pose.y, mx, my); } - _goal_coordinates = goal_coords; - _goal->setPose(_goal_coordinates); + _goal_manager.setRefGoalCoordinates(ref_goal_coord); + + unsigned int num_bins = NodeT::motion_table.num_angle_quantization; + // 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( + static_cast(mx), + static_cast(my), + dim_3)); + goal->setPose(typename NodeT::Coordinates(mx, my, static_cast(dim_3))); + _goal_manager.addGoal(goal); + break; + } + + case GoalHeadingMode::BIDIRECTIONAL: { + // Add two goals, one for each direction + // add goal in original direction + auto goal = addToGraph( + NodeT::getIndex( + static_cast(mx), + static_cast(my), + dim_3)); + goal->setPose(typename NodeT::Coordinates(mx, my, static_cast(dim_3))); + _goal_manager.addGoal(goal); + + // Add goal node in opposite (180°) direction + unsigned int opposite_heading = (dim_3 + (num_bins / 2)) % num_bins; + auto opposite_goal = addToGraph( + NodeT::getIndex( + static_cast(mx), + static_cast(my), + opposite_heading)); + opposite_goal->setPose( + typename NodeT::Coordinates(mx, my, static_cast(opposite_heading))); + _goal_manager.addGoal(opposite_goal); + 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( + static_cast(mx), + static_cast(my), + i)); + goal->setPose(typename NodeT::Coordinates(mx, my, static_cast(i))); + _goal_manager.addGoal(goal); + } + break; + } + case GoalHeadingMode::UNKNOWN: + throw std::runtime_error("Goal heading is UNKNOWN."); + } } template @@ -242,14 +307,15 @@ bool AStarAlgorithm::areInputsValid() } // Check if points were filled in - if (!_start || !_goal) { + if (!_start || _goal_manager.goalsIsEmpty()) { 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 (getToleranceHeuristic() < 0.001 && - !_goal->isNodeValid(_traverse_unknown, _collision_checker)) - { + if (_goal_manager.getGoalsSet().empty()) { throw nav2_core::GoalOccupied("Goal was in lethal cost"); } @@ -273,6 +339,10 @@ 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); @@ -339,13 +409,14 @@ bool AStarAlgorithm::createPath( // 2.1) Use an analytic expansion (if available) to generate a path expansion_result = nullptr; expansion_result = _expander->tryAnalyticExpansion( - current_node, getGoal(), neighborGetter, analytic_iterations, closest_distance); + current_node, coarse_check_goals, fine_check_goals, + _goal_manager.getGoalsCoordinates(), 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 (isGoal(current_node)) { + if (_goal_manager.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 @@ -386,24 +457,12 @@ 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() { @@ -426,9 +485,7 @@ float AStarAlgorithm::getHeuristicCost(const NodePtr & node) { const Coordinates node_coords = NodeT::getCoords(node->getIndex(), getSizeX(), getSizeDim3()); - float heuristic = NodeT::getHeuristicCost( - node_coords, _goal_coordinates); - + float heuristic = NodeT::getHeuristicCost(node_coords, _goal_manager.getGoalsCoordinates()); if (heuristic < _best_heuristic_node.first) { _best_heuristic_node = {heuristic, node->getIndex()}; } @@ -493,6 +550,18 @@ 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 ec0ccc31f02..13df5feb6af 100644 --- a/nav2_smac_planner/src/analytic_expansion.cpp +++ b/nav2_smac_planner/src/analytic_expansion.cpp @@ -12,10 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. Reserved. -#include -#include -#include - #include #include #include @@ -48,7 +44,10 @@ void AnalyticExpansion::setCollisionChecker( template typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalyticExpansion( - const NodePtr & current_node, const NodePtr & goal_node, + const NodePtr & current_node, + const NodeVector & coarse_check_goals, + const NodeVector & fine_check_goals, + const CoordinateVector & goals_coords, const NodeGetter & getter, int & analytic_iterations, int & closest_distance) { @@ -60,10 +59,15 @@ 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; + NodePtr current_best_node = nullptr; + float current_best_score = std::numeric_limits::max(); + closest_distance = std::min( closest_distance, - static_cast(NodeT::getHeuristicCost(node_coords, goal_node->pose))); - + static_cast(NodeT::getHeuristicCost(node_coords, goals_coords))); // 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) @@ -80,81 +84,57 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalytic if (analytic_iterations <= 0) { // Reset the counter and try the analytic path expansion analytic_iterations = desired_iterations; - 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; + 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.nodes.empty()) { + found_valid_expansion = true; + NodePtr node = current_node; + float score = refineAnalyticPath( + 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; } } + } - // 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); + // 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.nodes.empty()) { + NodePtr node = current_node; + float score = refineAnalyticPath( + 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; } - 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.nodes.empty()) { + return setAnalyticPath( + current_best_node, current_best_goal, + current_best_analytic_nodes); + } analytic_iterations--; } @@ -162,6 +142,28 @@ 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, @@ -179,6 +181,12 @@ 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); @@ -195,7 +203,7 @@ typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansion reals; double theta; @@ -230,7 +238,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.emplace_back(next, initial_node_coords, proposed_coordinates); + possible_nodes.add(next, initial_node_coords, proposed_coordinates); node_costs.emplace_back(next->getCost()); prev = next; } else { @@ -280,7 +288,7 @@ typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansionsetPose(node_pose.initial_coords); } @@ -289,9 +297,109 @@ typename AnalyticExpansion::AnalyticExpansionNodes AnalyticExpansion +float AnalyticExpansion::refineAnalyticPath( + NodePtr & node, + const NodePtr & goal_node, + const NodeGetter & getter, + AnalyticExpansionNodes & analytic_nodes) +{ + 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.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 { + 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.nodes.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.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) { + normalized_cost = iter->node->getCost() / 252.0f; + // 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 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); + + // 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) + { + analytic_nodes = refined_analytic_nodes; + best_score = score; + } + } + + return best_score; +} + template typename AnalyticExpansion::NodePtr AnalyticExpansion::setAnalyticPath( const NodePtr & node, @@ -301,7 +409,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) { + for (const auto & node_pose : expanded_nodes.nodes) { auto n = node_pose.node; cleanNode(n); if (n->getIndex() != goal_node->getIndex()) { @@ -345,6 +453,16 @@ getAnalyticPath( return AnalyticExpansionNodes(); } +template<> +float AnalyticExpansion::refineAnalyticPath( + NodePtr &, + const NodePtr &, + const NodeGetter &, + AnalyticExpansionNodes &) +{ + return std::numeric_limits::max(); +} + template<> typename AnalyticExpansion::NodePtr AnalyticExpansion::setAnalyticPath( const NodePtr &, @@ -356,7 +474,10 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::setAnalyt template<> typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalyticExpansion( - const NodePtr &, const NodePtr &, + const NodePtr &, + const NodeVector &, + const NodeVector &, + const CoordinateVector &, const NodeGetter &, int &, int &) { diff --git a/nav2_smac_planner/src/node_2d.cpp b/nav2_smac_planner/src/node_2d.cpp index bf64ae29e3e..11fdf8b3b41 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 Coordinates & goal_coordinates) + const CoordinateVector & goals_coords) { // Using Moore distance as it more accurately represents the distances // even a Van Neumann neighborhood robot can navigate. - auto dx = goal_coordinates.x - node_coords.x; - auto dy = goal_coordinates.y - node_coords.y; + auto dx = goals_coords[0].x - node_coords.x; + auto dy = goals_coords[0].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 9d46ab0f0ff..bbbae87ae7c 100644 --- a/nav2_smac_planner/src/node_hybrid.cpp +++ b/nav2_smac_planner/src/node_hybrid.cpp @@ -442,12 +442,18 @@ float NodeHybrid::getTraversalCost(const NodePtr & child) float NodeHybrid::getHeuristicCost( const Coordinates & node_coords, - const Coordinates & goal_coords) + const CoordinateVector & goals_coords) { + // obstacle heuristic does not depend on goal heading const float obstacle_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); + 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); } void NodeHybrid::initMotionModel( diff --git a/nav2_smac_planner/src/node_lattice.cpp b/nav2_smac_planner/src/node_lattice.cpp index fc03969e978..d9a7c8ebfa5 100644 --- a/nav2_smac_planner/src/node_lattice.cpp +++ b/nav2_smac_planner/src/node_lattice.cpp @@ -345,13 +345,18 @@ float NodeLattice::getTraversalCost(const NodePtr & child) float NodeLattice::getHeuristicCost( const Coordinates & node_coords, - const Coordinates & goal_coords) + const CoordinateVector & goals_coords) { // get obstacle heuristic value + // obstacle heuristic does not depend on goal heading const float obstacle_heuristic = getObstacleHeuristic( - node_coords, goal_coords, motion_table.cost_penalty); - const float distance_heuristic = - getDistanceHeuristic(node_coords, goal_coords, obstacle_heuristic); + 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); } @@ -467,7 +472,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. Deu to symmetries in the + // to help drive the search towards admissible approaches. Due 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 8830f021d1d..ffe69aab305 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -171,7 +171,24 @@ 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, @@ -194,6 +211,21 @@ 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!"); @@ -413,7 +445,8 @@ 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)); + _a_star->setGoal(mx_goal, my_goal, static_cast(orientation_bin), + _goal_heading_mode, _coarse_search_resolution); // Setup message nav_msgs::msg::Path plan; @@ -700,6 +733,31 @@ 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") { @@ -712,6 +770,22 @@ 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 22723d4eda1..759d6b34768 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -140,6 +140,22 @@ 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()); @@ -159,6 +175,20 @@ 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()); @@ -325,7 +355,8 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( } _a_star->setGoal( mx_goal, my_goal, - NodeLattice::motion_table.getClosestAngularBin(tf2::getYaw(goal.pose.orientation))); + NodeLattice::motion_table.getClosestAngularBin(tf2::getYaw(goal.pose.orientation)), + _goal_heading_mode, _coarse_search_resolution); // Setup message nav_msgs::msg::Path plan; @@ -593,6 +624,23 @@ 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") { @@ -604,6 +652,29 @@ 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 fc4abf20c75..24c86d9c24f 100644 --- a/nav2_smac_planner/test/CMakeLists.txt +++ b/nav2_smac_planner/test/CMakeLists.txt @@ -135,3 +135,13 @@ 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 53f9302b6f4..3c9bd9269bd 100644 --- a/nav2_smac_planner/test/test_a_star.cpp +++ b/nav2_smac_planner/test/test_a_star.cpp @@ -123,13 +123,32 @@ TEST(AStarTest, test_a_star_2d) } EXPECT_TRUE(a_star_2.getStart() != nullptr); - EXPECT_TRUE(a_star_2.getGoal() != nullptr); + EXPECT_NE(a_star_2.getGoalManager().getGoalsState().size(), 0); 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); + + nav2_smac_planner::Node2D * start = nullptr; + EXPECT_EQ(expander.refineAnalyticPath(start, 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.nodes.size(), 0); + delete costmapA; } @@ -391,10 +410,17 @@ 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 @@ -406,6 +432,102 @@ 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 @@ -417,6 +539,15 @@ 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); @@ -425,6 +556,18 @@ 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 new file mode 100644 index 00000000000..80f71b3a688 --- /dev/null +++ b/nav2_smac_planner/test/test_goal_manager.cpp @@ -0,0 +1,186 @@ +// 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 NodeT = NodeHybrid; +using NodePtr = NodeT *; +using GoalManagerHybrid = GoalManager; +using NodeVector = GoalManagerHybrid::NodeVector; +using CoordinateVector = GoalManagerHybrid::CoordinateVector; + +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)); + + goal_manager.addGoal(pose_a); + goal_manager.addGoal(pose_b); + + 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(pose_a)); + EXPECT_TRUE(goal_manager.isGoal(pose_b)); + + // Re-populate and validate reset state + goal_manager.clear(); + goal_manager.addGoal(pose_a); + goal_manager.addGoal(pose_b); + EXPECT_EQ(goal_manager.getGoalsSet().size(), 0); + EXPECT_EQ(goal_manager.getGoalsCoordinates().size(), 0); + + // Add invalid goal + NodePtr pose_c = new NodeHybrid(50); + pose_c->setPose(NodeHybrid::Coordinates(50, 50, 0)); // inside lethal zone + + goal_manager.addGoal(pose_c); + 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.clear(); + goal_manager.addGoal(pose_a); + goal_manager.addGoal(pose_b); + goal_manager.addGoal(pose_c); + + 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 == pose_c) { + EXPECT_FALSE(goal_state.is_valid); + } else { + EXPECT_TRUE(goal_state.is_valid); + } + } + + // Prepare goals for expansion + goal_manager.clear(); + unsigned int test_goal_size = 16; + + for (unsigned int i = 0; i < test_goal_size; ++i) { + NodePtr goal = new NodeHybrid(i); + goal->setPose(NodeHybrid::Coordinates(i, i, 0)); + goal_manager.addGoal(goal); + } + + 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); + + + // expect throw if we try to call removeinvalidgoals twice + // without clearing the goal manager first + EXPECT_THROW(goal_manager.removeInvalidGoals( + tolerance, checker.get(), allow_unknow), + std::runtime_error + ); + + 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 4e9b39be67b..526a263a526 100644 --- a/nav2_smac_planner/test/test_node2d.cpp +++ b/nav2_smac_planner/test/test_node2d.cpp @@ -67,8 +67,10 @@ 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); - EXPECT_NEAR(testB.getHeuristicCost(A, B), 11.18, 0.02); + B_vec.push_back(B); + EXPECT_NEAR(testB.getHeuristicCost(A, B_vec), 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 f269e9599c5..28c7719355a 100644 --- a/nav2_smac_planner/test/test_smac_hybrid.cpp +++ b/nav2_smac_planner/test/test_smac_hybrid.cpp @@ -29,6 +29,36 @@ #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 @@ -59,7 +89,46 @@ 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(); + 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)); + planner->configure(nodeSE2, "test", nullptr, costmap_ros); planner->activate(); @@ -93,7 +162,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(); @@ -107,7 +176,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", 100), + rclcpp::Parameter("test.angle_quantization_bins", 72), rclcpp::Parameter("test.allow_unknown", false), rclcpp::Parameter("test.max_iterations", -1), rclcpp::Parameter("test.minimum_turning_radius", 1.0), @@ -125,7 +194,9 @@ 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.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::spin_until_future_complete( nodeSE2->get_node_base_interface(), @@ -133,7 +204,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(), 100); + EXPECT_EQ(nodeSE2->get_parameter("test.angle_quantization_bins").as_int(), 72); 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); @@ -154,6 +225,9 @@ 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)}); @@ -161,6 +235,30 @@ 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 949b48a5305..a1eb918502f 100644 --- a/nav2_smac_planner/test/test_smac_lattice.cpp +++ b/nav2_smac_planner/test/test_smac_lattice.cpp @@ -36,6 +36,26 @@ 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 @@ -63,10 +83,38 @@ 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(); @@ -143,9 +191,41 @@ 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 - std::vector parameters; + parameters.clear(); parameters.push_back(rclcpp::Parameter("test.lattice_filepath", std::string("HI"))); EXPECT_THROW(planner->callDynamicParams(parameters), std::runtime_error); }