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 01aeca975d5..b602c367365 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" @@ -104,9 +105,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::vector> * 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 c782fe41e08..f944e3dac1f 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 06bc0520d43..333366d31ec 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -26,6 +26,8 @@ #include #include "nav2_smac_planner/a_star.hpp" +#include + using namespace std::chrono; // NOLINT namespace nav2_smac_planner @@ -221,7 +223,8 @@ bool AStarAlgorithm::areInputsValid() template bool AStarAlgorithm::createPath( CoordinateVector & path, int & iterations, - const float & tolerance) + const float & tolerance, + std::vector> * expansions_log) { steady_clock::time_point start_time = steady_clock::now(); _tolerance = tolerance; @@ -273,6 +276,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 ee614c7b946..2fdc82286af 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( @@ -328,9 +342,14 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( NodeHybrid::CoordinateVector path; int num_iterations = 0; std::string error; + std::unique_ptr>> expansions = nullptr; try { + if (_viz_expansions) { + expansions = std::make_unique>>(); + } if (!_a_star->createPath( - path, num_iterations, _tolerance / static_cast(costmap->getResolution()))) + path, num_iterations, _tolerance / static_cast(costmap->getResolution()), + expansions.get())) { if (num_iterations < _a_star->getMaxIterations()) { error = std::string("no valid path found"); @@ -343,6 +362,19 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( error += e.what(); } + 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); + } + if (!error.empty()) { RCLCPP_WARN( _logger,