Skip to content

Commit

Permalink
Analytic expansion (#43)
Browse files Browse the repository at this point in the history
* Use OMPL to generate heuristics

The calculation is run at every planning cycle. It does not seem to slow down
the planner - the calculation time seems to be quick enough that the
improvement in graph expansion accounts for it.

* Use OMPL to calculate analytic solution when near goal

* Make angles multiples of the bin size to stop looping behaviour

* Uncrustify

* Use faster std::sqrt function

* Fix analytic path so that the collision checker has coordinates to check!

* Pre-allocate variables in analytic path expansion

* Rename typedef to NodeGetter to more accurately describe function

* Use distance rather than heuristic to determine when to perform analytic expansion

Also force the analytic expansion to run on first iteration in case path is trivial.

* Move the check for motion model into the main A* loop

* Add copyright notices

* Remove comment about relaxing node match tolerances

The analytic expansion removes the need for this.

* Correctly reset node coordinates when aborting from analytic expansion

* Move analytic expansion logic to separate function

* Uncrustify

* Remove unneeded call to get goal coordinates

* Fix the calculation of intervals in the analytic path

Reserve the number of candidate nodes we are expecting.
Base calculations on intervals rather than points - makes distances between nodes work properly.

* Rescale heuristic so that analytic expansions are based on distance

* Repeatedly split analytic path in half when checking for collision

* Add parameter to control rate of analytic expansion attempts

* Uncrustify

* Fix incorrect type in templated function

* Cpplint

* Revert "Repeatedly split analytic path in half when checking for collision"

This reverts commit 94d9ee0.

There was a marginal speed gain (perhaps!) and the splitting approach made
the code harder to understand and maintain.

* Uncrustify

* Add doxygen comments

* Add parameter description for analytic expansion ratio

* Set lower limit of 2 on number of iterations between analytic expansions

* Reduce expected number of iterations because of analytic completion

* Refactor analytic expansion ratio calcs to make logic easier to understand
  • Loading branch information
james-ward authored and SteveMacenski committed Oct 7, 2020
1 parent a04546f commit 9c8e88c
Show file tree
Hide file tree
Showing 10 changed files with 253 additions and 58 deletions.
7 changes: 4 additions & 3 deletions doc/parameters/param_list.md
Original file line number Diff line number Diff line change
Expand Up @@ -166,7 +166,7 @@ When `plugins` parameter is not overridden, the following default plugins are lo
| `<data source>`.marking | true | Whether source should mark in costmap |
| `<data source>`.clearing | false | Whether source should raytrace clear in costmap |
| `<data source>`.obstacle_range | 2.5 | Maximum range to mark obstacles in costmap |
| `<data source>`.raytrace_range | 3.0 | Maximum range to raytrace clear obstacles from costmap |
| `<data source>`.raytrace_range | 3.0 | Maximum range to raytrace clear obstacles from costmap |

## keepout filter

Expand Down Expand Up @@ -285,7 +285,7 @@ When `controller_plugins`\`progress_checker_plugin`\`goal_checker_plugin` parame
| `<dwb plugin>`.`<name>`.x_only_threshold | 0.05 | Threshold to check in the X velocity direction |
| `<dwb plugin>`.`<name>`.scale | 1.0 | Weighed scale for critic |

## kinematic_parameters
## kinematic_parameters

| Parameter | Default | Description |
| ----------| --------| ------------|
Expand All @@ -304,7 +304,7 @@ When `controller_plugins`\`progress_checker_plugin`\`goal_checker_plugin` parame
| `<dwb plugin>`.decel_lim_y | 0.0 | Maximum deceleration Y (m/s^2) |
| `<dwb plugin>`.decel_lim_theta | 0.0 | Maximum deceleration rotation (rad/s^2) |

## xy_theta_iterator
## xy_theta_iterator

| Parameter | Default | Description |
| ----------| --------| ------------|
Expand Down Expand Up @@ -505,6 +505,7 @@ When `planner_plugins` parameter is not overridden, the following default plugin
| `<name>`.change_penalty | 0.5 | Penalty to apply to SE2 node if changing direction |
| `<name>`.non_straight_penalty | 1.1 | Penalty to apply to SE2 node if non-straight direction |
| `<name>`.cost_penalty | 1.2 | Penalty to apply to SE2 node for cost at pose |
| `<name>`.analytic_expansion_ratio | 2.0 | For SE2 nodes the planner will attempt an analytic path expansion every N iterations, where N = closest_distance_to_goal / analytic_expansion_ratio. Higher ratios result in more frequent expansions |
| `<name>`.smoother.smoother.w_curve | 1.5 | Smoother weight on curvature of path |
| `<name>`.smoother.smoother.w_dist | 0.0 | Smoother weight on distance from original path |
| `<name>`.smoother.smoother.w_smooth | 15000 | Smoother weight on distance between nodes |
Expand Down
19 changes: 19 additions & 0 deletions smac_planner/include/smac_planner/a_star.hpp
Original file line number Diff line number Diff line change
@@ -1,4 +1,5 @@
// Copyright (c) 2020, Samsung Research America
// Copyright (c) 2020, Applied Electric Vehicles Pty Ltd
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand Down Expand Up @@ -58,6 +59,7 @@ class AStarAlgorithm
typedef typename NodeT::Coordinates Coordinates;
typedef typename NodeT::CoordinateVector CoordinateVector;
typedef typename NodeVector::iterator NeighborIterator;
typedef std::function<bool (const unsigned int &, NodeT * &)> NodeGetter;

/**
* @struct smac_planner::NodeComparator
Expand Down Expand Up @@ -149,6 +151,14 @@ class AStarAlgorithm
*/
void setFootprint(nav2_costmap_2d::Footprint footprint, bool use_radius);

/**
* @brief Perform an analytic path expansion to the goal
* @param node The node to start the analytic path from
* @param getter The function object that gets valid nodes from the graph
* @return Node pointer to goal node if successful, else return nullptr
*/
NodePtr getAnalyticPath(const NodePtr & node, const NodeGetter & getter);

/**
* @brief Set the starting pose for planning, as a node index
* @param node Node pointer to the goal node to backtrace
Expand Down Expand Up @@ -272,6 +282,15 @@ class AStarAlgorithm
*/
inline void clearGraph();

/**
* @brief Attempt an analytic path completion
* @return Node pointer reference to goal node if successful, else
* return nullptr
*/
inline NodePtr tryAnalyticExpansion(
const NodePtr & current_node,
const NodeGetter & getter, int & iterations, int & best_cost);

bool _traverse_unknown;
int _max_iterations;
int _max_on_approach_iterations;
Expand Down
17 changes: 16 additions & 1 deletion smac_planner/include/smac_planner/node_se2.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -320,6 +320,21 @@ class NodeSE2
return angle + x * angle_quantization + y * width * angle_quantization;
}

/**
* @brief Get index at coordinates
* @param x X coordinate of point
* @param y Y coordinate of point
* @param angle Theta coordinate of point
* @return Index
*/
static inline unsigned int getIndex(
const unsigned int & x, const unsigned int & y, const unsigned int & angle)
{
return getIndex(
x, y, angle, motion_table.size_x,
motion_table.num_angle_quantization);
}

/**
* @brief Get coordinates at index
* @param index Index of point
Expand Down Expand Up @@ -391,7 +406,7 @@ class NodeSE2
NodeSE2 * parent;
Coordinates pose;
static double neutral_cost;
static MotionTable _motion_model;
static MotionTable motion_table;

private:
float _cell_cost;
Expand Down
1 change: 1 addition & 0 deletions smac_planner/include/smac_planner/types.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -34,6 +34,7 @@ struct SearchInfo
float change_penalty;
float reverse_penalty;
float cost_penalty;
float analytic_expansion_ratio;
};

} // namespace smac_planner
Expand Down
153 changes: 152 additions & 1 deletion smac_planner/src/a_star.cpp
Original file line number Diff line number Diff line change
@@ -1,4 +1,5 @@
// Copyright (c) 2020, Samsung Research America
// Copyright (c) 2020, Applied Electric Vehicles Pty Ltd
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand All @@ -14,6 +15,10 @@

#include <omp.h>

#include <ompl/base/ScopedState.h>
#include <ompl/base/spaces/DubinsStateSpace.h>
#include <ompl/base/spaces/ReedsSheppStateSpace.h>

#include <cmath>
#include <stdexcept>
#include <memory>
Expand All @@ -23,6 +28,7 @@
#include <chrono>
#include <thread>
#include <utility>
#include <vector>

#include "smac_planner/a_star.hpp"
using namespace std::chrono; // NOLINT
Expand Down Expand Up @@ -240,10 +246,12 @@ bool AStarAlgorithm<NodeT>::createPath(
NodeVector neighbors;
int approach_iterations = 0;
NeighborIterator neighbor_iterator;
int analytic_iterations = 0;
int closest_distance = std::numeric_limits<int>::max();

// Given an index, return a node ptr reference if its collision-free and valid
const unsigned int max_index = getSizeX() * getSizeY() * getSizeDim3();
std::function<bool(const unsigned int &, NodeT * &)> neighborGetter =
NodeGetter neighborGetter =
[&, this](const unsigned int & index, NodePtr & neighbor_rtn) -> bool
{
if (index < 0 || index >= max_index) {
Expand All @@ -269,6 +277,15 @@ bool AStarAlgorithm<NodeT>::createPath(
// 2) Mark Nbest as visited
current_node->visited();

// 2.a) Use an analytic expansion (if available) to generate a path
// to the goal.
NodePtr result = tryAnalyticExpansion(
current_node, neighborGetter, analytic_iterations,
closest_distance);
if (result != nullptr) {
current_node = result;
}

// 3) Check if we're at the goal, backtrace if required
if (isGoal(current_node)) {
return backtracePath(current_node, path);
Expand Down Expand Up @@ -317,6 +334,98 @@ bool AStarAlgorithm<NodeT>::isGoal(NodePtr & node)
return node == getGoal();
}

template<>
AStarAlgorithm<NodeSE2>::NodePtr AStarAlgorithm<NodeSE2>::getAnalyticPath(
const NodePtr & node,
const NodeGetter & node_getter)
{
ompl::base::ScopedState<> from(node->motion_table.state_space), to(
node->motion_table.state_space), s(node->motion_table.state_space);
const NodeSE2::Coordinates & node_coords = node->pose;
from[0] = node_coords.x;
from[1] = node_coords.y;
from[2] = node_coords.theta * node->motion_table.bin_size;
to[0] = _goal_coordinates.x;
to[1] = _goal_coordinates.y;
to[2] = _goal_coordinates.theta * node->motion_table.bin_size;

float d = node->motion_table.state_space->distance(from(), to());
NodePtr prev(node);
// A move of sqrt(2) is guaranteed to be in a new cell
constexpr float sqrt_2 = std::sqrt(2.);
unsigned int num_intervals = std::floor(d / sqrt_2);

using PossibleNode = std::pair<NodePtr, Coordinates>;
std::vector<PossibleNode> possible_nodes;
possible_nodes.reserve(num_intervals - 1); // We won't store this node or the goal
std::vector<double> reals;
// Pre-allocate
unsigned int index = 0;
NodePtr next(nullptr);
float angle = 0.0;
Coordinates proposed_coordinates;
// Don't generate the first point because we are already there!
// And the last point is the goal, so ignore it too!
for (float i = 1; i < num_intervals; i++) {
node->motion_table.state_space->interpolate(from(), to(), i / num_intervals, s());
reals = s.reals();
angle = reals[2] / node->motion_table.bin_size;
while (angle >= node->motion_table.num_angle_quantization_float) {
angle -= node->motion_table.num_angle_quantization_float;
}
while (angle < 0.0) {
angle += node->motion_table.num_angle_quantization_float;
}
// Turn the pose into a node, and check if it is valid
index = NodeSE2::getIndex(
static_cast<unsigned int>(reals[0]),
static_cast<unsigned int>(reals[1]),
static_cast<unsigned int>(angle));
// Get the node from the graph
if (node_getter(index, next)) {
Coordinates initial_node_coords = next->pose;
proposed_coordinates = {static_cast<float>(reals[0]), static_cast<float>(reals[1]), angle};
next->setPose(proposed_coordinates);
if (next->isNodeValid(_traverse_unknown, _collision_checker) && next != prev) {
// Save the node, and its previous coordinates in case we need to abort
possible_nodes.emplace_back(next, initial_node_coords);
prev = next;
} else {
next->setPose(initial_node_coords);
for (const auto & node_pose : possible_nodes) {
const auto & n = node_pose.first;
n->setPose(node_pose.second);
}
return NodePtr(nullptr);
}
} else {
// Abort
for (const auto & node_pose : possible_nodes) {
const auto & n = node_pose.first;
n->setPose(node_pose.second);
}
return NodePtr(nullptr);
}
}
// Legitimate path - set the parent relationships - poses already set
prev = node;
for (const auto & node_pose : possible_nodes) {
const auto & n = node_pose.first;
n->parent = prev;
prev = n;
}
_goal->parent = prev;
return _goal;
}

template<typename NodeT>
typename AStarAlgorithm<NodeT>::NodePtr AStarAlgorithm<NodeT>::getAnalyticPath(
const NodePtr & node,
const NodeGetter & node_getter)
{
return NodePtr(nullptr);
}

template<>
bool AStarAlgorithm<Node2D>::backtracePath(NodePtr & node, CoordinateVector & path)
{
Expand Down Expand Up @@ -483,6 +592,48 @@ unsigned int & AStarAlgorithm<NodeT>::getSizeDim3()
return _dim3_size;
}

template<typename NodeT>
typename AStarAlgorithm<NodeT>::NodePtr AStarAlgorithm<NodeT>::tryAnalyticExpansion(
const NodePtr & current_node, const NodeGetter & getter, int & analytic_iterations,
int & closest_distance)
{
if (_motion_model == MotionModel::DUBIN || _motion_model == MotionModel::REEDS_SHEPP) {
// This must be a NodeSE2 node if we are using these motion models

// See if we are closer and should be expanding more often
const Coordinates node_coords =
NodeT::getCoords(current_node->getIndex(), getSizeX(), getSizeDim3());
closest_distance =
std::min(
closest_distance,
static_cast<int>(NodeT::getHeuristicCost(
node_coords,
_goal_coordinates) / NodeT::neutral_cost)
);
// 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<int>(closest_distance / _search_info.analytic_expansion_ratio),
static_cast<int>(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;
return getAnalyticPath(current_node, getter);
}
analytic_iterations--;
}
// No valid motion model - return nullptr
return NodePtr(nullptr);
}

// Instantiate algorithm for the supported template types
template class AStarAlgorithm<Node2D>;
template class AStarAlgorithm<NodeSE2>;
Expand Down
1 change: 1 addition & 0 deletions smac_planner/src/node_2d.cpp
Original file line number Diff line number Diff line change
@@ -1,4 +1,5 @@
// Copyright (c) 2020, Samsung Research America
// Copyright (c) 2020, Applied Electric Vehicles Pty Ltd
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand Down
Loading

0 comments on commit 9c8e88c

Please sign in to comment.