From e01a99cf27f570d2032e8231ee809acc9e450e78 Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Tue, 16 May 2023 16:02:30 +0100 Subject: [PATCH 1/4] viz_expansions --- .../include/nav2_smac_planner/a_star.hpp | 5 ++- .../nav2_smac_planner/smac_planner_hybrid.hpp | 4 +++ nav2_smac_planner/src/a_star.cpp | 14 +++++++- nav2_smac_planner/src/smac_planner_hybrid.cpp | 32 +++++++++++++++++++ 4 files changed, 53 insertions(+), 2 deletions(-) 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 01aeca975d..6adb08ff04 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp @@ -104,9 +104,12 @@ class AStarAlgorithm * @param path Reference to a vector of indicies of generated path * @param num_iterations Reference to number of iterations to create plan * @param tolerance Reference to tolerance in costmap nodes + * @param expansions_log Optional expansions logged for debug * @return if plan was successful */ - bool createPath(CoordinateVector & path, int & num_iterations, const float & tolerance); + bool createPath( + CoordinateVector & path, int & num_iterations, const float & tolerance, + std::shared_ptr>> expansions_log = nullptr); /** * @brief Sets the collision checker to use 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 c782fe41e0..f944e3dac1 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 @@ -29,6 +29,7 @@ #include "nav2_costmap_2d/costmap_2d_ros.hpp" #include "nav2_costmap_2d/costmap_2d.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/pose_array.hpp" #include "nav2_util/lifecycle_node.hpp" #include "nav2_util/node_utils.hpp" #include "tf2/utils.h" @@ -116,9 +117,12 @@ class SmacPlannerHybrid : public nav2_core::GlobalPlanner double _max_planning_time; double _lookup_table_size; double _minimum_turning_radius_global_coords; + bool _viz_expansions; std::string _motion_model_for_search; MotionModel _motion_model; rclcpp_lifecycle::LifecyclePublisher::SharedPtr _raw_plan_publisher; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr + _expansions_publisher; std::mutex _mutex; rclcpp_lifecycle::LifecycleNode::WeakPtr _node; diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index 841b43e357..eb26f3a9ca 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -221,7 +221,8 @@ bool AStarAlgorithm::areInputsValid() template bool AStarAlgorithm::createPath( CoordinateVector & path, int & iterations, - const float & tolerance) + const float & tolerance, + std::shared_ptr>> expansions_log) { steady_clock::time_point start_time = steady_clock::now(); _tolerance = tolerance; @@ -273,6 +274,17 @@ bool AStarAlgorithm::createPath( // 1) Pick Nbest from O s.t. min(f(Nbest)), remove from queue current_node = getNextNode(); + // Save current node coordinates for debug + if (expansions_log) { + Coordinates coords = current_node->getCoords( + current_node->getIndex(), getSizeX(), getSizeDim3()); + expansions_log->push_back( + std::make_tuple( + _costmap->getOriginX() + (coords.x * _costmap->getResolution()), + _costmap->getOriginY() + (coords.y * _costmap->getResolution())) + ); + } + // We allow for nodes to be queued multiple times in case // shorter paths result in it, but we can visit only once if (current_node->wasVisited()) { diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index 2d0351c5b9..abc67eb55e 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -133,6 +133,10 @@ void SmacPlannerHybrid::configure( node, name + ".lookup_table_size", rclcpp::ParameterValue(20.0)); node->get_parameter(name + ".lookup_table_size", _lookup_table_size); + nav2_util::declare_parameter_if_not_declared( + node, name + ".viz_expansions", rclcpp::ParameterValue(false)); + node->get_parameter(name + ".viz_expansions", _viz_expansions); + nav2_util::declare_parameter_if_not_declared( node, name + ".motion_model_for_search", rclcpp::ParameterValue(std::string("DUBIN"))); node->get_parameter(name + ".motion_model_for_search", _motion_model_for_search); @@ -215,6 +219,9 @@ void SmacPlannerHybrid::configure( } _raw_plan_publisher = node->create_publisher("unsmoothed_plan", 1); + if (_viz_expansions) { + _expansions_publisher = node->create_publisher("expansions", 1); + } RCLCPP_INFO( _logger, "Configured plugin %s of type SmacPlannerHybrid with " @@ -231,6 +238,9 @@ void SmacPlannerHybrid::activate() _logger, "Activating plugin %s of type SmacPlannerHybrid", _name.c_str()); _raw_plan_publisher->on_activate(); + if (_viz_expansions) { + _expansions_publisher->on_activate(); + } if (_costmap_downsampler) { _costmap_downsampler->on_activate(); } @@ -246,6 +256,9 @@ void SmacPlannerHybrid::deactivate() _logger, "Deactivating plugin %s of type SmacPlannerHybrid", _name.c_str()); _raw_plan_publisher->on_deactivate(); + if (_viz_expansions) { + _expansions_publisher->on_deactivate(); + } if (_costmap_downsampler) { _costmap_downsampler->on_deactivate(); } @@ -264,6 +277,7 @@ void SmacPlannerHybrid::cleanup() _costmap_downsampler.reset(); } _raw_plan_publisher.reset(); + _expansions_publisher.reset(); } nav_msgs::msg::Path SmacPlannerHybrid::createPlan( @@ -337,6 +351,10 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( NodeHybrid::CoordinateVector path; int num_iterations = 0; + std::shared_ptr>> expansions = nullptr; + if (_viz_expansions) { + expansions = std::make_shared>>(); + } // Note: All exceptions thrown are handled by the planner server and returned to the action if (!_a_star->createPath(path, num_iterations, 0)) { if (num_iterations < _a_star->getMaxIterations()) { @@ -346,6 +364,20 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( } } + // Publish expansions for debug + if (_viz_expansions) { + geometry_msgs::msg::PoseArray msg; + geometry_msgs::msg::Pose msg_pose; + msg.header.stamp = _clock->now(); + msg.header.frame_id = _global_frame; + for (auto & e : *expansions) { + msg_pose.position.x = std::get<0>(e); + msg_pose.position.y = std::get<1>(e); + msg.poses.push_back(msg_pose); + } + _expansions_publisher->publish(msg); + } + // Convert to world coordinates plan.poses.reserve(path.size()); for (int i = path.size() - 1; i >= 0; --i) { From b924624e19b276d157db3aad14b4f9b79d45803c Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Tue, 16 May 2023 16:09:46 +0100 Subject: [PATCH 2/4] lint --- nav2_smac_planner/include/nav2_smac_planner/a_star.hpp | 1 + nav2_smac_planner/src/smac_planner_hybrid.cpp | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) 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 6adb08ff04..9e390080f3 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp @@ -22,6 +22,7 @@ #include #include #include +#include #include "Eigen/Core" #include "nav2_costmap_2d/costmap_2d.hpp" diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index abc67eb55e..9069c26d59 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -351,7 +351,7 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( NodeHybrid::CoordinateVector path; int num_iterations = 0; - std::shared_ptr>> expansions = nullptr; + std::shared_ptr>> expansions = nullptr; if (_viz_expansions) { expansions = std::make_shared>>(); } From e13eb2d994da234d0cd60b5838210c7b969b348d Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Wed, 17 May 2023 09:18:43 +0100 Subject: [PATCH 3/4] switch to unique_ptr --- nav2_smac_planner/include/nav2_smac_planner/a_star.hpp | 2 +- nav2_smac_planner/src/a_star.cpp | 2 +- nav2_smac_planner/src/smac_planner_hybrid.cpp | 8 ++++---- 3 files changed, 6 insertions(+), 6 deletions(-) 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 9e390080f3..b602c36736 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp @@ -110,7 +110,7 @@ class AStarAlgorithm */ bool createPath( CoordinateVector & path, int & num_iterations, const float & tolerance, - std::shared_ptr>> expansions_log = nullptr); + std::vector> * expansions_log = nullptr); /** * @brief Sets the collision checker to use diff --git a/nav2_smac_planner/src/a_star.cpp b/nav2_smac_planner/src/a_star.cpp index eb26f3a9ca..00517be873 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -222,7 +222,7 @@ template bool AStarAlgorithm::createPath( CoordinateVector & path, int & iterations, const float & tolerance, - std::shared_ptr>> expansions_log) + std::vector> * expansions_log) { steady_clock::time_point start_time = steady_clock::now(); _tolerance = tolerance; diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index 9069c26d59..209d36d39f 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -350,13 +350,13 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( // Compute plan NodeHybrid::CoordinateVector path; int num_iterations = 0; - - std::shared_ptr>> expansions = nullptr; + std::string error; + std::unique_ptr>> expansions = nullptr; if (_viz_expansions) { - expansions = std::make_shared>>(); + expansions = std::make_unique>>(); } // Note: All exceptions thrown are handled by the planner server and returned to the action - if (!_a_star->createPath(path, num_iterations, 0)) { + if (!_a_star->createPath(path, num_iterations, 0, expansions.get())) { if (num_iterations < _a_star->getMaxIterations()) { throw nav2_core::NoValidPathCouldBeFound("no valid path found"); } else { From 0554186fe522515a93513fbc180a68c3dead9c9a Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Mon, 22 May 2023 10:58:45 +0100 Subject: [PATCH 4/4] readme update --- nav2_smac_planner/README.md | 25 ++----------------------- 1 file changed, 2 insertions(+), 23 deletions(-) diff --git a/nav2_smac_planner/README.md b/nav2_smac_planner/README.md index 362a04e37b..e9a6861f05 100644 --- a/nav2_smac_planner/README.md +++ b/nav2_smac_planner/README.md @@ -126,6 +126,7 @@ planner_server: cache_obstacle_heuristic: True # For Hybrid nodes: Cache the obstacle map dynamic programming distance expansion heuristic between subsiquent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static. allow_reverse_expansion: False # For Lattice nodes: Whether to expand state lattice graph in forward primitives or reverse as well, will double the branching factor at each step. smooth_path: True # For Lattice/Hybrid nodes: Whether or not to smooth the path, always true for 2D nodes. + viz_expansions: True # For Hybrid nodes: Whether to publish expansions on the /expansions topic as an array of poses (the orientation has no meaning). WARNING: heavy to compute and to display, for debug only as it degrades the performance. smoother: max_iterations: 1000 w_smooth: 0.3 @@ -206,26 +207,4 @@ As such, it is recommended if you have sparse SLAM maps, gaps or holes in your m One interesting thing to note from the second figure is that you see a number of expansions in open space. This is due to travel / heuristic values being so similar, tuning values of the penalty weights can have a decent impact there. The defaults are set as a good middle ground between large open spaces and confined aisles (environment specific tuning could be done to reduce the number of expansions for a specific map, speeding up the planner). The planner actually runs substantially faster the more confined the areas of search / environments are -- but still plenty fast for even wide open areas! -Sometimes visualizing the expansions is very useful to debug potential concerns (why does this goal take longer to compute, why can't I find a path, etc), should you on rare occasion run into an issue. The following snippet is what I used to visualize the expansion in the images above which may help you in future endevours. - -``` cpp -// In createPath() -static auto node = std::make_shared("test"); -static auto pub = node->create_publisher("expansions", 1); -geometry_msgs::msg::PoseArray msg; -geometry_msgs::msg::Pose msg_pose; -msg.header.stamp = node->now(); -msg.header.frame_id = "map"; - -... - -// Each time we expand a new node -msg_pose.position.x = _costmap->getOriginX() + (current_node->pose.x * _costmap->getResolution()); -msg_pose.position.y = _costmap->getOriginY() + (current_node->pose.y * _costmap->getResolution()); -msg.poses.push_back(msg_pose); - -... - -// On backtrace or failure -pub->publish(msg); -``` +Sometimes visualizing the expansions is very useful to debug potential concerns (why does this goal take longer to compute, why can't I find a path, etc), should you on rare occasion run into an issue. You can enable the publication of the expansions on the `/expansions` topic for SmacHybrid with the parameter `viz_expansions: True`, beware that it should be used only for debug as it increases a lot the CPU usage.