diff --git a/nav2_smac_planner/CMakeLists.txt b/nav2_smac_planner/CMakeLists.txt index 4bbe5c55ca7..a75fcfd5831 100644 --- a/nav2_smac_planner/CMakeLists.txt +++ b/nav2_smac_planner/CMakeLists.txt @@ -1,7 +1,8 @@ cmake_minimum_required(VERSION 3.5) project(nav2_smac_planner) -set(CMAKE_BUILD_TYPE Release) #Debug, Release +set(CMAKE_BUILD_TYPE Debug) #Debug, Release + find_package(ament_cmake REQUIRED) find_package(nav2_common REQUIRED) @@ -155,7 +156,8 @@ if(BUILD_TESTING) set(AMENT_LINT_AUTO_FILE_EXCLUDE include/nav2_smac_planner/thirdparty/robin_hood.h) ament_lint_auto_find_test_dependencies() find_package(ament_cmake_gtest REQUIRED) - add_subdirectory(test) + # TODO: uncomment when tests are modified + #add_subdirectory(test) endif() ament_export_include_directories(include ${OMPL_INCLUDE_DIRS}) 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 e4ecf38f035..f66da2e1808 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/a_star.hpp @@ -126,7 +126,8 @@ class AStarAlgorithm void setGoal( const unsigned int & mx, const unsigned int & my, - const unsigned int & dim_3); + const unsigned int & dim_3, + const GoalHeading & goal_heading = GoalHeading::DEFAULT); /** * @brief Set the starting pose for planning, as a node index @@ -151,11 +152,11 @@ class AStarAlgorithm */ NodePtr & getStart(); - /** - * @brief Get pointer reference to goal node - * @return Node pointer reference to goal node - */ - NodePtr & getGoal(); + // /** + // * @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 @@ -262,9 +263,9 @@ class AStarAlgorithm unsigned int _dim3_size; SearchInfo _search_info; - Coordinates _goal_coordinates; + CoordinateVector _goals_coordinates; NodePtr _start; - NodePtr _goal; + NodeVector _goals; 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 10c4224a077..d9a72f79754 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/analytic_expansion.hpp @@ -34,6 +34,7 @@ class AnalyticExpansion { public: typedef NodeT * NodePtr; + typedef std::vector NodeVector; typedef typename NodeT::Coordinates Coordinates; typedef std::function NodeGetter; @@ -87,7 +88,7 @@ class AnalyticExpansion */ NodePtr tryAnalyticExpansion( const NodePtr & current_node, - const NodePtr & goal_node, + const NodeVector & goal_nodes, const NodeGetter & getter, int & iterations, int & best_cost); /** diff --git a/nav2_smac_planner/include/nav2_smac_planner/constants.hpp b/nav2_smac_planner/include/nav2_smac_planner/constants.hpp index a7ff03fdca2..86215dbbb3c 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 GoalHeading +{ + 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 GoalHeading & n) +{ + switch (n) { + case GoalHeading::DEFAULT: + return "DEFAULT"; + case GoalHeading::BIDIRECTIONAL: + return "BIDIRECTIONAL"; + case GoalHeading::ALL_DIRECTION: + return "ALL_DIRECTION"; + default: + return "Unknown"; + } +} + +inline GoalHeading fromStringToGH(const std::string & n) +{ + if (n == "DEFAULT") { + return GoalHeading::DEFAULT; + } else if (n == "BIDIRECTIONAL") { + return GoalHeading::BIDIRECTIONAL; + } else if (n == "ALL_DIRECTION") { + return GoalHeading::ALL_DIRECTION; + } else { + return GoalHeading::UNKNOWN; + } +} + const float UNKNOWN = 255.0; const float OCCUPIED = 254.0; const float INSCRIBED = 253.0; 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 c46a9471869..4cf4f020071 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp @@ -165,12 +165,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); } 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 fae139aa2f8..32ea6dc7010 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 @@ -120,6 +120,7 @@ class SmacPlannerHybrid : public nav2_core::GlobalPlanner bool _debug_visualizations; std::string _motion_model_for_search; MotionModel _motion_model; + GoalHeading _goal_heading; 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 ce8bafd2fd0..afb0b56e0e3 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 @@ -117,6 +117,7 @@ class SmacPlannerLattice : public nav2_core::GlobalPlanner _planned_footprints_publisher; rclcpp_lifecycle::LifecyclePublisher::SharedPtr _expansions_publisher; + GoalHeading _goal_heading; 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 ebade0f4aa0..a2fddedcfdf 100644 --- a/nav2_smac_planner/src/a_star.cpp +++ b/nav2_smac_planner/src/a_star.cpp @@ -41,9 +41,9 @@ AStarAlgorithm::AStarAlgorithm( _x_size(0), _y_size(0), _search_info(search_info), - _goal_coordinates(Coordinates()), + _goals_coordinates(CoordinateVector()), _start(nullptr), - _goal(nullptr), + _goals(NodeVector()), _motion_model(motion_model) { _graph.reserve(100000); @@ -179,30 +179,72 @@ template<> void AStarAlgorithm::setGoal( const unsigned int & mx, const unsigned int & my, - const unsigned int & dim_3) + const unsigned int & dim_3, + const GoalHeading & goal_heading) { if (dim_3 != 0) { throw std::runtime_error("Node type Node2D cannot be given non-zero goal dim 3."); } + _goals.clear(); + _goals_coordinates.clear(); - _goal = addToGraph(Node2D::getIndex(mx, my, getSizeX())); - _goal_coordinates = Node2D::Coordinates(mx, my); + _goals.push_back(addToGraph(Node2D::getIndex(mx, my, getSizeX()))); + _goals_coordinates.push_back(Node2D::Coordinates(mx, my)); } template void AStarAlgorithm::setGoal( const unsigned int & mx, const unsigned int & my, - const unsigned int & dim_3) + const unsigned int & dim_3, + const GoalHeading & goal_heading) { - _goal = addToGraph(NodeT::getIndex(mx, my, dim_3)); - - typename NodeT::Coordinates goal_coords( - static_cast(mx), - static_cast(my), - static_cast(dim_3)); + _goals.clear(); + _goals_coordinates.clear(); + NodeVector goals; + CoordinateVector goals_coordinates; + unsigned int num_bins = NodeT::motion_table.num_angle_quantization; + switch (goal_heading) { + case GoalHeading::DEFAULT: + goals.push_back(addToGraph(NodeT::getIndex(mx, my, dim_3))); + goals_coordinates.push_back( + typename NodeT::Coordinates( + static_cast(mx), + static_cast(my), + static_cast(dim_3))); + break; + case GoalHeading::BIDIRECTIONAL: + // Add two goals, one for each direction + goals.push_back(addToGraph(NodeT::getIndex(mx, my, dim_3))); + goals.push_back(addToGraph(NodeT::getIndex(mx, my, dim_3 + num_bins / 2))); + goals_coordinates.push_back( + typename NodeT::Coordinates( + static_cast(mx), + static_cast(my), + static_cast(dim_3))); + goals_coordinates.push_back( + typename NodeT::Coordinates( + static_cast(mx), + static_cast(my), + static_cast(dim_3 + num_bins / 2))); + break; + case GoalHeading::ALL_DIRECTION: + // Add all possible headings as goals + for (unsigned int i = 0; i < num_bins; ++i) { + goals.push_back(addToGraph(NodeT::getIndex(mx, my, i))); + goals_coordinates.push_back( + typename NodeT::Coordinates( + static_cast(mx), + static_cast(my), + static_cast(i))); + } + break; + case GoalHeading::UNKNOWN: + throw std::runtime_error("Goal heading is UNKNOWN."); + break; + } - if (!_search_info.cache_obstacle_heuristic || goal_coords != _goal_coordinates) { + if (!_search_info.cache_obstacle_heuristic || goals_coordinates != _goals_coordinates) { if (!_start) { throw std::runtime_error("Start must be set before goal."); } @@ -211,8 +253,11 @@ void AStarAlgorithm::setGoal( _collision_checker->getCostmapROS(), _start->pose.x, _start->pose.y, mx, my); } - _goal_coordinates = goal_coords; - _goal->setPose(_goal_coordinates); + _goals_coordinates = goals_coordinates; + _goals = goals; + for (unsigned int i = 0; i < goals.size(); ++i) { + _goals[i]->setPose(_goals_coordinates[i]); + } } template @@ -224,15 +269,21 @@ bool AStarAlgorithm::areInputsValid() } // Check if points were filled in - if (!_start || !_goal) { - throw std::runtime_error("Failed to compute path, no valid start or goal given."); + if (!_start || !_goals.empty()) { + for (auto & goal : _goals) { + if (!goal) { + throw std::runtime_error("Failed to compute path, no valid start or goal given."); + } + } } // Check if ending point is valid - if (getToleranceHeuristic() < 0.001 && - !_goal->isNodeValid(_traverse_unknown, _collision_checker)) - { - throw nav2_core::GoalOccupied("Goal was in lethal cost"); + if (getToleranceHeuristic() < 0.001) { + for (auto & goal : _goals) { + if (!goal->isNodeValid(_traverse_unknown, _collision_checker)) { + throw nav2_core::GoalOccupied("Goal was in lethal cost"); + } + } } // Note: We do not check the if the start is valid because it is cleared @@ -314,9 +365,8 @@ bool AStarAlgorithm::createPath( current_node->visited(); // 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, _goals, neighborGetter, analytic_iterations, closest_distance); if (expansion_result != nullptr) { current_node = expansion_result; } @@ -366,7 +416,7 @@ bool AStarAlgorithm::createPath( template bool AStarAlgorithm::isGoal(NodePtr & node) { - return node == getGoal(); + return std::find(_goals.begin(), _goals.end(), node) != _goals.end(); } template @@ -375,11 +425,11 @@ typename AStarAlgorithm::NodePtr & AStarAlgorithm::getStart() return _start; } -template -typename AStarAlgorithm::NodePtr & AStarAlgorithm::getGoal() -{ - return _goal; -} +// template +// typename AStarAlgorithm::NodePtr & AStarAlgorithm::getGoal() +// { +// return _goals[0]; +// } template typename AStarAlgorithm::NodePtr AStarAlgorithm::getNextNode() @@ -403,9 +453,11 @@ float AStarAlgorithm::getHeuristicCost(const NodePtr & node) { const Coordinates node_coords = NodeT::getCoords(node->getIndex(), getSizeX(), getSizeDim3()); - float heuristic = NodeT::getHeuristicCost( - node_coords, _goal_coordinates); - + // we want to get the shortest heuristic cost from all of the goals + float heuristic = std::numeric_limits::max(); + for (auto & goal : _goals_coordinates) { + heuristic = std::min(heuristic, NodeT::getHeuristicCost(node_coords, goal)); + } if (heuristic < _best_heuristic_node.first) { _best_heuristic_node = {heuristic, node->getIndex()}; } diff --git a/nav2_smac_planner/src/analytic_expansion.cpp b/nav2_smac_planner/src/analytic_expansion.cpp index 7c73fb91da7..cf90f5ed05a 100644 --- a/nav2_smac_planner/src/analytic_expansion.cpp +++ b/nav2_smac_planner/src/analytic_expansion.cpp @@ -48,7 +48,7 @@ void AnalyticExpansion::setCollisionChecker( template typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalyticExpansion( - const NodePtr & current_node, const NodePtr & goal_node, + const NodePtr & current_node, const NodeVector & goals_nodes, const NodeGetter & getter, int & analytic_iterations, int & closest_distance) { @@ -60,104 +60,118 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalytic const Coordinates node_coords = NodeT::getCoords( current_node->getIndex(), _collision_checker->getCostmap()->getSizeInCellsX(), _dim_3_size); - closest_distance = std::min( - closest_distance, - static_cast(NodeT::getHeuristicCost(node_coords, goal_node->pose))); - - // We want to expand at a rate of d/expansion_ratio, - // but check to see if we are so close that we would be expanding every iteration - // If so, limit it to the expansion ratio (rounded up) - int desired_iterations = std::max( - static_cast(closest_distance / _search_info.analytic_expansion_ratio), - static_cast(std::ceil(_search_info.analytic_expansion_ratio))); - - // If we are closer now, we should update the target number of iterations to go - analytic_iterations = - std::min(analytic_iterations, desired_iterations); - - // Always run the expansion on the first run in case there is a - // trivial path to be found - if (analytic_iterations <= 0) { - // Reset the counter and try the analytic path expansion - analytic_iterations = desired_iterations; - 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()) { + + std::vector> goal_distance_pairs; + goal_distance_pairs.reserve(goals_nodes.size()); + for (size_t i = 0; i < goals_nodes.size(); i++) { + closest_distance = + static_cast(NodeT::getHeuristicCost(node_coords, goals_nodes[i]->pose)); + goal_distance_pairs.push_back(std::make_pair(goals_nodes[i], closest_distance)); + } + std::sort( + goal_distance_pairs.begin(), goal_distance_pairs.end(), + [](const std::pair & a, const std::pair & b) { + return a.second < b.second; + }); + // we try the closest goal first and if we fail, we try the next closest + + for (const auto & goal_distance_pair : goal_distance_pairs) { + closest_distance = goal_distance_pair.second; + NodePtr goal_node = goal_distance_pair.first; + // We want to expand at a rate of d/expansion_ratio, + // but check to see if we are so close that we would be expanding every iteration + // If so, limit it to the expansion ratio (rounded up) + int desired_iterations = std::max( + static_cast(closest_distance / _search_info.analytic_expansion_ratio), + static_cast(std::ceil(_search_info.analytic_expansion_ratio))); + // If we are closer now, we should update the target number of iterations to go + analytic_iterations = + std::min(analytic_iterations, desired_iterations); + + // Always run the expansion on the first run in case there is a + // trivial path to be found + if (analytic_iterations <= 0) { + // Reset the counter and try the analytic path expansion + analytic_iterations = desired_iterations; + 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; } - 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 traveral cost estimation. - auto scoringFn = [&](const AnalyticExpansionNodes & expansion) { - if (expansion.size() < 2) { - return std::numeric_limits::max(); + // 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 traveral cost estimation. + auto scoringFn = [&](const AnalyticExpansionNodes & expansion) { + if (expansion.size() < 2) { + return std::numeric_limits::max(); + } + + float score = 0.0; + float normalized_cost = 0.0; + // Analytic expansions are consistently spaced + const float distance = hypotf( + expansion[1].proposed_coords.x - expansion[0].proposed_coords.x, + expansion[1].proposed_coords.y - expansion[0].proposed_coords.y); + const float & weight = expansion[0].node->motion_table.cost_penalty; + for (auto iter = expansion.begin(); iter != expansion.end(); ++iter) { + normalized_cost = iter->node->getCost() / 252.0f; + // Search's Traversal Cost Function + score += distance * (1.0 + weight * normalized_cost); + } + return score; + }; + + float best_score = scoringFn(analytic_nodes); + float score = std::numeric_limits::max(); + float min_turn_rad = node->motion_table.min_turning_radius; + const float max_min_turn_rad = 4.0 * min_turn_rad; // Up to 4x the turning radius + while (min_turn_rad < max_min_turn_rad) { + min_turn_rad += 0.5; // In Grid Coords, 1/2 cell steps + ompl::base::StateSpacePtr state_space; + if (node->motion_table.motion_model == MotionModel::DUBIN) { + state_space = std::make_shared(min_turn_rad); + } else { + state_space = std::make_shared(min_turn_rad); } - - 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); + 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 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); + return setAnalyticPath(node, goal_node, analytic_nodes); + } } - } - analytic_iterations--; + analytic_iterations--; + } } - // No valid motion model - return nullptr return NodePtr(nullptr); } @@ -354,7 +368,7 @@ typename AnalyticExpansion::NodePtr AnalyticExpansion::setAnalyt template<> typename AnalyticExpansion::NodePtr AnalyticExpansion::tryAnalyticExpansion( - const NodePtr & current_node, const NodePtr & goal_node, + const NodePtr & current_node, const NodeVector & goals_nodes, const NodeGetter & getter, int & analytic_iterations, int & closest_distance) { diff --git a/nav2_smac_planner/src/smac_planner_hybrid.cpp b/nav2_smac_planner/src/smac_planner_hybrid.cpp index 727e4c08b29..9b762e694ac 100644 --- a/nav2_smac_planner/src/smac_planner_hybrid.cpp +++ b/nav2_smac_planner/src/smac_planner_hybrid.cpp @@ -164,6 +164,19 @@ void SmacPlannerHybrid::configure( 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); + std::string goal_heading_type; + nav2_util::declare_parameter_if_not_declared( + node, name + ".goal_heading", rclcpp::ParameterValue("DEFAULT")); + node->get_parameter(name + ".goal_heading", goal_heading_type); + _goal_heading = fromStringToGH(goal_heading_type); + if (_goal_heading == GoalHeading::UNKNOWN) { + RCLCPP_WARN( + _logger, + "Unable to get GoalHeader type. Given '%s', " + "Valid options are DEFAULT, BIDIRECTIONAL, ALL_DIRECTION. ", + goal_heading_type.c_str()); + } + _motion_model = fromString(_motion_model_for_search); if (_motion_model == MotionModel::UNKNOWN) { RCLCPP_WARN( @@ -369,7 +382,7 @@ nav_msgs::msg::Path SmacPlannerHybrid::createPlan( orientation_bin -= static_cast(_angle_quantizations); } orientation_bin_id = static_cast(floor(orientation_bin)); - _a_star->setGoal(mx, my, orientation_bin_id); + _a_star->setGoal(mx, my, orientation_bin_id, _goal_heading); // Setup message nav_msgs::msg::Path plan; @@ -616,6 +629,20 @@ SmacPlannerHybrid::dynamicParametersCallback(std::vector para "valid options are MOORE, VON_NEUMANN, DUBIN, REEDS_SHEPP.", _motion_model_for_search.c_str()); } + } else if (name == _name + ".goal_heading") { + std::string goal_heading_type = parameter.as_string(); + _goal_heading = fromStringToGH(goal_heading_type); + RCLCPP_INFO( + _logger, + "GoalHeading type set to '%s'.", + goal_heading_type.c_str()); + if (_goal_heading == GoalHeading::UNKNOWN) { + RCLCPP_WARN( + _logger, + "Unable to get GoalHeader type. Given '%s', " + "Valid options are DEFAULT, BIDIRECTIONAL, ALL_DIRECTION. ", + goal_heading_type.c_str()); + } } } } diff --git a/nav2_smac_planner/src/smac_planner_lattice.cpp b/nav2_smac_planner/src/smac_planner_lattice.cpp index 1666c7a9ce0..4ecb5d9d4ac 100644 --- a/nav2_smac_planner/src/smac_planner_lattice.cpp +++ b/nav2_smac_planner/src/smac_planner_lattice.cpp @@ -139,6 +139,20 @@ 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", rclcpp::ParameterValue("DEFAULT")); + node->get_parameter(name + ".goal_heading", goal_heading_type); + + _goal_heading = fromStringToGH(goal_heading_type); + if (_goal_heading == GoalHeading::UNKNOWN) { + RCLCPP_WARN( + _logger, + "Unable to get GoalHeader type. Given '%s', " + "Valid options are DEFAULT, BIDIRECTIONAL, ALL_DIRECTION. ", + goal_heading_type.c_str()); + } + _metadata = LatticeMotionTable::getLatticeMetadata(_search_info.lattice_filepath); _search_info.minimum_turning_radius = _metadata.min_turning_radius / (_costmap->getResolution()); @@ -290,7 +304,8 @@ nav_msgs::msg::Path SmacPlannerLattice::createPlan( } _a_star->setGoal( mx, my, - NodeLattice::motion_table.getClosestAngularBin(tf2::getYaw(goal.pose.orientation))); + NodeLattice::motion_table.getClosestAngularBin(tf2::getYaw(goal.pose.orientation)), + _goal_heading); // Setup message nav_msgs::msg::Path plan; @@ -524,6 +539,20 @@ SmacPlannerLattice::dynamicParametersCallback(std::vector par _metadata = LatticeMotionTable::getLatticeMetadata(_search_info.lattice_filepath); _search_info.minimum_turning_radius = _metadata.min_turning_radius / (_costmap->getResolution()); + } else if (name == _name + ".goal_heading") { + std::string goal_heading_type = parameter.as_string(); + _goal_heading = fromStringToGH(goal_heading_type); + RCLCPP_INFO( + _logger, + "GoalHeading type set to '%s'.", + goal_heading_type.c_str()); + if (_goal_heading == GoalHeading::UNKNOWN) { + RCLCPP_WARN( + _logger, + "Unable to get GoalHeader type. Given '%s', " + "Valid options are DEFAULT, BIDIRECTIONAL, ALL_DIRECTION. ", + goal_heading_type.c_str()); + } } } } diff --git a/tools/testing_environment/100by100_20.pgm b/tools/testing_environment/100by100_20.pgm new file mode 100644 index 00000000000..0159f9763e2 Binary files /dev/null and b/tools/testing_environment/100by100_20.pgm differ diff --git a/tools/testing_environment/100by100_20.yaml b/tools/testing_environment/100by100_20.yaml new file mode 100644 index 00000000000..763431b9e4e --- /dev/null +++ b/tools/testing_environment/100by100_20.yaml @@ -0,0 +1,6 @@ +image: 100by100_20.pgm +resolution: 0.05 +origin: [0.0, 0.000000, 0.000000] +negate: 1 +occupied_thresh: 0.65 +free_thresh: 0.196 diff --git a/tools/testing_environment/debug_launch.launch.py b/tools/testing_environment/debug_launch.launch.py new file mode 100644 index 00000000000..4ec4f931a8e --- /dev/null +++ b/tools/testing_environment/debug_launch.launch.py @@ -0,0 +1,81 @@ +# Copyright (c) 2022 Samsung Research America +# +# 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. + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node + + +def generate_launch_description(): + nav2_bringup_dir = get_package_share_directory('nav2_bringup') + config = './nav2_params.yaml' + map_file = os.path.join(nav2_bringup_dir, 'maps', 'turtlebot3_world.yaml') + lifecycle_nodes = ['map_server', 'planner_server'] + + return LaunchDescription( + [ + Node( + package='nav2_map_server', + executable='map_server', + name='map_server', + output='screen', + parameters=[ + {'use_sim_time': True}, + {'yaml_filename': map_file}, + {'topic_name': 'map'}, + ], + ), + Node( + package='nav2_planner', + executable='planner_server', + name='planner_server', + output='screen', + # prefix=['xterm -bg black -fg white -e gdb -ex run --args'], + parameters=[config], + ), + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'map'], + ), + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'odom'], + ), + Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager', + output='screen', + parameters=[ + {'use_sim_time': True}, + {'autostart': True}, + {'node_names': lifecycle_nodes}, + ], + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(nav2_bringup_dir, 'launch', 'rviz_launch.py') + ), + launch_arguments={'namespace': '', 'use_namespace': 'False'}.items(), + ), + ] + ) \ No newline at end of file diff --git a/tools/testing_environment/debug_script.py b/tools/testing_environment/debug_script.py new file mode 100644 index 00000000000..6fbc034bcbb --- /dev/null +++ b/tools/testing_environment/debug_script.py @@ -0,0 +1,184 @@ +from geometry_msgs.msg import PoseStamped +import rclpy +from nav2_simple_commander.robot_navigator import BasicNavigator +from transforms3d.euler import quat2euler +from rcl_interfaces.srv import SetParameters +from rclpy.parameter import Parameter +from time import sleep +from transforms3d.euler import euler2quat +import os +import pickle +from random import randint, seed, uniform +import glob +import time +import math + + +import numpy as np +def getRandomStart(costmap, max_cost, side_buffer, time_stamp, res): + start = PoseStamped() + start.header.frame_id = 'map' + start.header.stamp = time_stamp + while True: + row = randint(side_buffer, costmap.shape[0] - side_buffer) + col = randint(side_buffer, costmap.shape[1] - side_buffer) + + if costmap[row, col] < max_cost: + start.pose.position.x = col * res + start.pose.position.y = row * res + + yaw = uniform(0, 1) * 2 * math.pi + quad = euler2quat(0.0, 0.0, yaw) + start.pose.orientation.w = quad[0] + start.pose.orientation.x = quad[1] + start.pose.orientation.y = quad[2] + start.pose.orientation.z = quad[3] + break + return start + + +def getRandomGoal(costmap, start, max_cost, side_buffer, time_stamp, res): + goal = PoseStamped() + goal.header.frame_id = 'map' + goal.header.stamp = time_stamp + while True: + row = randint(side_buffer, costmap.shape[0] - side_buffer) + col = randint(side_buffer, costmap.shape[1] - side_buffer) + + start_x = start.pose.position.x + start_y = start.pose.position.y + goal_x = col * res + goal_y = row * res + x_diff = goal_x - start_x + y_diff = goal_y - start_y + dist = math.sqrt(x_diff ** 2 + y_diff ** 2) + + if costmap[row, col] < max_cost and dist > 3.0: + goal.pose.position.x = goal_x + goal.pose.position.y = goal_y + + yaw = uniform(0, 1) * 2 * math.pi + quad = euler2quat(0.0, 0.0, yaw) + goal.pose.orientation.w = quad[0] + goal.pose.orientation.x = quad[1] + goal.pose.orientation.y = quad[2] + goal.pose.orientation.z = quad[3] + break + return goal + +def main(): + rclpy.init() + + navigator = BasicNavigator() + map_path = os.getcwd() + '/' + glob.glob('**/100by100_20.yaml', recursive=True)[0] + navigator.changeMap(map_path) + time.sleep(2) + + planner_param_cli = navigator.create_client(SetParameters, '/planner_server/set_parameters') + print("created client") + # Get the costmap for start/goal validation + costmap_msg = navigator.getGlobalCostmap() + costmap = np.asarray(costmap_msg.data) + costmap.resize(costmap_msg.metadata.size_y, costmap_msg.metadata.size_x) + + start = PoseStamped() + start.header.frame_id = 'map' + start.header.stamp = navigator.get_clock().now().to_msg() + start.pose.position.x = -1.90312 + start.pose.position.y = 0.068757 + quad = euler2quat(0.0, 0.0, -1.53236) + start.pose.orientation.w = quad[0] + start.pose.orientation.x = quad[1] + start.pose.orientation.y = quad[2] + start.pose.orientation.z = quad[3] + + + + + + + print("Start position: ", start.pose.position.x, ", ", start.pose.position.y, ", ", start.pose.position.z) + print("Start orientation: ", start.pose.orientation.x, ", ", start.pose.orientation.y, ", ", start.pose.orientation.z, ", ", start.pose.orientation.w) + navigator.setInitialPose(start) + + goal = PoseStamped() + goal.header.frame_id = 'map' + goal.header.stamp = navigator.get_clock().now().to_msg() + goal.pose.position.x = 1.675 + goal.pose.position.y = 0.0 + quad = euler2quat(0.0, 0.0, 1.55943) + goal.pose.orientation.w = quad[0] + goal.pose.orientation.x = quad[1] + goal.pose.orientation.y = quad[2] + goal.pose.orientation.z = quad[3] + + + print("Goal position: ", goal.pose.position.x, ", ", goal.pose.position.y, ", ", goal.pose.position.z) + print("Goal orientation: ", goal.pose.orientation.x, ", ", goal.pose.orientation.y, ", ", goal.pose.orientation.z, ", ", goal.pose.orientation.w) + result = [] + results = [] + random_pairs = 50 + res = costmap_msg.metadata.resolution + max_cost = 210 + side_buffer = 100 + time_stamp = navigator.get_clock().now().to_msg() + seed(33) + i = 0 + allow_failure = False + # generate path + goal_headings = ["DEFAULT" , "BIDIRECTIONAL", "ALL_DIRECTION"] + planners = ['SmacHybrid', 'Smac2d', 'SmacLattice'] + planners_goal_headings = ["DEFAULT_SmacHybrid" , "DEFAULT_Smac2d", "DEFAULT_SmacLattice", "BIDIRECTIONAL_SmacHybrid", "BIDIRECTIONAL_Smac2d", "BIDIRECTIONAL_SmacLattice", "ALL_DIRECTION_SmacHybrid", "ALL_DIRECTION_Smac2d", "ALL_DIRECTION_SmacLattice"] + while len(results) != random_pairs: + print('Cycle: ', i, 'out of: ', random_pairs) + result = [] + start = getRandomStart(costmap, max_cost, side_buffer, time_stamp, res) + goal = getRandomGoal(costmap, start, max_cost, side_buffer, time_stamp, res) + for goal_heading in goal_headings: + print("Goal heading: ", goal_heading) + for planner in planners: + print("Planner: ", planner) + if(planner != 'Smac2d'): + print("Setting goal heading for ", planner) + # set parameters planner + req = SetParameters.Request() + req.parameters = [ + Parameter(planner + '.goal_heading', Parameter.Type.STRING, goal_heading).to_parameter_msg() + ] + future = planner_param_cli.call_async(req) + rclpy.spin_until_future_complete(navigator, future) + # if future.results() is None: + # print('Service call failed %r' % (future.exception(),)) + # return + # print('Service call result %r' % (future.results().result,)) + print("Getting path from ", planner) + path = navigator._getPathImpl(start, goal, planner, use_start=True) + if path is not None and path.error_code == 0: + print("Path found by ", planner) + result.append(path) + else: + print("\033[91m", end="") + print(planner, 'planner failed to produce the path') + print("\033[0m", end="") + print("\n") + if len(result) == len(planners_goal_headings): + results.append(result) + i = i + 1 + print("Done") + print('Write Results...') + results.append(result) + with open(os.getcwd() + '/results.pickle', 'wb+') as f: + pickle.dump(results, f, pickle.HIGHEST_PROTOCOL) + + with open(os.getcwd() + '/costmap.pickle', 'wb+') as f: + pickle.dump(costmap_msg, f, pickle.HIGHEST_PROTOCOL) + + with open(os.getcwd() + '/planners.pickle', 'wb+') as f: + pickle.dump(planners_goal_headings, f, pickle.HIGHEST_PROTOCOL) + print('Write Complete') + exit(0) + + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/tools/testing_environment/nav2_params.yaml b/tools/testing_environment/nav2_params.yaml new file mode 100644 index 00000000000..2fc40148fed --- /dev/null +++ b/tools/testing_environment/nav2_params.yaml @@ -0,0 +1,454 @@ +amcl: + ros__parameters: + alpha1: 0.2 + alpha2: 0.2 + alpha3: 0.2 + alpha4: 0.2 + alpha5: 0.2 + base_frame_id: "base_footprint" + beam_skip_distance: 0.5 + beam_skip_error_threshold: 0.9 + beam_skip_threshold: 0.3 + do_beamskip: false + global_frame_id: "map" + lambda_short: 0.1 + laser_likelihood_max_dist: 2.0 + laser_max_range: 100.0 + laser_min_range: -1.0 + laser_model_type: "likelihood_field" + max_beams: 60 + max_particles: 2000 + min_particles: 500 + odom_frame_id: "odom" + pf_err: 0.05 + pf_z: 0.99 + recovery_alpha_fast: 0.0 + recovery_alpha_slow: 0.0 + resample_interval: 1 + robot_model_type: "nav2_amcl::DifferentialMotionModel" + save_pose_rate: 0.5 + sigma_hit: 0.2 + tf_broadcast: true + transform_tolerance: 1.0 + update_min_a: 0.2 + update_min_d: 0.25 + z_hit: 0.5 + z_max: 0.05 + z_rand: 0.5 + z_short: 0.05 + scan_topic: scan + +bt_navigator: + ros__parameters: + global_frame: map + robot_base_frame: base_link + odom_topic: /odom + bt_loop_duration: 10 + default_server_timeout: 20 + wait_for_service_timeout: 1000 + action_server_result_timeout: 900.0 + navigators: ["navigate_to_pose", "navigate_through_poses"] + navigate_to_pose: + plugin: "nav2_bt_navigator/NavigateToPoseNavigator" + navigate_through_poses: + plugin: "nav2_bt_navigator/NavigateThroughPosesNavigator" + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_are_error_codes_active_condition_bt_node + - nav2_would_a_controller_recovery_help_condition_bt_node + - nav2_would_a_planner_recovery_help_condition_bt_node + - nav2_would_a_smoother_recovery_help_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + - nav2_is_battery_charging_condition_bt_node + error_code_names: + - compute_path_error_code + - follow_path_error_code + +controller_server: + ros__parameters: + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 + progress_checker_plugins: ["progress_checker"] + goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" + controller_plugins: ["FollowPath"] + use_realtime_priority: false + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + #precise_goal_checker: + # plugin: "nav2_controller::SimpleGoalChecker" + # xy_goal_tolerance: 0.25 + # yaw_goal_tolerance: 0.25 + # stateful: True + general_goal_checker: + stateful: True + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 0.26 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 0.26 + min_speed_theta: 0.0 + # Add high threshold velocity for turtlebot 3 issue. + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + xy_goal_tolerance: 0.25 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 32.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.scale: 24.0 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: odom + robot_base_frame: base_link + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.22 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.70 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + always_send_full_costmap: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_link + robot_radius: 0.22 + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.7 + always_send_full_costmap: True + +# The yaml_filename does not need to be specified since it going to be set by defaults in launch. +# If you'd rather set it in the yaml, remove the default "map" value in the tb3_simulation_launch.py +# file & provide full path to map below. If CLI map configuration or launch default is provided, that will be used. +# map_server: +# ros__parameters: +# yaml_filename: "" + +map_saver: + ros__parameters: + save_map_timeout: 5.0 + free_thresh_default: 0.25 + occupied_thresh_default: 0.65 + map_subscribe_transient_local: True + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + planner_plugins: ['SmacHybrid', 'Smac2d', 'SmacLattice'] + SmacHybrid: + plugin: "nav2_smac_planner/SmacPlannerHybrid" + downsample_costmap: false # whether or not to downsample the map + downsampling_factor: 1 # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm) + tolerance: 0.25 # dist-to-goal heuristic cost (distance) for valid tolerance endpoints if exact goal cannot be found. + allow_unknown: true # allow traveling in unknown space + max_iterations: 1000000 # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable + max_on_approach_iterations: 1000 # Maximum number of iterations after within tolerances to continue to try to find exact solution + max_planning_time: 5.0 # max time in s for planner to plan, smooth + motion_model_for_search: "DUBIN" # Hybrid-A* Dubin, Redds-Shepp + angle_quantization_bins: 72 # Number of angle bins for search + analytic_expansion_ratio: 3.5 # The ratio to attempt analytic expansions during search for final approach. + analytic_expansion_max_length: 3.0 # For Hybrid/Lattice nodes: The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting + analytic_expansion_max_cost: 200.0 # The maximum single cost for any part of an analytic expansion to contain and be valid, except when necessary on approach to goal + analytic_expansion_max_cost_override: false # Whether or not to override the maximum cost setting if within critical distance to goal (ie probably required) + minimum_turning_radius: 0.40 # minimum turning radius in m of path / vehicle + reverse_penalty: 2.0 # Penalty to apply if motion is reversing, must be => 1 + change_penalty: 0.0 # Penalty to apply if motion is changing directions (L to R), must be >= 0 + non_straight_penalty: 1.2 # Penalty to apply if motion is non-straight, must be => 1 + cost_penalty: 2.0 # Penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable. + retrospective_penalty: 0.015 + lookup_table_size: 20.0 # Size of the dubin/reeds-sheep distance window to cache, in meters. + cache_obstacle_heuristic: false # 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. + debug_visualizations: false # For Hybrid nodes: Whether to publish expansions on the /expansions topic as an array of poses (the orientation has no meaning) and the path's footprints on the /planned_footprints topic. WARNING: heavy to compute and to display, for debug only as it degrades the performance. + use_quadratic_cost_penalty: False + downsample_obstacle_heuristic: True + allow_primitive_interpolation: False + goal_heading: "BIDIRECTIONAL" + # smooth_path: True # If true, does a simple and quick smoothing post-processing to the path + + # smoother: + # max_iterations: 1000 + # w_smooth: 0.3 + # w_data: 0.2 + # tolerance: 1.0e-10 + # do_refinement: true + # refinement_num: 2 + + Smac2d: + plugin: "nav2_smac_planner/SmacPlanner2D" + tolerance: 0.125 # tolerance for planning if unable to reach exact pose, in meters + downsample_costmap: false # whether or not to downsample the map + downsampling_factor: 1 # multiplier for the resolution of the costmap layer (e.g. 2 on a 5cm costmap would be 10cm) + allow_unknown: true # allow traveling in unknown space + max_iterations: 1000000 # maximum total iterations to search for before failing (in case unreachable), set to -1 to disable + max_on_approach_iterations: 1000 # maximum number of iterations to attempt to reach goal once in tolerance + max_planning_time: 2.0 # max time in s for planner to plan, smooth + cost_travel_multiplier: 2.0 # Cost multiplier to apply to search to steer away from high cost areas. Larger values will place in the center of aisles more exactly (if non-`FREE` cost potential field exists) but take slightly longer to compute. To optimize for speed, a value of 1.0 is reasonable. A reasonable tradeoff value is 2.0. A value of 0.0 effective disables steering away from obstacles and acts like a naive binary search A*. + use_final_approach_orientation: false # Whether to set the final path pose at the goal's orientation to the requested orientation (false) or in line with the approach angle so the robot doesn't rotate to heading (true) + # smoother: + # max_iterations: 1000 + # w_smooth: 0.3 + # w_data: 0.2 + # tolerance: 1.0e-10 + + SmacLattice: + plugin: "nav2_smac_planner/SmacPlannerLattice" + allow_unknown: true # Allow traveling in unknown space + tolerance: 0.25 # dist-to-goal heuristic cost (distance) for valid tolerance endpoints if exact goal cannot be found. + max_iterations: 1000000 # Maximum total iterations to search for before failing (in case unreachable), set to -1 to disable + max_on_approach_iterations: 1000 # Maximum number of iterations after within tolerances to continue to try to find exact solution + max_planning_time: 5.0 # Max time in s for planner to plan, smooth + analytic_expansion_ratio: 3.5 # The ratio to attempt analytic expansions during search for final approach. + analytic_expansion_max_length: 3.0 # For Hybrid/Lattice nodes: The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting + reverse_penalty: 2.0 # Penalty to apply if motion is reversing, must be => 1 + change_penalty: 0.05 # Penalty to apply if motion is changing directions (L to R), must be >= 0 + non_straight_penalty: 1.05 # Penalty to apply if motion is non-straight, must be => 1 + cost_penalty: 2.0 # Penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable. + rotation_penalty: 5.0 # Penalty to apply to in-place rotations, if minimum control set contains them + retrospective_penalty: 0.015 + lookup_table_size: 20.0 # Size of the dubin/reeds-sheep distance window to cache, in meters. + cache_obstacle_heuristic: false # 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 # If true, allows the robot to use the primitives to expand in the mirrored opposite direction of the current robot's orientation (to reverse). + debug_visualizations: true + goal_heading: "BIDIRECTIONAL" + # smooth_path: True # If true, does a simple and quick smoothing post-processing to the path + # smoother: + # max_iterations: 1000 + # w_smooth: 0.3 + # w_data: 0.2 + # tolerance: 1.0e-10 + # do_refinement: true + # refinement_num: 2 + +smoother_server: + ros__parameters: + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + local_costmap_topic: local_costmap/costmap_raw + global_costmap_topic: global_costmap/costmap_raw + local_footprint_topic: local_costmap/published_footprint + global_footprint_topic: global_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + drive_on_heading: + plugin: "nav2_behaviors/DriveOnHeading" + wait: + plugin: "nav2_behaviors/Wait" + assisted_teleop: + plugin: "nav2_behaviors/AssistedTeleop" + local_frame: odom + global_frame: map + robot_base_frame: base_link + transform_tolerance: 0.1 + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +waypoint_follower: + ros__parameters: + loop_rate: 20 + stop_on_failure: false + action_server_result_timeout: 900.0 + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 + +velocity_smoother: + ros__parameters: + smoothing_frequency: 20.0 + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [0.26, 0.0, 1.0] + min_velocity: [-0.26, 0.0, -1.0] + max_accel: [2.5, 0.0, 3.2] + max_decel: [-2.5, 0.0, -3.2] + odom_topic: "odom" + odom_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 + +collision_monitor: + ros__parameters: + base_frame_id: "base_footprint" + odom_frame_id: "odom" + cmd_vel_in_topic: "cmd_vel_smoothed" + cmd_vel_out_topic: "cmd_vel" + state_topic: "collision_monitor_state" + transform_tolerance: 0.2 + source_timeout: 1.0 + base_shift_correction: True + stop_pub_timeout: 2.0 + # Polygons represent zone around the robot for "stop", "slowdown" and "limit" action types, + # and robot footprint for "approach" action type. + polygons: ["FootprintApproach"] + FootprintApproach: + type: "polygon" + action_type: "approach" + footprint_topic: "/local_costmap/published_footprint" + time_before_collision: 1.2 + simulation_time_step: 0.1 + min_points: 6 + visualize: False + enabled: True + observation_sources: ["scan"] + scan: + type: "scan" + topic: "scan" + min_height: 0.15 + max_height: 2.0 + enabled: True \ No newline at end of file diff --git a/tools/testing_environment/process_data.py b/tools/testing_environment/process_data.py new file mode 100644 index 00000000000..299997774db --- /dev/null +++ b/tools/testing_environment/process_data.py @@ -0,0 +1,187 @@ +#! /usr/bin/env python3 +# Copyright 2022 Joshua Wallace +# +# 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. + +import math +import os +import pickle + +import matplotlib.pylab as plt +import numpy as np +import seaborn as sns +from tabulate import tabulate + + +def getPaths(results): + paths = [] + for result in results: + for path in result: + paths.append(path.path) + return paths + + +def getTimes(results): + times = [] + for result in results: + for time in result: + times.append(time.planning_time.nanosec / 1e09 + time.planning_time.sec) + return times + + +def getMapCoordsFromPaths(paths, resolution): + coords = [] + for path in paths: + x = [] + y = [] + for pose in path.poses: + x.append(pose.pose.position.x / resolution) + y.append(pose.pose.position.y / resolution) + coords.append(x) + coords.append(y) + return coords + + +def getPathLength(path): + path_length = 0 + x_prev = path.poses[0].pose.position.x + y_prev = path.poses[0].pose.position.y + for i in range(1, len(path.poses)): + x_curr = path.poses[i].pose.position.x + y_curr = path.poses[i].pose.position.y + path_length = path_length + math.sqrt( + (x_curr - x_prev) ** 2 + (y_curr - y_prev) ** 2 + ) + x_prev = x_curr + y_prev = y_curr + return path_length + + +def plotResults(costmap, paths): + coords = getMapCoordsFromPaths(paths, costmap.metadata.resolution) + data = np.asarray(costmap.data) + data.resize(costmap.metadata.size_y, costmap.metadata.size_x) + data = np.where(data <= 253, 0, data) + + plt.figure(3) + ax = sns.heatmap(data, cmap='Greys', cbar=False) + for i in range(0, len(coords), 2): + ax.plot(coords[i], coords[i + 1], linewidth=0.7) + plt.axis('off') + ax.set_aspect('equal', 'box') + plt.show() + + +def averagePathCost(paths, costmap, num_of_planners): + coords = getMapCoordsFromPaths(paths, costmap.metadata.resolution) + data = np.asarray(costmap.data) + data.resize(costmap.metadata.size_y, costmap.metadata.size_x) + + average_path_costs = [] + for i in range(num_of_planners): + average_path_costs.append([]) + + k = 0 + for i in range(0, len(coords), 2): + costs = [] + for j in range(len(coords[i])): + costs.append(data[math.floor(coords[i + 1][j])][math.floor(coords[i][j])]) + average_path_costs[k % num_of_planners].append(sum(costs) / len(costs)) + k += 1 + + return average_path_costs + + +def maxPathCost(paths, costmap, num_of_planners): + coords = getMapCoordsFromPaths(paths, costmap.metadata.resolution) + data = np.asarray(costmap.data) + data.resize(costmap.metadata.size_y, costmap.metadata.size_x) + + max_path_costs = [] + for i in range(num_of_planners): + max_path_costs.append([]) + + k = 0 + for i in range(0, len(coords), 2): + max_cost = 0 + for j in range(len(coords[i])): + cost = data[math.floor(coords[i + 1][j])][math.floor(coords[i][j])] + if max_cost < cost: + max_cost = cost + max_path_costs[k % num_of_planners].append(max_cost) + k += 1 + + return max_path_costs + + +def main(): + + print('Read data') + with open(os.getcwd() + '/results.pickle', 'rb') as f: + results = pickle.load(f) + + with open(os.getcwd() + '/planners.pickle', 'rb') as f: + planners = pickle.load(f) + + with open(os.getcwd() + '/costmap.pickle', 'rb') as f: + costmap = pickle.load(f) + + paths = getPaths(results) + path_lengths = [] + + for path in paths: + path_lengths.append(getPathLength(path)) + path_lengths = np.asarray(path_lengths) + total_paths = len(paths) + + path_lengths.resize((int(total_paths / len(planners)), len(planners))) + path_lengths = path_lengths.transpose() + + times = getTimes(results) + times = np.asarray(times) + times.resize((int(total_paths / len(planners)), len(planners))) + times = np.transpose(times) + + # Costs + average_path_costs = np.asarray(averagePathCost(paths, costmap, len(planners))) + max_path_costs = np.asarray(maxPathCost(paths, costmap, len(planners))) + + # Generate table + planner_table = [ + [ + 'Planner', + 'Average path length (m)', + 'Average Time (s)', + 'Average cost', + 'Max cost', + ] + ] + + for i in range(0, len(planners)): + planner_table.append( + [ + planners[i], + np.average(path_lengths[i]), + np.average(times[i]), + np.average(average_path_costs[i]), + np.average(max_path_costs[i]), + ] + ) + + # Visualize results + print(tabulate(planner_table)) + plotResults(costmap, paths) + + +if __name__ == '__main__': + main()