Skip to content

Commit

Permalink
Prevent analytic expansions from shortcutting Smac Planner feasible p…
Browse files Browse the repository at this point in the history
…aths (ros-navigation#3962)

* a potential solution to smac shortcutting

* costmap reoslution

* some fixes

* completed prototype

* some fixes for collision detection and performance

* completing shortcutting fix

* updating tests

* adding readme

---------

Signed-off-by: Steve Macenski <[email protected]>

(cherry picked from commit e2781e5)
  • Loading branch information
SteveMacenski authored and jplapp committed Nov 11, 2024
1 parent 0090f3d commit 2422490
Show file tree
Hide file tree
Showing 15 changed files with 181 additions and 49 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -274,8 +274,7 @@ bool BtActionServer<ActionT>::loadBehaviorTree(const std::string & bt_xml_filena

// Create the Behavior Tree from the XML input
try {
tree_ = bt_->createTreeFromFile(filename, blackboard_);
for (auto & blackboard : tree_.blackboard_stack) {
for (auto & blackboard : tree_->blackboard_stack) {
blackboard->set<rclcpp::Node::SharedPtr>("node", client_node_);
blackboard->set<std::chrono::milliseconds>("server_timeout", default_server_timeout_);
blackboard->set<std::chrono::milliseconds>("bt_loop_duration", bt_loop_duration_);
Expand Down
2 changes: 2 additions & 0 deletions nav2_smac_planner/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -115,6 +115,8 @@ planner_server:
angle_quantization_bins: 64 # For Hybrid nodes: Number of angle bins for search, must be 1 for 2D node (no angle search)
analytic_expansion_ratio: 3.5 # For Hybrid/Lattice nodes: 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 (in meters). This should be scaled with minimum turning radius and be no less than 4-5x the minimum radius
analytic_expansion_max_cost: true # For Hybrid/Lattice nodes: 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 # For Hybrid/Lattice nodes: Whether or not to override the maximum cost setting if within critical distance to goal (ie probably required)
minimum_turning_radius: 0.40 # For Hybrid/Lattice nodes: minimum turning radius in m of path / vehicle
reverse_penalty: 2.1 # For Reeds-Shepp model: penalty to apply if motion is reversing, must be => 1
change_penalty: 0.0 # For Hybrid nodes: penalty to apply if motion is changing directions, must be >= 0
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -73,7 +73,7 @@ class AnalyticExpansion
* @brief Sets the collision checker and costmap to use in expansion validation
* @param collision_checker Collision checker to use
*/
void setCollisionChecker(GridCollisionChecker * collision_checker);
void setCollisionChecker(GridCollisionChecker * & collision_checker);

/**
* @brief Attempt an analytic path completion
Expand All @@ -95,11 +95,12 @@ class AnalyticExpansion
* @param node The node to start the analytic path from
* @param goal The goal node to plan to
* @param getter The function object that gets valid nodes from the graph
* @param state_space State space to use for computing analytic expansions
* @return A set of analytically expanded nodes to the goal from current node, if possible
*/
AnalyticExpansionNodes getAnalyticPath(
const NodePtr & node, const NodePtr & goal,
const NodeGetter & getter);
const NodeGetter & getter, const ompl::base::StateSpacePtr & state_space);

/**
* @brief Takes final analytic expansion and appends to current expanded node
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -116,10 +116,10 @@ class GridCollisionChecker
protected:
std::vector<nav2_costmap_2d::Footprint> oriented_footprints_;
nav2_costmap_2d::Footprint unoriented_footprint_;
double footprint_cost_;
float footprint_cost_;
bool footprint_is_radius_;
std::vector<float> angles_;
double possible_inscribed_cost_{-1};
float possible_inscribed_cost_{-1};
rclcpp::Logger logger_{rclcpp::get_logger("SmacPlannerCollisionChecker")};
rclcpp::Clock::SharedPtr clock_;
};
Expand Down
8 changes: 4 additions & 4 deletions nav2_smac_planner/include/nav2_smac_planner/node_2d.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -87,7 +87,7 @@ class Node2D
* @brief Gets the accumulated cost at this node
* @return accumulated cost
*/
inline float & getAccumulatedCost()
inline float getAccumulatedCost()
{
return _accumulated_cost;
}
Expand All @@ -105,7 +105,7 @@ class Node2D
* @brief Gets the costmap cost at this node
* @return costmap cost
*/
inline float & getCost()
inline float getCost()
{
return _cell_cost;
}
Expand All @@ -123,7 +123,7 @@ class Node2D
* @brief Gets if cell has been visited in search
* @param If cell was visited
*/
inline bool & wasVisited()
inline bool wasVisited()
{
return _was_visited;
}
Expand Down Expand Up @@ -158,7 +158,7 @@ class Node2D
* @brief Gets cell index
* @return Reference to cell index
*/
inline unsigned int & getIndex()
inline unsigned int getIndex()
{
return _index;
}
Expand Down
8 changes: 4 additions & 4 deletions nav2_smac_planner/include/nav2_smac_planner/node_hybrid.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -214,7 +214,7 @@ class NodeHybrid
* @brief Gets the accumulated cost at this node
* @return accumulated cost
*/
inline float & getAccumulatedCost()
inline float getAccumulatedCost()
{
return _accumulated_cost;
}
Expand Down Expand Up @@ -250,7 +250,7 @@ class NodeHybrid
* @brief Gets the costmap cost at this node
* @return costmap cost
*/
inline float & getCost()
inline float getCost()
{
return _cell_cost;
}
Expand All @@ -259,7 +259,7 @@ class NodeHybrid
* @brief Gets if cell has been visited in search
* @param If cell was visited
*/
inline bool & wasVisited()
inline bool wasVisited()
{
return _was_visited;
}
Expand All @@ -276,7 +276,7 @@ class NodeHybrid
* @brief Gets cell index
* @return Reference to cell index
*/
inline unsigned int & getIndex()
inline unsigned int getIndex()
{
return _index;
}
Expand Down
10 changes: 6 additions & 4 deletions nav2_smac_planner/include/nav2_smac_planner/node_lattice.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -102,12 +102,14 @@ struct LatticeMotionTable
float reverse_penalty;
float travel_distance_reward;
float rotation_penalty;
float min_turning_radius;
bool allow_reverse_expansion;
std::vector<std::vector<MotionPrimitive>> motion_primitives;
ompl::base::StateSpacePtr state_space;
std::vector<TrigValues> trig_values;
std::string current_lattice_filepath;
LatticeMetadata lattice_metadata;
MotionModel motion_model = MotionModel::UNKNOWN;
};

/**
Expand Down Expand Up @@ -180,7 +182,7 @@ class NodeLattice
* @brief Gets the accumulated cost at this node
* @return accumulated cost
*/
inline float & getAccumulatedCost()
inline float getAccumulatedCost()
{
return _accumulated_cost;
}
Expand All @@ -198,7 +200,7 @@ class NodeLattice
* @brief Gets the costmap cost at this node
* @return costmap cost
*/
inline float & getCost()
inline float getCost()
{
return _cell_cost;
}
Expand All @@ -207,7 +209,7 @@ class NodeLattice
* @brief Gets if cell has been visited in search
* @param If cell was visited
*/
inline bool & wasVisited()
inline bool wasVisited()
{
return _was_visited;
}
Expand All @@ -224,7 +226,7 @@ class NodeLattice
* @brief Gets cell index
* @return Reference to cell index
*/
inline unsigned int & getIndex()
inline unsigned int getIndex()
{
return _index;
}
Expand Down
20 changes: 11 additions & 9 deletions nav2_smac_planner/include/nav2_smac_planner/types.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -34,15 +34,17 @@ typedef std::pair<float, unsigned int> NodeHeuristicPair;
*/
struct SearchInfo
{
float minimum_turning_radius;
float non_straight_penalty;
float change_penalty;
float reverse_penalty;
float cost_penalty;
float retrospective_penalty;
float rotation_penalty;
float analytic_expansion_ratio;
float analytic_expansion_max_length;
float minimum_turning_radius{8.0};
float non_straight_penalty{1.05};
float change_penalty{0.0};
float reverse_penalty{2.0};
float cost_penalty{2.0};
float retrospective_penalty{0.015};
float rotation_penalty{5.0};
float analytic_expansion_ratio{3.5};
float analytic_expansion_max_length{60.0};
float analytic_expansion_max_cost{200.0};
bool analytic_expansion_max_cost_override{false};
std::string lattice_filepath;
bool cache_obstacle_heuristic;
bool allow_reverse_expansion;
Expand Down
2 changes: 1 addition & 1 deletion nav2_smac_planner/src/a_star.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -114,7 +114,7 @@ void AStarAlgorithm<NodeT>::setCollisionChecker(GridCollisionChecker * collision
_y_size = y_size;
NodeT::initMotionModel(_motion_model, _x_size, _y_size, _dim3_size, _search_info);
}
_expander->setCollisionChecker(collision_checker);
_expander->setCollisionChecker(_collision_checker);
}

template<typename NodeT>
Expand Down
102 changes: 92 additions & 10 deletions nav2_smac_planner/src/analytic_expansion.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -41,7 +41,7 @@ AnalyticExpansion<NodeT>::AnalyticExpansion(

template<typename NodeT>
void AnalyticExpansion<NodeT>::setCollisionChecker(
GridCollisionChecker * collision_checker)
GridCollisionChecker * & collision_checker)
{
_collision_checker = collision_checker;
}
Expand Down Expand Up @@ -80,7 +80,8 @@ typename AnalyticExpansion<NodeT>::NodePtr AnalyticExpansion<NodeT>::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);
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;
Expand All @@ -94,7 +95,8 @@ typename AnalyticExpansion<NodeT>::NodePtr AnalyticExpansion<NodeT>::tryAnalytic
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);
refined_analytic_nodes =
getAnalyticPath(test_node, goal_node, getter, test_node->motion_table.state_space);
if (refined_analytic_nodes.empty()) {
break;
}
Expand All @@ -105,6 +107,50 @@ typename AnalyticExpansion<NodeT>::NodePtr AnalyticExpansion<NodeT>::tryAnalytic
}
}

// 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<float>::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<float>::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<ompl::base::DubinsStateSpace>(min_turn_rad);
} else {
state_space = std::make_shared<ompl::base::ReedsSheppStateSpace>(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);
}
}
Expand All @@ -120,18 +166,18 @@ template<typename NodeT>
typename AnalyticExpansion<NodeT>::AnalyticExpansionNodes AnalyticExpansion<NodeT>::getAnalyticPath(
const NodePtr & node,
const NodePtr & goal,
const NodeGetter & node_getter)
const NodeGetter & node_getter,
const ompl::base::StateSpacePtr & state_space)
{
static ompl::base::ScopedState<> from(node->motion_table.state_space), to(
node->motion_table.state_space), s(node->motion_table.state_space);
static ompl::base::ScopedState<> from(state_space), to(state_space), s(state_space);
from[0] = node->pose.x;
from[1] = node->pose.y;
from[2] = node->motion_table.getAngleFromBin(node->pose.theta);
to[0] = goal->pose.x;
to[1] = goal->pose.y;
to[2] = node->motion_table.getAngleFromBin(goal->pose.theta);

float d = node->motion_table.state_space->distance(from(), to());
float d = state_space->distance(from(), to());

// If the length is too far, exit. This prevents unsafe shortcutting of paths
// into higher cost areas far out from the goal itself, let search to the work of getting
Expand All @@ -142,8 +188,8 @@ typename AnalyticExpansion<NodeT>::AnalyticExpansionNodes AnalyticExpansion<Node
}

// A move of sqrt(2) is guaranteed to be in a new cell
static const float sqrt_2 = std::sqrt(2.);
unsigned int num_intervals = std::floor(d / sqrt_2);
static const float sqrt_2 = std::sqrt(2.0f);
unsigned int num_intervals = static_cast<unsigned int>(std::floor(d / sqrt_2));

AnalyticExpansionNodes possible_nodes;
// When "from" and "to" are zero or one cell away,
Expand All @@ -159,6 +205,8 @@ typename AnalyticExpansion<NodeT>::AnalyticExpansionNodes AnalyticExpansion<Node
float angle = 0.0;
Coordinates proposed_coordinates;
bool failure = false;
std::vector<float> node_costs;
node_costs.reserve(num_intervals);

// Check intermediary poses (non-goal, non-start)
for (float i = 1; i <= num_intervals; i++) {
Expand All @@ -182,6 +230,7 @@ typename AnalyticExpansion<NodeT>::AnalyticExpansionNodes AnalyticExpansion<Node
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);
node_costs.emplace_back(next->getCost());
prev = next;
} else {
// Abort
Expand All @@ -196,6 +245,38 @@ typename AnalyticExpansion<NodeT>::AnalyticExpansionNodes AnalyticExpansion<Node
}
}

if (!failure) {
// We found 'a' valid expansion. Now to tell if its a quality option...
const float max_cost = _search_info.analytic_expansion_max_cost;
if (*std::max_element(node_costs.begin(), node_costs.end()) > max_cost) {
// If any element is above the comfortable cost limit, check edge cases:
// (1) Check if goal is in greater than max_cost space requiring
// entering it, but only entering it on final approach, not in-and-out
// (2) Checks if goal is in normal space, but enters costed space unnecessarily
// mid-way through, skirting obstacle or in non-globally confined space
bool cost_exit_high_cost_region = false;
for (auto iter = node_costs.rbegin(); iter != node_costs.rend(); ++iter) {
const float & curr_cost = *iter;
if (curr_cost <= max_cost) {
cost_exit_high_cost_region = true;
} else if (curr_cost > max_cost && cost_exit_high_cost_region) {
failure = true;
break;
}
}

// (3) Handle exception: there may be no other option close to goal
// if max cost is set too low (optional)
if (failure) {
if (d < 2.0f * M_PI * goal->motion_table.min_turning_radius &&
_search_info.analytic_expansion_max_cost_override)
{
failure = false;
}
}
}
}

// Reset to initial poses to not impact future searches
for (const auto & node_pose : possible_nodes) {
const auto & n = node_pose.node;
Expand Down Expand Up @@ -256,7 +337,8 @@ typename AnalyticExpansion<Node2D>::AnalyticExpansionNodes AnalyticExpansion<Nod
getAnalyticPath(
const NodePtr & node,
const NodePtr & goal,
const NodeGetter & node_getter)
const NodeGetter & node_getter,
const ompl::base::StateSpacePtr & state_space)
{
return AnalyticExpansionNodes();
}
Expand Down
Loading

0 comments on commit 2422490

Please sign in to comment.