add humble-navigation2
This commit is contained in:
X-lanni
@@ -0,0 +1,447 @@
|
||||
// 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.
|
||||
// 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. Reserved.
|
||||
|
||||
#include <omp.h>
|
||||
#include <cmath>
|
||||
#include <stdexcept>
|
||||
#include <memory>
|
||||
#include <algorithm>
|
||||
#include <limits>
|
||||
#include <type_traits>
|
||||
#include <chrono>
|
||||
#include <thread>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include "nav2_smac_planner/a_star.hpp"
|
||||
using namespace std::chrono; // NOLINT
|
||||
|
||||
namespace nav2_smac_planner
|
||||
{
|
||||
|
||||
template<typename NodeT>
|
||||
AStarAlgorithm<NodeT>::AStarAlgorithm(
|
||||
const MotionModel & motion_model,
|
||||
const SearchInfo & search_info)
|
||||
: _traverse_unknown(true),
|
||||
_is_initialized(false),
|
||||
_max_iterations(0),
|
||||
_max_planning_time(0),
|
||||
_x_size(0),
|
||||
_y_size(0),
|
||||
_search_info(search_info),
|
||||
_goal_coordinates(Coordinates()),
|
||||
_start(nullptr),
|
||||
_goal(nullptr),
|
||||
_motion_model(motion_model)
|
||||
{
|
||||
_graph.reserve(100000);
|
||||
}
|
||||
|
||||
template<typename NodeT>
|
||||
AStarAlgorithm<NodeT>::~AStarAlgorithm()
|
||||
{
|
||||
}
|
||||
|
||||
template<typename NodeT>
|
||||
void AStarAlgorithm<NodeT>::initialize(
|
||||
const bool & allow_unknown,
|
||||
int & max_iterations,
|
||||
const int & max_on_approach_iterations,
|
||||
const double & max_planning_time,
|
||||
const float & lookup_table_size,
|
||||
const unsigned int & dim_3_size)
|
||||
{
|
||||
_traverse_unknown = allow_unknown;
|
||||
_max_iterations = max_iterations;
|
||||
_max_on_approach_iterations = max_on_approach_iterations;
|
||||
_max_planning_time = max_planning_time;
|
||||
if(!_is_initialized) {
|
||||
NodeT::precomputeDistanceHeuristic(lookup_table_size, _motion_model, dim_3_size, _search_info);
|
||||
}
|
||||
_is_initialized = true;
|
||||
_dim3_size = dim_3_size;
|
||||
_expander = std::make_unique<AnalyticExpansion<NodeT>>(
|
||||
_motion_model, _search_info, _traverse_unknown, _dim3_size);
|
||||
}
|
||||
|
||||
template<>
|
||||
void AStarAlgorithm<Node2D>::initialize(
|
||||
const bool & allow_unknown,
|
||||
int & max_iterations,
|
||||
const int & max_on_approach_iterations,
|
||||
const double & max_planning_time,
|
||||
const float & /*lookup_table_size*/,
|
||||
const unsigned int & dim_3_size)
|
||||
{
|
||||
_traverse_unknown = allow_unknown;
|
||||
_max_iterations = max_iterations;
|
||||
_max_on_approach_iterations = max_on_approach_iterations;
|
||||
_max_planning_time = max_planning_time;
|
||||
|
||||
if (dim_3_size != 1) {
|
||||
throw std::runtime_error("Node type Node2D cannot be given non-1 dim 3 quantization.");
|
||||
}
|
||||
_dim3_size = dim_3_size;
|
||||
_expander = std::make_unique<AnalyticExpansion<Node2D>>(
|
||||
_motion_model, _search_info, _traverse_unknown, _dim3_size);
|
||||
}
|
||||
|
||||
template<typename NodeT>
|
||||
void AStarAlgorithm<NodeT>::setCollisionChecker(GridCollisionChecker * collision_checker)
|
||||
{
|
||||
_collision_checker = collision_checker;
|
||||
_costmap = collision_checker->getCostmap();
|
||||
unsigned int x_size = _costmap->getSizeInCellsX();
|
||||
unsigned int y_size = _costmap->getSizeInCellsY();
|
||||
|
||||
clearGraph();
|
||||
|
||||
if (getSizeX() != x_size || getSizeY() != y_size) {
|
||||
_x_size = x_size;
|
||||
_y_size = y_size;
|
||||
NodeT::initMotionModel(_motion_model, _x_size, _y_size, _dim3_size, _search_info);
|
||||
}
|
||||
_expander->setCollisionChecker(collision_checker);
|
||||
}
|
||||
|
||||
template<typename NodeT>
|
||||
typename AStarAlgorithm<NodeT>::NodePtr AStarAlgorithm<NodeT>::addToGraph(
|
||||
const unsigned int & index)
|
||||
{
|
||||
auto iter = _graph.find(index);
|
||||
if (iter != _graph.end()) {
|
||||
return &(iter->second);
|
||||
}
|
||||
|
||||
return &(_graph.emplace(index, NodeT(index)).first->second);
|
||||
}
|
||||
|
||||
template<>
|
||||
void AStarAlgorithm<Node2D>::setStart(
|
||||
const unsigned int & mx,
|
||||
const unsigned int & my,
|
||||
const unsigned int & dim_3)
|
||||
{
|
||||
if (dim_3 != 0) {
|
||||
throw std::runtime_error("Node type Node2D cannot be given non-zero starting dim 3.");
|
||||
}
|
||||
_start = addToGraph(Node2D::getIndex(mx, my, getSizeX()));
|
||||
}
|
||||
|
||||
template<typename NodeT>
|
||||
void AStarAlgorithm<NodeT>::setStart(
|
||||
const unsigned int & mx,
|
||||
const unsigned int & my,
|
||||
const unsigned int & dim_3)
|
||||
{
|
||||
_start = addToGraph(NodeT::getIndex(mx, my, dim_3));
|
||||
_start->setPose(
|
||||
Coordinates(
|
||||
static_cast<float>(mx),
|
||||
static_cast<float>(my),
|
||||
static_cast<float>(dim_3)));
|
||||
}
|
||||
|
||||
template<>
|
||||
void AStarAlgorithm<Node2D>::setGoal(
|
||||
const unsigned int & mx,
|
||||
const unsigned int & my,
|
||||
const unsigned int & dim_3)
|
||||
{
|
||||
if (dim_3 != 0) {
|
||||
throw std::runtime_error("Node type Node2D cannot be given non-zero goal dim 3.");
|
||||
}
|
||||
|
||||
_goal = addToGraph(Node2D::getIndex(mx, my, getSizeX()));
|
||||
_goal_coordinates = Node2D::Coordinates(mx, my);
|
||||
}
|
||||
|
||||
template<typename NodeT>
|
||||
void AStarAlgorithm<NodeT>::setGoal(
|
||||
const unsigned int & mx,
|
||||
const unsigned int & my,
|
||||
const unsigned int & dim_3)
|
||||
{
|
||||
_goal = addToGraph(NodeT::getIndex(mx, my, dim_3));
|
||||
|
||||
typename NodeT::Coordinates goal_coords(
|
||||
static_cast<float>(mx),
|
||||
static_cast<float>(my),
|
||||
static_cast<float>(dim_3));
|
||||
|
||||
if (!_search_info.cache_obstacle_heuristic || goal_coords != _goal_coordinates) {
|
||||
if (!_start) {
|
||||
throw std::runtime_error("Start must be set before goal.");
|
||||
}
|
||||
|
||||
NodeT::resetObstacleHeuristic(_costmap, _start->pose.x, _start->pose.y, mx, my);
|
||||
}
|
||||
|
||||
_goal_coordinates = goal_coords;
|
||||
_goal->setPose(_goal_coordinates);
|
||||
}
|
||||
|
||||
template<typename NodeT>
|
||||
bool AStarAlgorithm<NodeT>::areInputsValid()
|
||||
{
|
||||
// Check if graph was filled in
|
||||
if (_graph.empty()) {
|
||||
throw std::runtime_error("Failed to compute path, no costmap given.");
|
||||
}
|
||||
|
||||
// Check if points were filled in
|
||||
if (!_start || !_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 std::runtime_error("Failed to compute path, goal is occupied with no tolerance.");
|
||||
}
|
||||
|
||||
// Check if starting point is valid
|
||||
if (!_start->isNodeValid(_traverse_unknown, _collision_checker)) {
|
||||
throw std::runtime_error("Starting point in lethal space! Cannot create feasible plan.");
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
template<typename NodeT>
|
||||
bool AStarAlgorithm<NodeT>::createPath(
|
||||
CoordinateVector & path, int & iterations,
|
||||
const float & tolerance)
|
||||
{
|
||||
steady_clock::time_point start_time = steady_clock::now();
|
||||
_tolerance = tolerance;
|
||||
_best_heuristic_node = {std::numeric_limits<float>::max(), 0};
|
||||
clearQueue();
|
||||
|
||||
if (!areInputsValid()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// 0) Add starting point to the open set
|
||||
addNode(0.0, getStart());
|
||||
getStart()->setAccumulatedCost(0.0);
|
||||
|
||||
// Optimization: preallocate all variables
|
||||
NodePtr current_node = nullptr;
|
||||
NodePtr neighbor = nullptr;
|
||||
NodePtr expansion_result = nullptr;
|
||||
float g_cost = 0.0;
|
||||
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();
|
||||
NodeGetter neighborGetter =
|
||||
[&, this](const unsigned int & index, NodePtr & neighbor_rtn) -> bool
|
||||
{
|
||||
if (index >= max_index) {
|
||||
return false;
|
||||
}
|
||||
|
||||
neighbor_rtn = addToGraph(index);
|
||||
return true;
|
||||
};
|
||||
|
||||
while (iterations < getMaxIterations() && !_queue.empty()) {
|
||||
// Check for planning timeout only on every Nth iteration
|
||||
if (iterations % _timing_interval == 0) {
|
||||
std::chrono::duration<double> planning_duration =
|
||||
std::chrono::duration_cast<std::chrono::duration<double>>(steady_clock::now() - start_time);
|
||||
if (static_cast<double>(planning_duration.count()) >= _max_planning_time) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
// 1) Pick Nbest from O s.t. min(f(Nbest)), remove from queue
|
||||
current_node = getNextNode();
|
||||
|
||||
// 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()) {
|
||||
continue;
|
||||
}
|
||||
|
||||
iterations++;
|
||||
|
||||
// 2) Mark Nbest as visited
|
||||
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);
|
||||
if (expansion_result != nullptr) {
|
||||
current_node = expansion_result;
|
||||
}
|
||||
|
||||
// 3) Check if we're at the goal, backtrace if required
|
||||
if (isGoal(current_node)) {
|
||||
return current_node->backtracePath(path);
|
||||
} else if (_best_heuristic_node.first < getToleranceHeuristic()) {
|
||||
// Optimization: Let us find when in tolerance and refine within reason
|
||||
approach_iterations++;
|
||||
if (approach_iterations >= getOnApproachMaxIterations()) {
|
||||
return _graph.at(_best_heuristic_node.second).backtracePath(path);
|
||||
}
|
||||
}
|
||||
|
||||
// 4) Expand neighbors of Nbest not visited
|
||||
neighbors.clear();
|
||||
current_node->getNeighbors(neighborGetter, _collision_checker, _traverse_unknown, neighbors);
|
||||
|
||||
for (neighbor_iterator = neighbors.begin();
|
||||
neighbor_iterator != neighbors.end(); ++neighbor_iterator)
|
||||
{
|
||||
neighbor = *neighbor_iterator;
|
||||
|
||||
// 4.1) Compute the cost to go to this node
|
||||
g_cost = current_node->getAccumulatedCost() + current_node->getTraversalCost(neighbor);
|
||||
|
||||
// 4.2) If this is a lower cost than prior, we set this as the new cost and new approach
|
||||
if (g_cost < neighbor->getAccumulatedCost()) {
|
||||
neighbor->setAccumulatedCost(g_cost);
|
||||
neighbor->parent = current_node;
|
||||
|
||||
// 4.3) Add to queue with heuristic cost
|
||||
addNode(g_cost + getHeuristicCost(neighbor), neighbor);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (_best_heuristic_node.first < getToleranceHeuristic()) {
|
||||
// If we run out of serach options, return the path that is closest, if within tolerance.
|
||||
return _graph.at(_best_heuristic_node.second).backtracePath(path);
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
template<typename NodeT>
|
||||
bool AStarAlgorithm<NodeT>::isGoal(NodePtr & node)
|
||||
{
|
||||
return node == getGoal();
|
||||
}
|
||||
|
||||
template<typename NodeT>
|
||||
typename AStarAlgorithm<NodeT>::NodePtr & AStarAlgorithm<NodeT>::getStart()
|
||||
{
|
||||
return _start;
|
||||
}
|
||||
|
||||
template<typename NodeT>
|
||||
typename AStarAlgorithm<NodeT>::NodePtr & AStarAlgorithm<NodeT>::getGoal()
|
||||
{
|
||||
return _goal;
|
||||
}
|
||||
|
||||
template<typename NodeT>
|
||||
typename AStarAlgorithm<NodeT>::NodePtr AStarAlgorithm<NodeT>::getNextNode()
|
||||
{
|
||||
NodeBasic<NodeT> node = _queue.top().second;
|
||||
_queue.pop();
|
||||
node.processSearchNode();
|
||||
return node.graph_node_ptr;
|
||||
}
|
||||
|
||||
template<typename NodeT>
|
||||
void AStarAlgorithm<NodeT>::addNode(const float & cost, NodePtr & node)
|
||||
{
|
||||
NodeBasic<NodeT> queued_node(node->getIndex());
|
||||
queued_node.populateSearchNode(node);
|
||||
_queue.emplace(cost, queued_node);
|
||||
}
|
||||
|
||||
template<typename NodeT>
|
||||
float AStarAlgorithm<NodeT>::getHeuristicCost(const NodePtr & node)
|
||||
{
|
||||
const Coordinates node_coords =
|
||||
NodeT::getCoords(node->getIndex(), getSizeX(), getSizeDim3());
|
||||
float heuristic = NodeT::getHeuristicCost(
|
||||
node_coords, _goal_coordinates, _costmap);
|
||||
|
||||
if (heuristic < _best_heuristic_node.first) {
|
||||
_best_heuristic_node = {heuristic, node->getIndex()};
|
||||
}
|
||||
|
||||
return heuristic;
|
||||
}
|
||||
|
||||
template<typename NodeT>
|
||||
void AStarAlgorithm<NodeT>::clearQueue()
|
||||
{
|
||||
NodeQueue q;
|
||||
std::swap(_queue, q);
|
||||
}
|
||||
|
||||
template<typename NodeT>
|
||||
void AStarAlgorithm<NodeT>::clearGraph()
|
||||
{
|
||||
Graph g;
|
||||
std::swap(_graph, g);
|
||||
_graph.reserve(100000);
|
||||
}
|
||||
|
||||
template<typename NodeT>
|
||||
int & AStarAlgorithm<NodeT>::getMaxIterations()
|
||||
{
|
||||
return _max_iterations;
|
||||
}
|
||||
|
||||
template<typename NodeT>
|
||||
int & AStarAlgorithm<NodeT>::getOnApproachMaxIterations()
|
||||
{
|
||||
return _max_on_approach_iterations;
|
||||
}
|
||||
|
||||
template<typename NodeT>
|
||||
float & AStarAlgorithm<NodeT>::getToleranceHeuristic()
|
||||
{
|
||||
return _tolerance;
|
||||
}
|
||||
|
||||
template<typename NodeT>
|
||||
unsigned int & AStarAlgorithm<NodeT>::getSizeX()
|
||||
{
|
||||
return _x_size;
|
||||
}
|
||||
|
||||
template<typename NodeT>
|
||||
unsigned int & AStarAlgorithm<NodeT>::getSizeY()
|
||||
{
|
||||
return _y_size;
|
||||
}
|
||||
|
||||
template<typename NodeT>
|
||||
unsigned int & AStarAlgorithm<NodeT>::getSizeDim3()
|
||||
{
|
||||
return _dim3_size;
|
||||
}
|
||||
|
||||
// Instantiate algorithm for the supported template types
|
||||
template class AStarAlgorithm<Node2D>;
|
||||
template class AStarAlgorithm<NodeHybrid>;
|
||||
template class AStarAlgorithm<NodeLattice>;
|
||||
|
||||
} // namespace nav2_smac_planner
|
||||
@@ -0,0 +1,286 @@
|
||||
// Copyright (c) 2021, 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. Reserved.
|
||||
|
||||
#include <ompl/base/ScopedState.h>
|
||||
#include <ompl/base/spaces/DubinsStateSpace.h>
|
||||
#include <ompl/base/spaces/ReedsSheppStateSpace.h>
|
||||
|
||||
#include <algorithm>
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
|
||||
#include "nav2_smac_planner/analytic_expansion.hpp"
|
||||
|
||||
namespace nav2_smac_planner
|
||||
{
|
||||
|
||||
template<typename NodeT>
|
||||
AnalyticExpansion<NodeT>::AnalyticExpansion(
|
||||
const MotionModel & motion_model,
|
||||
const SearchInfo & search_info,
|
||||
const bool & traverse_unknown,
|
||||
const unsigned int & dim_3_size)
|
||||
: _motion_model(motion_model),
|
||||
_search_info(search_info),
|
||||
_traverse_unknown(traverse_unknown),
|
||||
_dim_3_size(dim_3_size),
|
||||
_collision_checker(nullptr)
|
||||
{
|
||||
}
|
||||
|
||||
template<typename NodeT>
|
||||
void AnalyticExpansion<NodeT>::setCollisionChecker(
|
||||
GridCollisionChecker * collision_checker)
|
||||
{
|
||||
_collision_checker = collision_checker;
|
||||
}
|
||||
|
||||
template<typename NodeT>
|
||||
typename AnalyticExpansion<NodeT>::NodePtr AnalyticExpansion<NodeT>::tryAnalyticExpansion(
|
||||
const NodePtr & current_node, const NodePtr & goal_node,
|
||||
const NodeGetter & getter, int & analytic_iterations,
|
||||
int & closest_distance)
|
||||
{
|
||||
// This must be a valid motion model for analytic expansion to be attempted
|
||||
if (_motion_model == MotionModel::DUBIN || _motion_model == MotionModel::REEDS_SHEPP ||
|
||||
_motion_model == MotionModel::STATE_LATTICE)
|
||||
{
|
||||
// See if we are closer and should be expanding more often
|
||||
auto costmap = _collision_checker->getCostmap();
|
||||
const Coordinates node_coords =
|
||||
NodeT::getCoords(current_node->getIndex(), costmap->getSizeInCellsX(), _dim_3_size);
|
||||
closest_distance = std::min(
|
||||
closest_distance,
|
||||
static_cast<int>(NodeT::getHeuristicCost(node_coords, goal_node->pose, costmap)));
|
||||
|
||||
// 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;
|
||||
AnalyticExpansionNodes analytic_nodes = getAnalyticPath(current_node, goal_node, getter);
|
||||
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);
|
||||
if (refined_analytic_nodes.empty()) {
|
||||
break;
|
||||
}
|
||||
analytic_nodes = refined_analytic_nodes;
|
||||
node = test_node;
|
||||
} else {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
return setAnalyticPath(node, goal_node, analytic_nodes);
|
||||
}
|
||||
}
|
||||
|
||||
analytic_iterations--;
|
||||
}
|
||||
|
||||
// No valid motion model - return nullptr
|
||||
return NodePtr(nullptr);
|
||||
}
|
||||
|
||||
template<typename NodeT>
|
||||
typename AnalyticExpansion<NodeT>::AnalyticExpansionNodes AnalyticExpansion<NodeT>::getAnalyticPath(
|
||||
const NodePtr & node,
|
||||
const NodePtr & goal,
|
||||
const NodeGetter & node_getter)
|
||||
{
|
||||
static ompl::base::ScopedState<> from(node->motion_table.state_space), to(
|
||||
node->motion_table.state_space), s(node->motion_table.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());
|
||||
|
||||
// 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
|
||||
// close before the analytic expansion brings it home. This should never be smaller than
|
||||
// 4-5x the minimum turning radius being used, or planning times will begin to spike.
|
||||
if (d > _search_info.analytic_expansion_max_length) {
|
||||
return AnalyticExpansionNodes();
|
||||
}
|
||||
|
||||
// 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);
|
||||
|
||||
AnalyticExpansionNodes possible_nodes;
|
||||
// When "from" and "to" are zero or one cell away,
|
||||
// num_intervals == 0
|
||||
possible_nodes.reserve(num_intervals); // We won't store this node or the goal
|
||||
std::vector<double> reals;
|
||||
double theta;
|
||||
|
||||
// Pre-allocate
|
||||
NodePtr prev(node);
|
||||
unsigned int index = 0;
|
||||
NodePtr next(nullptr);
|
||||
float angle = 0.0;
|
||||
Coordinates proposed_coordinates;
|
||||
bool failure = false;
|
||||
|
||||
// Check intermediary poses (non-goal, non-start)
|
||||
for (float i = 1; i <= num_intervals; i++) {
|
||||
node->motion_table.state_space->interpolate(from(), to(), i / num_intervals, s());
|
||||
reals = s.reals();
|
||||
// Make sure in range [0, 2PI)
|
||||
theta = (reals[2] < 0.0) ? (reals[2] + 2.0 * M_PI) : reals[2];
|
||||
theta = (theta > 2.0 * M_PI) ? (theta - 2.0 * M_PI) : theta;
|
||||
angle = node->motion_table.getClosestAngularBin(theta);
|
||||
|
||||
// Turn the pose into a node, and check if it is valid
|
||||
index = NodeT::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, proposed_coordinates);
|
||||
prev = next;
|
||||
} else {
|
||||
// Abort
|
||||
next->setPose(initial_node_coords);
|
||||
failure = true;
|
||||
break;
|
||||
}
|
||||
} else {
|
||||
// Abort
|
||||
failure = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// Reset to initial poses to not impact future searches
|
||||
for (const auto & node_pose : possible_nodes) {
|
||||
const auto & n = node_pose.node;
|
||||
n->setPose(node_pose.initial_coords);
|
||||
}
|
||||
|
||||
if (failure) {
|
||||
return AnalyticExpansionNodes();
|
||||
}
|
||||
|
||||
return possible_nodes;
|
||||
}
|
||||
|
||||
template<typename NodeT>
|
||||
typename AnalyticExpansion<NodeT>::NodePtr AnalyticExpansion<NodeT>::setAnalyticPath(
|
||||
const NodePtr & node,
|
||||
const NodePtr & goal_node,
|
||||
const AnalyticExpansionNodes & expanded_nodes)
|
||||
{
|
||||
_detached_nodes.clear();
|
||||
// Legitimate final path - set the parent relationships, states, and poses
|
||||
NodePtr prev = node;
|
||||
for (const auto & node_pose : expanded_nodes) {
|
||||
auto n = node_pose.node;
|
||||
cleanNode(n);
|
||||
if (n->getIndex() != goal_node->getIndex()) {
|
||||
if (n->wasVisited()) {
|
||||
_detached_nodes.push_back(std::make_unique<NodeT>(-1));
|
||||
n = _detached_nodes.back().get();
|
||||
}
|
||||
n->parent = prev;
|
||||
n->pose = node_pose.proposed_coords;
|
||||
n->visited();
|
||||
prev = n;
|
||||
}
|
||||
}
|
||||
if (goal_node != prev) {
|
||||
goal_node->parent = prev;
|
||||
cleanNode(goal_node);
|
||||
goal_node->visited();
|
||||
}
|
||||
return goal_node;
|
||||
}
|
||||
|
||||
template<>
|
||||
void AnalyticExpansion<NodeLattice>::cleanNode(const NodePtr & node)
|
||||
{
|
||||
node->setMotionPrimitive(nullptr);
|
||||
}
|
||||
|
||||
template<typename NodeT>
|
||||
void AnalyticExpansion<NodeT>::cleanNode(const NodePtr & /*expanded_nodes*/)
|
||||
{
|
||||
}
|
||||
|
||||
template<>
|
||||
typename AnalyticExpansion<Node2D>::AnalyticExpansionNodes AnalyticExpansion<Node2D>::
|
||||
getAnalyticPath(
|
||||
const NodePtr & node,
|
||||
const NodePtr & goal,
|
||||
const NodeGetter & node_getter)
|
||||
{
|
||||
return AnalyticExpansionNodes();
|
||||
}
|
||||
|
||||
template<>
|
||||
typename AnalyticExpansion<Node2D>::NodePtr AnalyticExpansion<Node2D>::setAnalyticPath(
|
||||
const NodePtr & node,
|
||||
const NodePtr & goal_node,
|
||||
const AnalyticExpansionNodes & expanded_nodes)
|
||||
{
|
||||
return NodePtr(nullptr);
|
||||
}
|
||||
|
||||
template<>
|
||||
typename AnalyticExpansion<Node2D>::NodePtr AnalyticExpansion<Node2D>::tryAnalyticExpansion(
|
||||
const NodePtr & current_node, const NodePtr & goal_node,
|
||||
const NodeGetter & getter, int & analytic_iterations,
|
||||
int & closest_distance)
|
||||
{
|
||||
return NodePtr(nullptr);
|
||||
}
|
||||
|
||||
template class AnalyticExpansion<Node2D>;
|
||||
template class AnalyticExpansion<NodeHybrid>;
|
||||
template class AnalyticExpansion<NodeLattice>;
|
||||
|
||||
} // namespace nav2_smac_planner
|
||||
@@ -0,0 +1,196 @@
|
||||
// Copyright (c) 2021, 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. Reserved.
|
||||
|
||||
#include "nav2_smac_planner/collision_checker.hpp"
|
||||
|
||||
namespace nav2_smac_planner
|
||||
{
|
||||
|
||||
GridCollisionChecker::GridCollisionChecker(
|
||||
nav2_costmap_2d::Costmap2D * costmap,
|
||||
unsigned int num_quantizations,
|
||||
rclcpp_lifecycle::LifecycleNode::SharedPtr node)
|
||||
: FootprintCollisionChecker(costmap)
|
||||
{
|
||||
if (node) {
|
||||
clock_ = node->get_clock();
|
||||
logger_ = node->get_logger();
|
||||
}
|
||||
|
||||
// Convert number of regular bins into angles
|
||||
float bin_size = 2 * M_PI / static_cast<float>(num_quantizations);
|
||||
angles_.reserve(num_quantizations);
|
||||
for (unsigned int i = 0; i != num_quantizations; i++) {
|
||||
angles_.push_back(bin_size * i);
|
||||
}
|
||||
}
|
||||
|
||||
// GridCollisionChecker::GridCollisionChecker(
|
||||
// nav2_costmap_2d::Costmap2D * costmap,
|
||||
// std::vector<float> & angles)
|
||||
// : FootprintCollisionChecker(costmap),
|
||||
// angles_(angles)
|
||||
// {
|
||||
// }
|
||||
|
||||
void GridCollisionChecker::setFootprint(
|
||||
const nav2_costmap_2d::Footprint & footprint,
|
||||
const bool & radius,
|
||||
const double & possible_inscribed_cost)
|
||||
{
|
||||
possible_inscribed_cost_ = possible_inscribed_cost;
|
||||
footprint_is_radius_ = radius;
|
||||
|
||||
// Use radius, no caching required
|
||||
if (radius) {
|
||||
return;
|
||||
}
|
||||
|
||||
// No change, no updates required
|
||||
if (footprint == unoriented_footprint_) {
|
||||
return;
|
||||
}
|
||||
|
||||
oriented_footprints_.clear();
|
||||
oriented_footprints_.reserve(angles_.size());
|
||||
double sin_th, cos_th;
|
||||
geometry_msgs::msg::Point new_pt;
|
||||
const unsigned int footprint_size = footprint.size();
|
||||
|
||||
// Precompute the orientation bins for checking to use
|
||||
for (unsigned int i = 0; i != angles_.size(); i++) {
|
||||
sin_th = sin(angles_[i]);
|
||||
cos_th = cos(angles_[i]);
|
||||
nav2_costmap_2d::Footprint oriented_footprint;
|
||||
oriented_footprint.reserve(footprint_size);
|
||||
|
||||
for (unsigned int j = 0; j < footprint_size; j++) {
|
||||
new_pt.x = footprint[j].x * cos_th - footprint[j].y * sin_th;
|
||||
new_pt.y = footprint[j].x * sin_th + footprint[j].y * cos_th;
|
||||
oriented_footprint.push_back(new_pt);
|
||||
}
|
||||
|
||||
oriented_footprints_.push_back(oriented_footprint);
|
||||
}
|
||||
|
||||
unoriented_footprint_ = footprint;
|
||||
}
|
||||
|
||||
bool GridCollisionChecker::inCollision(
|
||||
const float & x,
|
||||
const float & y,
|
||||
const float & angle_bin,
|
||||
const bool & traverse_unknown)
|
||||
{
|
||||
// Check to make sure cell is inside the map
|
||||
if (outsideRange(costmap_->getSizeInCellsX(), x) ||
|
||||
outsideRange(costmap_->getSizeInCellsY(), y))
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
// Assumes setFootprint already set
|
||||
double wx, wy;
|
||||
costmap_->mapToWorld(static_cast<double>(x), static_cast<double>(y), wx, wy);
|
||||
|
||||
if (!footprint_is_radius_) {
|
||||
// if footprint, then we check for the footprint's points, but first see
|
||||
// if the robot is even potentially in an inscribed collision
|
||||
footprint_cost_ = costmap_->getCost(
|
||||
static_cast<unsigned int>(x), static_cast<unsigned int>(y));
|
||||
|
||||
if (footprint_cost_ < possible_inscribed_cost_) {
|
||||
if (possible_inscribed_cost_ > 0) {
|
||||
return false;
|
||||
} else {
|
||||
RCLCPP_ERROR_THROTTLE(
|
||||
logger_, *clock_, 1000,
|
||||
"Inflation layer either not found or inflation is not set sufficiently for "
|
||||
"optimized non-circular collision checking capabilities. It is HIGHLY recommended to set"
|
||||
" the inflation radius to be at MINIMUM half of the robot's largest cross-section. See "
|
||||
"github.com/ros-planning/navigation2/tree/main/nav2_smac_planner#potential-fields"
|
||||
" for full instructions. This will substantially impact run-time performance.");
|
||||
}
|
||||
}
|
||||
|
||||
// If its inscribed, in collision, or unknown in the middle,
|
||||
// no need to even check the footprint, its invalid
|
||||
if (footprint_cost_ == UNKNOWN && !traverse_unknown) {
|
||||
return true;
|
||||
}
|
||||
|
||||
if (footprint_cost_ == INSCRIBED || footprint_cost_ == OCCUPIED) {
|
||||
return true;
|
||||
}
|
||||
|
||||
// if possible inscribed, need to check actual footprint pose.
|
||||
// Use precomputed oriented footprints are done on initialization,
|
||||
// offset by translation value to collision check
|
||||
geometry_msgs::msg::Point new_pt;
|
||||
const nav2_costmap_2d::Footprint & oriented_footprint = oriented_footprints_[angle_bin];
|
||||
nav2_costmap_2d::Footprint current_footprint;
|
||||
current_footprint.reserve(oriented_footprint.size());
|
||||
for (unsigned int i = 0; i < oriented_footprint.size(); ++i) {
|
||||
new_pt.x = wx + oriented_footprint[i].x;
|
||||
new_pt.y = wy + oriented_footprint[i].y;
|
||||
current_footprint.push_back(new_pt);
|
||||
}
|
||||
|
||||
footprint_cost_ = footprintCost(current_footprint);
|
||||
|
||||
if (footprint_cost_ == UNKNOWN && traverse_unknown) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// if occupied or unknown and not to traverse unknown space
|
||||
return footprint_cost_ >= OCCUPIED;
|
||||
} else {
|
||||
// if radius, then we can check the center of the cost assuming inflation is used
|
||||
footprint_cost_ = costmap_->getCost(
|
||||
static_cast<unsigned int>(x), static_cast<unsigned int>(y));
|
||||
|
||||
if (footprint_cost_ == UNKNOWN && traverse_unknown) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// if occupied or unknown and not to traverse unknown space
|
||||
return static_cast<double>(footprint_cost_) >= INSCRIBED;
|
||||
}
|
||||
}
|
||||
|
||||
bool GridCollisionChecker::inCollision(
|
||||
const unsigned int & i,
|
||||
const bool & traverse_unknown)
|
||||
{
|
||||
footprint_cost_ = costmap_->getCost(i);
|
||||
if (footprint_cost_ == UNKNOWN && traverse_unknown) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// if occupied or unknown and not to traverse unknown space
|
||||
return footprint_cost_ >= INSCRIBED;
|
||||
}
|
||||
|
||||
float GridCollisionChecker::getCost()
|
||||
{
|
||||
// Assumes inCollision called prior
|
||||
return static_cast<float>(footprint_cost_);
|
||||
}
|
||||
|
||||
bool GridCollisionChecker::outsideRange(const unsigned int & max, const float & value)
|
||||
{
|
||||
return value < 0.0f || value > max;
|
||||
}
|
||||
|
||||
} // namespace nav2_smac_planner
|
||||
@@ -0,0 +1,154 @@
|
||||
// Copyright (c) 2020, Carlos Luis
|
||||
// Copyright (c) 2020, 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. Reserved.
|
||||
|
||||
#include "nav2_smac_planner/costmap_downsampler.hpp"
|
||||
|
||||
#include <string>
|
||||
#include <memory>
|
||||
#include <algorithm>
|
||||
|
||||
namespace nav2_smac_planner
|
||||
{
|
||||
|
||||
CostmapDownsampler::CostmapDownsampler()
|
||||
: _costmap(nullptr),
|
||||
_downsampled_costmap(nullptr),
|
||||
_downsampled_costmap_pub(nullptr)
|
||||
{
|
||||
}
|
||||
|
||||
CostmapDownsampler::~CostmapDownsampler()
|
||||
{
|
||||
}
|
||||
|
||||
void CostmapDownsampler::on_configure(
|
||||
const nav2_util::LifecycleNode::WeakPtr & node,
|
||||
const std::string & global_frame,
|
||||
const std::string & topic_name,
|
||||
nav2_costmap_2d::Costmap2D * const costmap,
|
||||
const unsigned int & downsampling_factor,
|
||||
const bool & use_min_cost_neighbor)
|
||||
{
|
||||
_costmap = costmap;
|
||||
_downsampling_factor = downsampling_factor;
|
||||
_use_min_cost_neighbor = use_min_cost_neighbor;
|
||||
updateCostmapSize();
|
||||
|
||||
_downsampled_costmap = std::make_unique<nav2_costmap_2d::Costmap2D>(
|
||||
_downsampled_size_x, _downsampled_size_y, _downsampled_resolution,
|
||||
_costmap->getOriginX(), _costmap->getOriginY(), UNKNOWN);
|
||||
|
||||
if (!node.expired()) {
|
||||
_downsampled_costmap_pub = std::make_unique<nav2_costmap_2d::Costmap2DPublisher>(
|
||||
node, _downsampled_costmap.get(), global_frame, topic_name, false);
|
||||
}
|
||||
}
|
||||
|
||||
void CostmapDownsampler::on_activate()
|
||||
{
|
||||
if (_downsampled_costmap_pub) {
|
||||
_downsampled_costmap_pub->on_activate();
|
||||
}
|
||||
}
|
||||
|
||||
void CostmapDownsampler::on_deactivate()
|
||||
{
|
||||
if (_downsampled_costmap_pub) {
|
||||
_downsampled_costmap_pub->on_deactivate();
|
||||
}
|
||||
}
|
||||
|
||||
void CostmapDownsampler::on_cleanup()
|
||||
{
|
||||
_costmap = nullptr;
|
||||
_downsampled_costmap.reset();
|
||||
_downsampled_costmap_pub.reset();
|
||||
}
|
||||
|
||||
nav2_costmap_2d::Costmap2D * CostmapDownsampler::downsample(
|
||||
const unsigned int & downsampling_factor)
|
||||
{
|
||||
_downsampling_factor = downsampling_factor;
|
||||
updateCostmapSize();
|
||||
|
||||
// Adjust costmap size if needed
|
||||
if (_downsampled_costmap->getSizeInCellsX() != _downsampled_size_x ||
|
||||
_downsampled_costmap->getSizeInCellsY() != _downsampled_size_y ||
|
||||
_downsampled_costmap->getResolution() != _downsampled_resolution)
|
||||
{
|
||||
resizeCostmap();
|
||||
}
|
||||
|
||||
// Assign costs
|
||||
for (unsigned int i = 0; i < _downsampled_size_x; ++i) {
|
||||
for (unsigned int j = 0; j < _downsampled_size_y; ++j) {
|
||||
setCostOfCell(i, j);
|
||||
}
|
||||
}
|
||||
|
||||
if (_downsampled_costmap_pub) {
|
||||
_downsampled_costmap_pub->publishCostmap();
|
||||
}
|
||||
return _downsampled_costmap.get();
|
||||
}
|
||||
|
||||
void CostmapDownsampler::updateCostmapSize()
|
||||
{
|
||||
_size_x = _costmap->getSizeInCellsX();
|
||||
_size_y = _costmap->getSizeInCellsY();
|
||||
_downsampled_size_x = ceil(static_cast<float>(_size_x) / _downsampling_factor);
|
||||
_downsampled_size_y = ceil(static_cast<float>(_size_y) / _downsampling_factor);
|
||||
_downsampled_resolution = _downsampling_factor * _costmap->getResolution();
|
||||
}
|
||||
|
||||
void CostmapDownsampler::resizeCostmap()
|
||||
{
|
||||
_downsampled_costmap->resizeMap(
|
||||
_downsampled_size_x,
|
||||
_downsampled_size_y,
|
||||
_downsampled_resolution,
|
||||
_costmap->getOriginX(),
|
||||
_costmap->getOriginY());
|
||||
}
|
||||
|
||||
void CostmapDownsampler::setCostOfCell(
|
||||
const unsigned int & new_mx,
|
||||
const unsigned int & new_my)
|
||||
{
|
||||
unsigned int mx, my;
|
||||
unsigned char cost = _use_min_cost_neighbor ? 255 : 0;
|
||||
unsigned int x_offset = new_mx * _downsampling_factor;
|
||||
unsigned int y_offset = new_my * _downsampling_factor;
|
||||
|
||||
for (unsigned int i = 0; i < _downsampling_factor; ++i) {
|
||||
mx = x_offset + i;
|
||||
if (mx >= _size_x) {
|
||||
continue;
|
||||
}
|
||||
for (unsigned int j = 0; j < _downsampling_factor; ++j) {
|
||||
my = y_offset + j;
|
||||
if (my >= _size_y) {
|
||||
continue;
|
||||
}
|
||||
cost = _use_min_cost_neighbor ?
|
||||
std::min(cost, _costmap->getCost(mx, my)) :
|
||||
std::max(cost, _costmap->getCost(mx, my));
|
||||
}
|
||||
}
|
||||
|
||||
_downsampled_costmap->setCost(new_mx, new_my, cost);
|
||||
}
|
||||
|
||||
} // namespace nav2_smac_planner
|
||||
@@ -0,0 +1,171 @@
|
||||
// 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.
|
||||
// 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. Reserved.
|
||||
|
||||
#include "nav2_smac_planner/node_2d.hpp"
|
||||
|
||||
#include <vector>
|
||||
#include <limits>
|
||||
|
||||
namespace nav2_smac_planner
|
||||
{
|
||||
|
||||
// defining static member for all instance to share
|
||||
std::vector<int> Node2D::_neighbors_grid_offsets;
|
||||
float Node2D::cost_travel_multiplier = 2.0;
|
||||
|
||||
Node2D::Node2D(const unsigned int index)
|
||||
: parent(nullptr),
|
||||
_cell_cost(std::numeric_limits<float>::quiet_NaN()),
|
||||
_accumulated_cost(std::numeric_limits<float>::max()),
|
||||
_index(index),
|
||||
_was_visited(false),
|
||||
_is_queued(false)
|
||||
{
|
||||
}
|
||||
|
||||
Node2D::~Node2D()
|
||||
{
|
||||
parent = nullptr;
|
||||
}
|
||||
|
||||
void Node2D::reset()
|
||||
{
|
||||
parent = nullptr;
|
||||
_cell_cost = std::numeric_limits<float>::quiet_NaN();
|
||||
_accumulated_cost = std::numeric_limits<float>::max();
|
||||
_was_visited = false;
|
||||
_is_queued = false;
|
||||
}
|
||||
|
||||
bool Node2D::isNodeValid(
|
||||
const bool & traverse_unknown,
|
||||
GridCollisionChecker * collision_checker)
|
||||
{
|
||||
if (collision_checker->inCollision(this->getIndex(), traverse_unknown)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
_cell_cost = collision_checker->getCost();
|
||||
return true;
|
||||
}
|
||||
|
||||
float Node2D::getTraversalCost(const NodePtr & child)
|
||||
{
|
||||
float normalized_cost = child->getCost() / 252.0;
|
||||
const Coordinates A = getCoords(child->getIndex());
|
||||
const Coordinates B = getCoords(this->getIndex());
|
||||
const float & dx = A.x - B.x;
|
||||
const float & dy = A.y - B.y;
|
||||
static float sqrt_2 = sqrt(2);
|
||||
|
||||
// If a diagonal move, travel cost is sqrt(2) not 1.0.
|
||||
if ((dx * dx + dy * dy) > 1.05) {
|
||||
return sqrt_2 * (1.0 + cost_travel_multiplier * normalized_cost);
|
||||
}
|
||||
|
||||
// Length = 1.0
|
||||
return 1.0 + cost_travel_multiplier * normalized_cost;
|
||||
}
|
||||
|
||||
float Node2D::getHeuristicCost(
|
||||
const Coordinates & node_coords,
|
||||
const Coordinates & goal_coordinates,
|
||||
const nav2_costmap_2d::Costmap2D * /*costmap*/)
|
||||
{
|
||||
// Using Moore distance as it more accurately represents the distances
|
||||
// even a Van Neumann neighborhood robot can navigate.
|
||||
auto dx = goal_coordinates.x - node_coords.x;
|
||||
auto dy = goal_coordinates.y - node_coords.y;
|
||||
return std::sqrt(dx * dx + dy * dy);
|
||||
}
|
||||
|
||||
void Node2D::initMotionModel(
|
||||
const MotionModel & motion_model,
|
||||
unsigned int & x_size_uint,
|
||||
unsigned int & /*size_y*/,
|
||||
unsigned int & /*num_angle_quantization*/,
|
||||
SearchInfo & search_info)
|
||||
{
|
||||
if (motion_model != MotionModel::TWOD) {
|
||||
throw std::runtime_error("Invalid motion model for 2D node.");
|
||||
}
|
||||
|
||||
int x_size = static_cast<int>(x_size_uint);
|
||||
cost_travel_multiplier = search_info.cost_penalty;
|
||||
_neighbors_grid_offsets = {-1, +1, -x_size, +x_size, -x_size - 1,
|
||||
-x_size + 1, +x_size - 1, +x_size + 1};
|
||||
}
|
||||
|
||||
void Node2D::getNeighbors(
|
||||
std::function<bool(const unsigned int &, nav2_smac_planner::Node2D * &)> & NeighborGetter,
|
||||
GridCollisionChecker * collision_checker,
|
||||
const bool & traverse_unknown,
|
||||
NodeVector & neighbors)
|
||||
{
|
||||
// NOTE(stevemacenski): Irritatingly, the order here matters. If you start in free
|
||||
// space and then expand 8-connected, the first set of neighbors will be all cost
|
||||
// 1.0. Then its expansion will all be 2 * 1.0 but now multiple
|
||||
// nodes are touching that node so the last cell to update the back pointer wins.
|
||||
// Thusly, the ordering ends with the cardinal directions for both sets such that
|
||||
// behavior is consistent in large free spaces between them.
|
||||
// 100 50 0
|
||||
// 100 50 50
|
||||
// 100 100 100 where lower-middle '100' is visited with same cost by both bottom '50' nodes
|
||||
// Therefore, it is valuable to have some low-potential across the entire map
|
||||
// rather than a small inflation around the obstacles
|
||||
int index;
|
||||
NodePtr neighbor;
|
||||
int node_i = this->getIndex();
|
||||
const Coordinates parent = getCoords(this->getIndex());
|
||||
Coordinates child;
|
||||
|
||||
for (unsigned int i = 0; i != _neighbors_grid_offsets.size(); ++i) {
|
||||
index = node_i + _neighbors_grid_offsets[i];
|
||||
|
||||
// Check for wrap around conditions
|
||||
child = getCoords(index);
|
||||
if (fabs(parent.x - child.x) > 1 || fabs(parent.y - child.y) > 1) {
|
||||
continue;
|
||||
}
|
||||
|
||||
if (NeighborGetter(index, neighbor)) {
|
||||
if (neighbor->isNodeValid(traverse_unknown, collision_checker) && !neighbor->wasVisited()) {
|
||||
neighbors.push_back(neighbor);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool Node2D::backtracePath(CoordinateVector & path)
|
||||
{
|
||||
if (!this->parent) {
|
||||
return false;
|
||||
}
|
||||
|
||||
NodePtr current_node = this;
|
||||
|
||||
while (current_node->parent) {
|
||||
path.push_back(
|
||||
Node2D::getCoords(current_node->getIndex()));
|
||||
current_node = current_node->parent;
|
||||
}
|
||||
|
||||
// add the start pose
|
||||
path.push_back(Node2D::getCoords(current_node->getIndex()));
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
} // namespace nav2_smac_planner
|
||||
@@ -0,0 +1,77 @@
|
||||
// Copyright (c) 2021, 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. Reserved.
|
||||
|
||||
#include "nav2_smac_planner/node_basic.hpp"
|
||||
|
||||
namespace nav2_smac_planner
|
||||
{
|
||||
|
||||
template<typename Node2D>
|
||||
void NodeBasic<Node2D>::processSearchNode()
|
||||
{
|
||||
}
|
||||
|
||||
template<>
|
||||
void NodeBasic<NodeHybrid>::processSearchNode()
|
||||
{
|
||||
// We only want to override the node's pose if it has not yet been visited
|
||||
// to prevent the case that a node has been queued multiple times and
|
||||
// a new branch is overriding one of lower cost already visited.
|
||||
if (!this->graph_node_ptr->wasVisited()) {
|
||||
this->graph_node_ptr->pose = this->pose;
|
||||
this->graph_node_ptr->setMotionPrimitiveIndex(this->motion_index);
|
||||
}
|
||||
}
|
||||
|
||||
template<>
|
||||
void NodeBasic<NodeLattice>::processSearchNode()
|
||||
{
|
||||
// We only want to override the node's pose/primitive if it has not yet been visited
|
||||
// to prevent the case that a node has been queued multiple times and
|
||||
// a new branch is overriding one of lower cost already visited.
|
||||
if (!this->graph_node_ptr->wasVisited()) {
|
||||
this->graph_node_ptr->pose = this->pose;
|
||||
this->graph_node_ptr->setMotionPrimitive(this->prim_ptr);
|
||||
this->graph_node_ptr->backwards(this->backward);
|
||||
}
|
||||
}
|
||||
|
||||
template<>
|
||||
void NodeBasic<Node2D>::populateSearchNode(Node2D * & node)
|
||||
{
|
||||
this->graph_node_ptr = node;
|
||||
}
|
||||
|
||||
template<>
|
||||
void NodeBasic<NodeHybrid>::populateSearchNode(NodeHybrid * & node)
|
||||
{
|
||||
this->pose = node->pose;
|
||||
this->graph_node_ptr = node;
|
||||
this->motion_index = node->getMotionPrimitiveIndex();
|
||||
}
|
||||
|
||||
template<>
|
||||
void NodeBasic<NodeLattice>::populateSearchNode(NodeLattice * & node)
|
||||
{
|
||||
this->pose = node->pose;
|
||||
this->graph_node_ptr = node;
|
||||
this->prim_ptr = node->getMotionPrimitive();
|
||||
this->backward = node->isBackward();
|
||||
}
|
||||
|
||||
template class NodeBasic<Node2D>;
|
||||
template class NodeBasic<NodeHybrid>;
|
||||
template class NodeBasic<NodeLattice>;
|
||||
|
||||
} // namespace nav2_smac_planner
|
||||
@@ -0,0 +1,724 @@
|
||||
// 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.
|
||||
// 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. Reserved.
|
||||
|
||||
#include <math.h>
|
||||
#include <chrono>
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
#include <algorithm>
|
||||
#include <queue>
|
||||
#include <limits>
|
||||
#include <utility>
|
||||
|
||||
#include "ompl/base/ScopedState.h"
|
||||
#include "ompl/base/spaces/DubinsStateSpace.h"
|
||||
#include "ompl/base/spaces/ReedsSheppStateSpace.h"
|
||||
|
||||
#include "nav2_smac_planner/node_hybrid.hpp"
|
||||
|
||||
using namespace std::chrono; // NOLINT
|
||||
|
||||
namespace nav2_smac_planner
|
||||
{
|
||||
|
||||
// defining static member for all instance to share
|
||||
LookupTable NodeHybrid::obstacle_heuristic_lookup_table;
|
||||
double NodeHybrid::travel_distance_cost = sqrt(2);
|
||||
HybridMotionTable NodeHybrid::motion_table;
|
||||
float NodeHybrid::size_lookup = 25;
|
||||
LookupTable NodeHybrid::dist_heuristic_lookup_table;
|
||||
nav2_costmap_2d::Costmap2D * NodeHybrid::sampled_costmap = nullptr;
|
||||
CostmapDownsampler NodeHybrid::downsampler;
|
||||
ObstacleHeuristicQueue NodeHybrid::obstacle_heuristic_queue;
|
||||
|
||||
// Each of these tables are the projected motion models through
|
||||
// time and space applied to the search on the current node in
|
||||
// continuous map-coordinates (e.g. not meters but partial map cells)
|
||||
// Currently, these are set to project *at minimum* into a neighboring
|
||||
// cell. Though this could be later modified to project a certain
|
||||
// amount of time or particular distance forward.
|
||||
|
||||
// http://planning.cs.uiuc.edu/node821.html
|
||||
// Model for ackermann style vehicle with minimum radius restriction
|
||||
void HybridMotionTable::initDubin(
|
||||
unsigned int & size_x_in,
|
||||
unsigned int & /*size_y_in*/,
|
||||
unsigned int & num_angle_quantization_in,
|
||||
SearchInfo & search_info)
|
||||
{
|
||||
size_x = size_x_in;
|
||||
change_penalty = search_info.change_penalty;
|
||||
non_straight_penalty = search_info.non_straight_penalty;
|
||||
cost_penalty = search_info.cost_penalty;
|
||||
reverse_penalty = search_info.reverse_penalty;
|
||||
travel_distance_reward = 1.0f - search_info.retrospective_penalty;
|
||||
|
||||
// if nothing changed, no need to re-compute primitives
|
||||
if (num_angle_quantization_in == num_angle_quantization &&
|
||||
min_turning_radius == search_info.minimum_turning_radius &&
|
||||
motion_model == MotionModel::DUBIN)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
num_angle_quantization = num_angle_quantization_in;
|
||||
num_angle_quantization_float = static_cast<float>(num_angle_quantization);
|
||||
min_turning_radius = search_info.minimum_turning_radius;
|
||||
motion_model = MotionModel::DUBIN;
|
||||
|
||||
// angle must meet 3 requirements:
|
||||
// 1) be increment of quantized bin size
|
||||
// 2) chord length must be greater than sqrt(2) to leave current cell
|
||||
// 3) maximum curvature must be respected, represented by minimum turning angle
|
||||
// Thusly:
|
||||
// On circle of radius minimum turning angle, we need select motion primatives
|
||||
// with chord length > sqrt(2) and be an increment of our bin size
|
||||
//
|
||||
// chord >= sqrt(2) >= 2 * R * sin (angle / 2); where angle / N = quantized bin size
|
||||
// Thusly: angle <= 2.0 * asin(sqrt(2) / (2 * R))
|
||||
float angle = 2.0 * asin(sqrt(2.0) / (2 * min_turning_radius));
|
||||
// Now make sure angle is an increment of the quantized bin size
|
||||
// And since its based on the minimum chord, we need to make sure its always larger
|
||||
bin_size =
|
||||
2.0f * static_cast<float>(M_PI) / static_cast<float>(num_angle_quantization);
|
||||
float increments;
|
||||
if (angle < bin_size) {
|
||||
increments = 1.0f;
|
||||
} else {
|
||||
// Search dimensions are clean multiples of quantization - this prevents
|
||||
// paths with loops in them
|
||||
increments = ceil(angle / bin_size);
|
||||
}
|
||||
angle = increments * bin_size;
|
||||
|
||||
// find deflections
|
||||
// If we make a right triangle out of the chord in circle of radius
|
||||
// min turning angle, we can see that delta X = R * sin (angle)
|
||||
float delta_x = min_turning_radius * sin(angle);
|
||||
// Using that same right triangle, we can see that the complement
|
||||
// to delta Y is R * cos (angle). If we subtract R, we get the actual value
|
||||
float delta_y = min_turning_radius - (min_turning_radius * cos(angle));
|
||||
|
||||
projections.clear();
|
||||
projections.reserve(3);
|
||||
projections.emplace_back(hypotf(delta_x, delta_y), 0.0, 0.0); // Forward
|
||||
projections.emplace_back(delta_x, delta_y, increments); // Left
|
||||
projections.emplace_back(delta_x, -delta_y, -increments); // Right
|
||||
|
||||
// Create the correct OMPL state space
|
||||
state_space = std::make_unique<ompl::base::DubinsStateSpace>(min_turning_radius);
|
||||
|
||||
// Precompute projection deltas
|
||||
delta_xs.resize(projections.size());
|
||||
delta_ys.resize(projections.size());
|
||||
trig_values.resize(num_angle_quantization);
|
||||
|
||||
for (unsigned int i = 0; i != projections.size(); i++) {
|
||||
delta_xs[i].resize(num_angle_quantization);
|
||||
delta_ys[i].resize(num_angle_quantization);
|
||||
|
||||
for (unsigned int j = 0; j != num_angle_quantization; j++) {
|
||||
double cos_theta = cos(bin_size * j);
|
||||
double sin_theta = sin(bin_size * j);
|
||||
if (i == 0) {
|
||||
// if first iteration, cache the trig values for later
|
||||
trig_values[j] = {cos_theta, sin_theta};
|
||||
}
|
||||
delta_xs[i][j] = projections[i]._x * cos_theta - projections[i]._y * sin_theta;
|
||||
delta_ys[i][j] = projections[i]._x * sin_theta + projections[i]._y * cos_theta;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// http://planning.cs.uiuc.edu/node822.html
|
||||
// Same as Dubin model but now reverse is valid
|
||||
// See notes in Dubin for explanation
|
||||
void HybridMotionTable::initReedsShepp(
|
||||
unsigned int & size_x_in,
|
||||
unsigned int & /*size_y_in*/,
|
||||
unsigned int & num_angle_quantization_in,
|
||||
SearchInfo & search_info)
|
||||
{
|
||||
size_x = size_x_in;
|
||||
change_penalty = search_info.change_penalty;
|
||||
non_straight_penalty = search_info.non_straight_penalty;
|
||||
cost_penalty = search_info.cost_penalty;
|
||||
reverse_penalty = search_info.reverse_penalty;
|
||||
travel_distance_reward = 1.0f - search_info.retrospective_penalty;
|
||||
|
||||
// if nothing changed, no need to re-compute primitives
|
||||
if (num_angle_quantization_in == num_angle_quantization &&
|
||||
min_turning_radius == search_info.minimum_turning_radius &&
|
||||
motion_model == MotionModel::REEDS_SHEPP)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
num_angle_quantization = num_angle_quantization_in;
|
||||
num_angle_quantization_float = static_cast<float>(num_angle_quantization);
|
||||
min_turning_radius = search_info.minimum_turning_radius;
|
||||
motion_model = MotionModel::REEDS_SHEPP;
|
||||
|
||||
float angle = 2.0 * asin(sqrt(2.0) / (2 * min_turning_radius));
|
||||
bin_size =
|
||||
2.0f * static_cast<float>(M_PI) / static_cast<float>(num_angle_quantization);
|
||||
float increments;
|
||||
if (angle < bin_size) {
|
||||
increments = 1.0f;
|
||||
} else {
|
||||
increments = ceil(angle / bin_size);
|
||||
}
|
||||
angle = increments * bin_size;
|
||||
|
||||
float delta_x = min_turning_radius * sin(angle);
|
||||
float delta_y = min_turning_radius - (min_turning_radius * cos(angle));
|
||||
|
||||
projections.clear();
|
||||
projections.reserve(6);
|
||||
projections.emplace_back(hypotf(delta_x, delta_y), 0.0, 0.0); // Forward
|
||||
projections.emplace_back(delta_x, delta_y, increments); // Forward + Left
|
||||
projections.emplace_back(delta_x, -delta_y, -increments); // Forward + Right
|
||||
projections.emplace_back(-hypotf(delta_x, delta_y), 0.0, 0.0); // Backward
|
||||
projections.emplace_back(-delta_x, delta_y, -increments); // Backward + Left
|
||||
projections.emplace_back(-delta_x, -delta_y, increments); // Backward + Right
|
||||
|
||||
// Create the correct OMPL state space
|
||||
state_space = std::make_unique<ompl::base::ReedsSheppStateSpace>(min_turning_radius);
|
||||
|
||||
// Precompute projection deltas
|
||||
delta_xs.resize(projections.size());
|
||||
delta_ys.resize(projections.size());
|
||||
trig_values.resize(num_angle_quantization);
|
||||
|
||||
for (unsigned int i = 0; i != projections.size(); i++) {
|
||||
delta_xs[i].resize(num_angle_quantization);
|
||||
delta_ys[i].resize(num_angle_quantization);
|
||||
|
||||
for (unsigned int j = 0; j != num_angle_quantization; j++) {
|
||||
double cos_theta = cos(bin_size * j);
|
||||
double sin_theta = sin(bin_size * j);
|
||||
if (i == 0) {
|
||||
// if first iteration, cache the trig values for later
|
||||
trig_values[j] = {cos_theta, sin_theta};
|
||||
}
|
||||
delta_xs[i][j] = projections[i]._x * cos_theta - projections[i]._y * sin_theta;
|
||||
delta_ys[i][j] = projections[i]._x * sin_theta + projections[i]._y * cos_theta;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
MotionPoses HybridMotionTable::getProjections(const NodeHybrid * node)
|
||||
{
|
||||
MotionPoses projection_list;
|
||||
projection_list.reserve(projections.size());
|
||||
|
||||
for (unsigned int i = 0; i != projections.size(); i++) {
|
||||
const MotionPose & motion_model = projections[i];
|
||||
|
||||
// normalize theta, I know its overkill, but I've been burned before...
|
||||
const float & node_heading = node->pose.theta;
|
||||
float new_heading = node_heading + motion_model._theta;
|
||||
|
||||
if (new_heading < 0.0) {
|
||||
new_heading += num_angle_quantization_float;
|
||||
}
|
||||
|
||||
if (new_heading >= num_angle_quantization_float) {
|
||||
new_heading -= num_angle_quantization_float;
|
||||
}
|
||||
|
||||
projection_list.emplace_back(
|
||||
delta_xs[i][node_heading] + node->pose.x,
|
||||
delta_ys[i][node_heading] + node->pose.y,
|
||||
new_heading);
|
||||
}
|
||||
|
||||
return projection_list;
|
||||
}
|
||||
|
||||
unsigned int HybridMotionTable::getClosestAngularBin(const double & theta)
|
||||
{
|
||||
auto bin = static_cast<unsigned int>(round(static_cast<float>(theta) / bin_size));
|
||||
return bin < num_angle_quantization ? bin : 0u;
|
||||
}
|
||||
|
||||
float HybridMotionTable::getAngleFromBin(const unsigned int & bin_idx)
|
||||
{
|
||||
return bin_idx * bin_size;
|
||||
}
|
||||
|
||||
NodeHybrid::NodeHybrid(const unsigned int index)
|
||||
: parent(nullptr),
|
||||
pose(0.0f, 0.0f, 0.0f),
|
||||
_cell_cost(std::numeric_limits<float>::quiet_NaN()),
|
||||
_accumulated_cost(std::numeric_limits<float>::max()),
|
||||
_index(index),
|
||||
_was_visited(false),
|
||||
_motion_primitive_index(std::numeric_limits<unsigned int>::max())
|
||||
{
|
||||
}
|
||||
|
||||
NodeHybrid::~NodeHybrid()
|
||||
{
|
||||
parent = nullptr;
|
||||
}
|
||||
|
||||
void NodeHybrid::reset()
|
||||
{
|
||||
parent = nullptr;
|
||||
_cell_cost = std::numeric_limits<float>::quiet_NaN();
|
||||
_accumulated_cost = std::numeric_limits<float>::max();
|
||||
_was_visited = false;
|
||||
_motion_primitive_index = std::numeric_limits<unsigned int>::max();
|
||||
pose.x = 0.0f;
|
||||
pose.y = 0.0f;
|
||||
pose.theta = 0.0f;
|
||||
}
|
||||
|
||||
bool NodeHybrid::isNodeValid(
|
||||
const bool & traverse_unknown,
|
||||
GridCollisionChecker * collision_checker)
|
||||
{
|
||||
if (collision_checker->inCollision(
|
||||
this->pose.x, this->pose.y, this->pose.theta /*bin number*/, traverse_unknown))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
_cell_cost = collision_checker->getCost();
|
||||
return true;
|
||||
}
|
||||
|
||||
float NodeHybrid::getTraversalCost(const NodePtr & child)
|
||||
{
|
||||
const float normalized_cost = child->getCost() / 252.0;
|
||||
if (std::isnan(normalized_cost)) {
|
||||
throw std::runtime_error(
|
||||
"Node attempted to get traversal "
|
||||
"cost without a known SE2 collision cost!");
|
||||
}
|
||||
|
||||
// this is the first node
|
||||
if (getMotionPrimitiveIndex() == std::numeric_limits<unsigned int>::max()) {
|
||||
return NodeHybrid::travel_distance_cost;
|
||||
}
|
||||
|
||||
float travel_cost = 0.0;
|
||||
float travel_cost_raw =
|
||||
NodeHybrid::travel_distance_cost *
|
||||
(motion_table.travel_distance_reward + motion_table.cost_penalty * normalized_cost);
|
||||
|
||||
if (child->getMotionPrimitiveIndex() == 0 || child->getMotionPrimitiveIndex() == 3) {
|
||||
// New motion is a straight motion, no additional costs to be applied
|
||||
travel_cost = travel_cost_raw;
|
||||
} else {
|
||||
if (getMotionPrimitiveIndex() == child->getMotionPrimitiveIndex()) {
|
||||
// Turning motion but keeps in same direction: encourages to commit to turning if starting it
|
||||
travel_cost = travel_cost_raw * motion_table.non_straight_penalty;
|
||||
} else {
|
||||
// Turning motion and changing direction: penalizes wiggling
|
||||
travel_cost = travel_cost_raw *
|
||||
(motion_table.non_straight_penalty + motion_table.change_penalty);
|
||||
}
|
||||
}
|
||||
|
||||
if (child->getMotionPrimitiveIndex() > 2) {
|
||||
// reverse direction
|
||||
travel_cost *= motion_table.reverse_penalty;
|
||||
}
|
||||
|
||||
return travel_cost;
|
||||
}
|
||||
|
||||
float NodeHybrid::getHeuristicCost(
|
||||
const Coordinates & node_coords,
|
||||
const Coordinates & goal_coords,
|
||||
const nav2_costmap_2d::Costmap2D * /*costmap*/)
|
||||
{
|
||||
const float obstacle_heuristic =
|
||||
getObstacleHeuristic(node_coords, goal_coords, motion_table.cost_penalty);
|
||||
const float dist_heuristic = getDistanceHeuristic(node_coords, goal_coords, obstacle_heuristic);
|
||||
return std::max(obstacle_heuristic, dist_heuristic);
|
||||
}
|
||||
|
||||
void NodeHybrid::initMotionModel(
|
||||
const MotionModel & motion_model,
|
||||
unsigned int & size_x,
|
||||
unsigned int & size_y,
|
||||
unsigned int & num_angle_quantization,
|
||||
SearchInfo & search_info)
|
||||
{
|
||||
// find the motion model selected
|
||||
switch (motion_model) {
|
||||
case MotionModel::DUBIN:
|
||||
motion_table.initDubin(size_x, size_y, num_angle_quantization, search_info);
|
||||
break;
|
||||
case MotionModel::REEDS_SHEPP:
|
||||
motion_table.initReedsShepp(size_x, size_y, num_angle_quantization, search_info);
|
||||
break;
|
||||
default:
|
||||
throw std::runtime_error(
|
||||
"Invalid motion model for Hybrid A*. Please select between"
|
||||
" Dubin (Ackermann forward only),"
|
||||
" Reeds-Shepp (Ackermann forward and back).");
|
||||
}
|
||||
|
||||
travel_distance_cost = motion_table.projections[0]._x;
|
||||
}
|
||||
|
||||
inline float distanceHeuristic2D(
|
||||
const unsigned int idx, const unsigned int size_x,
|
||||
const unsigned int target_x, const unsigned int target_y)
|
||||
{
|
||||
int dx = static_cast<int>(idx % size_x) - static_cast<int>(target_x);
|
||||
int dy = static_cast<int>(idx / size_x) - static_cast<int>(target_y);
|
||||
return std::sqrt(dx * dx + dy * dy);
|
||||
}
|
||||
|
||||
void NodeHybrid::resetObstacleHeuristic(
|
||||
nav2_costmap_2d::Costmap2D * costmap,
|
||||
const unsigned int & start_x, const unsigned int & start_y,
|
||||
const unsigned int & goal_x, const unsigned int & goal_y)
|
||||
{
|
||||
// Downsample costmap 2x to compute a sparse obstacle heuristic. This speeds up
|
||||
// the planner considerably to search through 75% less cells with no detectable
|
||||
// erosion of path quality after even modest smoothing. The error would be no more
|
||||
// than 0.05 * normalized cost. Since this is just a search prior, there's no loss in generality
|
||||
std::weak_ptr<nav2_util::LifecycleNode> ptr;
|
||||
downsampler.on_configure(ptr, "fake_frame", "fake_topic", costmap, 2.0, true);
|
||||
downsampler.on_activate();
|
||||
sampled_costmap = downsampler.downsample(2.0);
|
||||
|
||||
// Clear lookup table
|
||||
unsigned int size = sampled_costmap->getSizeInCellsX() * sampled_costmap->getSizeInCellsY();
|
||||
if (obstacle_heuristic_lookup_table.size() == size) {
|
||||
// must reset all values
|
||||
std::fill(
|
||||
obstacle_heuristic_lookup_table.begin(),
|
||||
obstacle_heuristic_lookup_table.end(), 0.0);
|
||||
} else {
|
||||
unsigned int obstacle_size = obstacle_heuristic_lookup_table.size();
|
||||
obstacle_heuristic_lookup_table.resize(size, 0.0);
|
||||
// must reset values for non-constructed indices
|
||||
std::fill_n(
|
||||
obstacle_heuristic_lookup_table.begin(), obstacle_size, 0.0);
|
||||
}
|
||||
|
||||
obstacle_heuristic_queue.clear();
|
||||
obstacle_heuristic_queue.reserve(
|
||||
sampled_costmap->getSizeInCellsX() * sampled_costmap->getSizeInCellsY());
|
||||
|
||||
// Set initial goal point to queue from. Divided by 2 due to downsampled costmap.
|
||||
const unsigned int size_x = sampled_costmap->getSizeInCellsX();
|
||||
const unsigned int goal_index = floor(goal_y / 2.0) * size_x + floor(goal_x / 2.0);
|
||||
obstacle_heuristic_queue.emplace_back(
|
||||
distanceHeuristic2D(goal_index, size_x, start_x, start_y), goal_index);
|
||||
|
||||
// initialize goal cell with a very small value to differentiate it from 0.0 (~uninitialized)
|
||||
// the negative value means the cell is in the open set
|
||||
obstacle_heuristic_lookup_table[goal_index] = -0.00001f;
|
||||
}
|
||||
|
||||
float NodeHybrid::getObstacleHeuristic(
|
||||
const Coordinates & node_coords,
|
||||
const Coordinates & goal_coords,
|
||||
const double & cost_penalty)
|
||||
{
|
||||
// If already expanded, return the cost
|
||||
const unsigned int size_x = sampled_costmap->getSizeInCellsX();
|
||||
// Divided by 2 due to downsampled costmap.
|
||||
const unsigned int start_y = floor(node_coords.y / 2.0);
|
||||
const unsigned int start_x = floor(node_coords.x / 2.0);
|
||||
const unsigned int start_index = start_y * size_x + start_x;
|
||||
const float & requested_node_cost = obstacle_heuristic_lookup_table[start_index];
|
||||
if (requested_node_cost > 0.0f) {
|
||||
// costs are doubled due to downsampling
|
||||
return 2.0 * requested_node_cost;
|
||||
}
|
||||
|
||||
// If not, expand until it is included. This dynamic programming ensures that
|
||||
// we only expand the MINIMUM spanning set of the costmap per planning request.
|
||||
// Rather than naively expanding the entire (potentially massive) map for a limited
|
||||
// path, we only expand to the extent required for the furthest expansion in the
|
||||
// search-planning request that dynamically updates during search as needed.
|
||||
|
||||
// start_x and start_y have changed since last call
|
||||
// we need to recompute 2D distance heuristic and reprioritize queue
|
||||
for (auto & n : obstacle_heuristic_queue) {
|
||||
n.first = -obstacle_heuristic_lookup_table[n.second] +
|
||||
distanceHeuristic2D(n.second, size_x, start_x, start_y);
|
||||
}
|
||||
std::make_heap(
|
||||
obstacle_heuristic_queue.begin(), obstacle_heuristic_queue.end(),
|
||||
ObstacleHeuristicComparator{});
|
||||
|
||||
const int size_x_int = static_cast<int>(size_x);
|
||||
const unsigned int size_y = sampled_costmap->getSizeInCellsY();
|
||||
const float sqrt_2 = sqrt(2);
|
||||
float c_cost, cost, travel_cost, new_cost, existing_cost;
|
||||
unsigned int idx, mx, my, mx_idx, my_idx;
|
||||
unsigned int new_idx = 0;
|
||||
|
||||
const std::vector<int> neighborhood = {1, -1, // left right
|
||||
size_x_int, -size_x_int, // up down
|
||||
size_x_int + 1, size_x_int - 1, // upper diagonals
|
||||
-size_x_int + 1, -size_x_int - 1}; // lower diagonals
|
||||
|
||||
while (!obstacle_heuristic_queue.empty()) {
|
||||
idx = obstacle_heuristic_queue.front().second;
|
||||
std::pop_heap(
|
||||
obstacle_heuristic_queue.begin(), obstacle_heuristic_queue.end(),
|
||||
ObstacleHeuristicComparator{});
|
||||
obstacle_heuristic_queue.pop_back();
|
||||
c_cost = obstacle_heuristic_lookup_table[idx];
|
||||
if (c_cost > 0.0f) {
|
||||
// cell has been processed and closed, no further cost improvements
|
||||
// are mathematically possible thanks to euclidean distance heuristic consistency
|
||||
continue;
|
||||
}
|
||||
c_cost = -c_cost;
|
||||
obstacle_heuristic_lookup_table[idx] = c_cost; // set a positive value to close the cell
|
||||
|
||||
my_idx = idx / size_x;
|
||||
mx_idx = idx - (my_idx * size_x);
|
||||
|
||||
// find neighbors
|
||||
for (unsigned int i = 0; i != neighborhood.size(); i++) {
|
||||
new_idx = static_cast<unsigned int>(static_cast<int>(idx) + neighborhood[i]);
|
||||
|
||||
// if neighbor path is better and non-lethal, set new cost and add to queue
|
||||
if (new_idx < size_x * size_y) {
|
||||
cost = static_cast<float>(sampled_costmap->getCost(new_idx));
|
||||
if (cost >= INSCRIBED) {
|
||||
continue;
|
||||
}
|
||||
|
||||
my = new_idx / size_x;
|
||||
mx = new_idx - (my * size_x);
|
||||
|
||||
if (mx == 0 && mx_idx >= size_x - 1 || mx >= size_x - 1 && mx_idx == 0) {
|
||||
continue;
|
||||
}
|
||||
if (my == 0 && my_idx >= size_y - 1 || my >= size_y - 1 && my_idx == 0) {
|
||||
continue;
|
||||
}
|
||||
|
||||
existing_cost = obstacle_heuristic_lookup_table[new_idx];
|
||||
if (existing_cost <= 0.0f) {
|
||||
travel_cost =
|
||||
((i <= 3) ? 1.0f : sqrt_2) * (1.0f + (cost_penalty * cost / 252.0f));
|
||||
new_cost = c_cost + travel_cost;
|
||||
if (existing_cost == 0.0f || -existing_cost > new_cost) {
|
||||
// the negative value means the cell is in the open set
|
||||
obstacle_heuristic_lookup_table[new_idx] = -new_cost;
|
||||
obstacle_heuristic_queue.emplace_back(
|
||||
new_cost + distanceHeuristic2D(new_idx, size_x, start_x, start_y), new_idx);
|
||||
std::push_heap(
|
||||
obstacle_heuristic_queue.begin(), obstacle_heuristic_queue.end(),
|
||||
ObstacleHeuristicComparator{});
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (idx == start_index) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// return requested_node_cost which has been updated by the search
|
||||
// costs are doubled due to downsampling
|
||||
return 2.0 * requested_node_cost;
|
||||
}
|
||||
|
||||
float NodeHybrid::getDistanceHeuristic(
|
||||
const Coordinates & node_coords,
|
||||
const Coordinates & goal_coords,
|
||||
const float & obstacle_heuristic)
|
||||
{
|
||||
// rotate and translate node_coords such that goal_coords relative is (0,0,0)
|
||||
// Due to the rounding involved in exact cell increments for caching,
|
||||
// this is not an exact replica of a live heuristic, but has bounded error.
|
||||
// (Usually less than 1 cell)
|
||||
|
||||
// This angle is negative since we are de-rotating the current node
|
||||
// by the goal angle; cos(-th) = cos(th) & sin(-th) = -sin(th)
|
||||
const TrigValues & trig_vals = motion_table.trig_values[goal_coords.theta];
|
||||
const float cos_th = trig_vals.first;
|
||||
const float sin_th = -trig_vals.second;
|
||||
const float dx = node_coords.x - goal_coords.x;
|
||||
const float dy = node_coords.y - goal_coords.y;
|
||||
|
||||
double dtheta_bin = node_coords.theta - goal_coords.theta;
|
||||
if (dtheta_bin < 0) {
|
||||
dtheta_bin += motion_table.num_angle_quantization;
|
||||
}
|
||||
if (dtheta_bin > motion_table.num_angle_quantization) {
|
||||
dtheta_bin -= motion_table.num_angle_quantization;
|
||||
}
|
||||
|
||||
Coordinates node_coords_relative(
|
||||
round(dx * cos_th - dy * sin_th),
|
||||
round(dx * sin_th + dy * cos_th),
|
||||
round(dtheta_bin));
|
||||
|
||||
// Check if the relative node coordinate is within the localized window around the goal
|
||||
// to apply the distance heuristic. Since the lookup table is contains only the positive
|
||||
// X axis, we mirror the Y and theta values across the X axis to find the heuristic values.
|
||||
float motion_heuristic = 0.0;
|
||||
const int floored_size = floor(size_lookup / 2.0);
|
||||
const int ceiling_size = ceil(size_lookup / 2.0);
|
||||
const float mirrored_relative_y = abs(node_coords_relative.y);
|
||||
if (abs(node_coords_relative.x) < floored_size && mirrored_relative_y < floored_size) {
|
||||
// Need to mirror angle if Y coordinate was mirrored
|
||||
int theta_pos;
|
||||
if (node_coords_relative.y < 0.0) {
|
||||
theta_pos = motion_table.num_angle_quantization - node_coords_relative.theta;
|
||||
} else {
|
||||
theta_pos = node_coords_relative.theta;
|
||||
}
|
||||
const int x_pos = node_coords_relative.x + floored_size;
|
||||
const int y_pos = static_cast<int>(mirrored_relative_y);
|
||||
const int index =
|
||||
x_pos * ceiling_size * motion_table.num_angle_quantization +
|
||||
y_pos * motion_table.num_angle_quantization +
|
||||
theta_pos;
|
||||
motion_heuristic = dist_heuristic_lookup_table[index];
|
||||
} else if (obstacle_heuristic <= 0.0) {
|
||||
// If no obstacle heuristic value, must have some H to use
|
||||
// In nominal situations, this should never be called.
|
||||
static ompl::base::ScopedState<> from(motion_table.state_space), to(motion_table.state_space);
|
||||
to[0] = goal_coords.x;
|
||||
to[1] = goal_coords.y;
|
||||
to[2] = goal_coords.theta * motion_table.num_angle_quantization;
|
||||
from[0] = node_coords.x;
|
||||
from[1] = node_coords.y;
|
||||
from[2] = node_coords.theta * motion_table.num_angle_quantization;
|
||||
motion_heuristic = motion_table.state_space->distance(from(), to());
|
||||
}
|
||||
|
||||
return motion_heuristic;
|
||||
}
|
||||
|
||||
void NodeHybrid::precomputeDistanceHeuristic(
|
||||
const float & lookup_table_dim,
|
||||
const MotionModel & motion_model,
|
||||
const unsigned int & dim_3_size,
|
||||
const SearchInfo & search_info)
|
||||
{
|
||||
// Dubin or Reeds-Shepp shortest distances
|
||||
if (motion_model == MotionModel::DUBIN) {
|
||||
motion_table.state_space = std::make_unique<ompl::base::DubinsStateSpace>(
|
||||
search_info.minimum_turning_radius);
|
||||
} else if (motion_model == MotionModel::REEDS_SHEPP) {
|
||||
motion_table.state_space = std::make_unique<ompl::base::ReedsSheppStateSpace>(
|
||||
search_info.minimum_turning_radius);
|
||||
} else {
|
||||
throw std::runtime_error(
|
||||
"Node attempted to precompute distance heuristics "
|
||||
"with invalid motion model!");
|
||||
}
|
||||
|
||||
ompl::base::ScopedState<> from(motion_table.state_space), to(motion_table.state_space);
|
||||
to[0] = 0.0;
|
||||
to[1] = 0.0;
|
||||
to[2] = 0.0;
|
||||
size_lookup = lookup_table_dim;
|
||||
float motion_heuristic = 0.0;
|
||||
unsigned int index = 0;
|
||||
int dim_3_size_int = static_cast<int>(dim_3_size);
|
||||
float angular_bin_size = 2 * M_PI / static_cast<float>(dim_3_size);
|
||||
|
||||
// Create a lookup table of Dubin/Reeds-Shepp distances in a window around the goal
|
||||
// to help drive the search towards admissible approaches. Deu to symmetries in the
|
||||
// Heuristic space, we need to only store 2 of the 4 quadrants and simply mirror
|
||||
// around the X axis any relative node lookup. This reduces memory overhead and increases
|
||||
// the size of a window a platform can store in memory.
|
||||
dist_heuristic_lookup_table.resize(size_lookup * ceil(size_lookup / 2.0) * dim_3_size_int);
|
||||
for (float x = ceil(-size_lookup / 2.0); x <= floor(size_lookup / 2.0); x += 1.0) {
|
||||
for (float y = 0.0; y <= floor(size_lookup / 2.0); y += 1.0) {
|
||||
for (int heading = 0; heading != dim_3_size_int; heading++) {
|
||||
from[0] = x;
|
||||
from[1] = y;
|
||||
from[2] = heading * angular_bin_size;
|
||||
motion_heuristic = motion_table.state_space->distance(from(), to());
|
||||
dist_heuristic_lookup_table[index] = motion_heuristic;
|
||||
index++;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void NodeHybrid::getNeighbors(
|
||||
std::function<bool(const unsigned int &, nav2_smac_planner::NodeHybrid * &)> & NeighborGetter,
|
||||
GridCollisionChecker * collision_checker,
|
||||
const bool & traverse_unknown,
|
||||
NodeVector & neighbors)
|
||||
{
|
||||
unsigned int index = 0;
|
||||
NodePtr neighbor = nullptr;
|
||||
Coordinates initial_node_coords;
|
||||
const MotionPoses motion_projections = motion_table.getProjections(this);
|
||||
|
||||
for (unsigned int i = 0; i != motion_projections.size(); i++) {
|
||||
index = NodeHybrid::getIndex(
|
||||
static_cast<unsigned int>(motion_projections[i]._x),
|
||||
static_cast<unsigned int>(motion_projections[i]._y),
|
||||
static_cast<unsigned int>(motion_projections[i]._theta),
|
||||
motion_table.size_x, motion_table.num_angle_quantization);
|
||||
|
||||
if (NeighborGetter(index, neighbor) && !neighbor->wasVisited()) {
|
||||
// Cache the initial pose in case it was visited but valid
|
||||
// don't want to disrupt continuous coordinate expansion
|
||||
initial_node_coords = neighbor->pose;
|
||||
neighbor->setPose(
|
||||
Coordinates(
|
||||
motion_projections[i]._x,
|
||||
motion_projections[i]._y,
|
||||
motion_projections[i]._theta));
|
||||
if (neighbor->isNodeValid(traverse_unknown, collision_checker)) {
|
||||
neighbor->setMotionPrimitiveIndex(i);
|
||||
neighbors.push_back(neighbor);
|
||||
} else {
|
||||
neighbor->setPose(initial_node_coords);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool NodeHybrid::backtracePath(CoordinateVector & path)
|
||||
{
|
||||
if (!this->parent) {
|
||||
return false;
|
||||
}
|
||||
|
||||
NodePtr current_node = this;
|
||||
|
||||
while (current_node->parent) {
|
||||
path.push_back(current_node->pose);
|
||||
// Convert angle to radians
|
||||
path.back().theta = NodeHybrid::motion_table.getAngleFromBin(path.back().theta);
|
||||
current_node = current_node->parent;
|
||||
}
|
||||
|
||||
// add the start pose
|
||||
path.push_back(current_node->pose);
|
||||
// Convert angle to radians
|
||||
path.back().theta = NodeHybrid::motion_table.getAngleFromBin(path.back().theta);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
} // namespace nav2_smac_planner
|
||||
@@ -0,0 +1,591 @@
|
||||
// Copyright (c) 2021, 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. Reserved.
|
||||
|
||||
#include <math.h>
|
||||
#include <chrono>
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
#include <algorithm>
|
||||
#include <queue>
|
||||
#include <limits>
|
||||
#include <string>
|
||||
#include <fstream>
|
||||
#include <cmath>
|
||||
|
||||
#include "ompl/base/ScopedState.h"
|
||||
#include "ompl/base/spaces/DubinsStateSpace.h"
|
||||
#include "ompl/base/spaces/ReedsSheppStateSpace.h"
|
||||
|
||||
#include "nav2_smac_planner/node_lattice.hpp"
|
||||
|
||||
using namespace std::chrono; // NOLINT
|
||||
|
||||
namespace nav2_smac_planner
|
||||
{
|
||||
|
||||
// defining static member for all instance to share
|
||||
LatticeMotionTable NodeLattice::motion_table;
|
||||
float NodeLattice::size_lookup = 25;
|
||||
LookupTable NodeLattice::dist_heuristic_lookup_table;
|
||||
|
||||
// Each of these tables are the projected motion models through
|
||||
// time and space applied to the search on the current node in
|
||||
// continuous map-coordinates (e.g. not meters but partial map cells)
|
||||
// Currently, these are set to project *at minimum* into a neighboring
|
||||
// cell. Though this could be later modified to project a certain
|
||||
// amount of time or particular distance forward.
|
||||
void LatticeMotionTable::initMotionModel(
|
||||
unsigned int & size_x_in,
|
||||
SearchInfo & search_info)
|
||||
{
|
||||
size_x = size_x_in;
|
||||
|
||||
if (current_lattice_filepath == search_info.lattice_filepath) {
|
||||
return;
|
||||
}
|
||||
|
||||
size_x = size_x_in;
|
||||
change_penalty = search_info.change_penalty;
|
||||
non_straight_penalty = search_info.non_straight_penalty;
|
||||
cost_penalty = search_info.cost_penalty;
|
||||
reverse_penalty = search_info.reverse_penalty;
|
||||
travel_distance_reward = 1.0f - search_info.retrospective_penalty;
|
||||
current_lattice_filepath = search_info.lattice_filepath;
|
||||
allow_reverse_expansion = search_info.allow_reverse_expansion;
|
||||
rotation_penalty = search_info.rotation_penalty;
|
||||
|
||||
// Get the metadata about this minimum control set
|
||||
lattice_metadata = getLatticeMetadata(current_lattice_filepath);
|
||||
std::ifstream latticeFile(current_lattice_filepath);
|
||||
if (!latticeFile.is_open()) {
|
||||
throw std::runtime_error("Could not open lattice file");
|
||||
}
|
||||
nlohmann::json json;
|
||||
latticeFile >> json;
|
||||
num_angle_quantization = lattice_metadata.number_of_headings;
|
||||
|
||||
if (!state_space) {
|
||||
if (!allow_reverse_expansion) {
|
||||
state_space = std::make_unique<ompl::base::DubinsStateSpace>(
|
||||
lattice_metadata.min_turning_radius);
|
||||
} else {
|
||||
state_space = std::make_unique<ompl::base::ReedsSheppStateSpace>(
|
||||
lattice_metadata.min_turning_radius);
|
||||
}
|
||||
}
|
||||
|
||||
// Populate the motion primitives at each heading angle
|
||||
float prev_start_angle = 0.0;
|
||||
std::vector<MotionPrimitive> primitives;
|
||||
nlohmann::json json_primitives = json["primitives"];
|
||||
for (unsigned int i = 0; i < json_primitives.size(); ++i) {
|
||||
MotionPrimitive new_primitive;
|
||||
fromJsonToMotionPrimitive(json_primitives[i], new_primitive);
|
||||
|
||||
if (prev_start_angle != new_primitive.start_angle) {
|
||||
motion_primitives.push_back(primitives);
|
||||
primitives.clear();
|
||||
prev_start_angle = new_primitive.start_angle;
|
||||
}
|
||||
primitives.push_back(new_primitive);
|
||||
}
|
||||
motion_primitives.push_back(primitives);
|
||||
|
||||
// Populate useful precomputed values to be leveraged
|
||||
trig_values.reserve(lattice_metadata.number_of_headings);
|
||||
for (unsigned int i = 0; i < lattice_metadata.heading_angles.size(); ++i) {
|
||||
trig_values.emplace_back(
|
||||
cos(lattice_metadata.heading_angles[i]),
|
||||
sin(lattice_metadata.heading_angles[i]));
|
||||
}
|
||||
}
|
||||
|
||||
MotionPrimitivePtrs LatticeMotionTable::getMotionPrimitives(const NodeLattice * node)
|
||||
{
|
||||
MotionPrimitives & prims_at_heading = motion_primitives[node->pose.theta];
|
||||
MotionPrimitivePtrs primitive_projection_list;
|
||||
for (unsigned int i = 0; i != prims_at_heading.size(); i++) {
|
||||
primitive_projection_list.push_back(&prims_at_heading[i]);
|
||||
}
|
||||
|
||||
if (allow_reverse_expansion) {
|
||||
// Find normalized heading bin of the reverse expansion
|
||||
double reserve_heading = node->pose.theta - (num_angle_quantization / 2);
|
||||
if (reserve_heading < 0) {
|
||||
reserve_heading += num_angle_quantization;
|
||||
}
|
||||
if (reserve_heading > num_angle_quantization) {
|
||||
reserve_heading -= num_angle_quantization;
|
||||
}
|
||||
|
||||
MotionPrimitives & prims_at_reverse_heading = motion_primitives[reserve_heading];
|
||||
for (unsigned int i = 0; i != prims_at_reverse_heading.size(); i++) {
|
||||
primitive_projection_list.push_back(&prims_at_reverse_heading[i]);
|
||||
}
|
||||
}
|
||||
|
||||
return primitive_projection_list;
|
||||
}
|
||||
|
||||
LatticeMetadata LatticeMotionTable::getLatticeMetadata(const std::string & lattice_filepath)
|
||||
{
|
||||
std::ifstream lattice_file(lattice_filepath);
|
||||
if (!lattice_file.is_open()) {
|
||||
throw std::runtime_error("Could not open lattice file!");
|
||||
}
|
||||
|
||||
nlohmann::json j;
|
||||
lattice_file >> j;
|
||||
LatticeMetadata metadata;
|
||||
fromJsonToMetaData(j["lattice_metadata"], metadata);
|
||||
return metadata;
|
||||
}
|
||||
|
||||
unsigned int LatticeMotionTable::getClosestAngularBin(const double & theta)
|
||||
{
|
||||
float min_dist = std::numeric_limits<float>::max();
|
||||
unsigned int closest_idx = 0;
|
||||
float dist = 0.0;
|
||||
for (unsigned int i = 0; i != lattice_metadata.heading_angles.size(); i++) {
|
||||
dist = fabs(angles::shortest_angular_distance(theta, lattice_metadata.heading_angles[i]));
|
||||
if (dist < min_dist) {
|
||||
min_dist = dist;
|
||||
closest_idx = i;
|
||||
}
|
||||
}
|
||||
return closest_idx;
|
||||
}
|
||||
|
||||
float & LatticeMotionTable::getAngleFromBin(const unsigned int & bin_idx)
|
||||
{
|
||||
return lattice_metadata.heading_angles[bin_idx];
|
||||
}
|
||||
|
||||
NodeLattice::NodeLattice(const unsigned int index)
|
||||
: parent(nullptr),
|
||||
pose(0.0f, 0.0f, 0.0f),
|
||||
_cell_cost(std::numeric_limits<float>::quiet_NaN()),
|
||||
_accumulated_cost(std::numeric_limits<float>::max()),
|
||||
_index(index),
|
||||
_was_visited(false),
|
||||
_motion_primitive(nullptr),
|
||||
_backwards(false)
|
||||
{
|
||||
}
|
||||
|
||||
NodeLattice::~NodeLattice()
|
||||
{
|
||||
parent = nullptr;
|
||||
}
|
||||
|
||||
void NodeLattice::reset()
|
||||
{
|
||||
parent = nullptr;
|
||||
_cell_cost = std::numeric_limits<float>::quiet_NaN();
|
||||
_accumulated_cost = std::numeric_limits<float>::max();
|
||||
_was_visited = false;
|
||||
pose.x = 0.0f;
|
||||
pose.y = 0.0f;
|
||||
pose.theta = 0.0f;
|
||||
_motion_primitive = nullptr;
|
||||
_backwards = false;
|
||||
}
|
||||
|
||||
bool NodeLattice::isNodeValid(
|
||||
const bool & traverse_unknown,
|
||||
GridCollisionChecker * collision_checker,
|
||||
MotionPrimitive * motion_primitive,
|
||||
bool is_backwards)
|
||||
{
|
||||
// Check primitive end pose
|
||||
// Convert grid quantization of primitives to radians, then collision checker quantization
|
||||
static const double bin_size = 2.0 * M_PI / collision_checker->getPrecomputedAngles().size();
|
||||
const double & angle = motion_table.getAngleFromBin(this->pose.theta) / bin_size;
|
||||
if (collision_checker->inCollision(
|
||||
this->pose.x, this->pose.y, angle /*bin in collision checker*/, traverse_unknown))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
// Set the cost of a node to the highest cost across the primitive
|
||||
float max_cell_cost = collision_checker->getCost();
|
||||
|
||||
// If valid motion primitives are set, check intermediary poses > 1 cell apart
|
||||
if (motion_primitive) {
|
||||
const float & grid_resolution = motion_table.lattice_metadata.grid_resolution;
|
||||
const float & resolution_diag_sq = 2.0 * grid_resolution * grid_resolution;
|
||||
MotionPose last_pose(1e9, 1e9, 1e9), pose_dist(0.0, 0.0, 0.0);
|
||||
|
||||
// Back out the initial node starting point to move motion primitive relative to
|
||||
MotionPose initial_pose, prim_pose;
|
||||
initial_pose._x = this->pose.x - (motion_primitive->poses.back()._x / grid_resolution);
|
||||
initial_pose._y = this->pose.y - (motion_primitive->poses.back()._y / grid_resolution);
|
||||
initial_pose._theta = motion_table.getAngleFromBin(motion_primitive->start_angle);
|
||||
|
||||
for (auto it = motion_primitive->poses.begin(); it != motion_primitive->poses.end(); ++it) {
|
||||
// poses are in metric coordinates from (0, 0), not grid space yet
|
||||
pose_dist = *it - last_pose;
|
||||
// Avoid square roots by (hypot(x, y) > res) == (x*x+y*y > diag*diag)
|
||||
if (pose_dist._x * pose_dist._x + pose_dist._y * pose_dist._y > resolution_diag_sq) {
|
||||
last_pose = *it;
|
||||
// Convert primitive pose into grid space if it should be checked
|
||||
prim_pose._x = initial_pose._x + (it->_x / grid_resolution);
|
||||
prim_pose._y = initial_pose._y + (it->_y / grid_resolution);
|
||||
// If reversing, invert the angle because the robot is backing into the primitive
|
||||
// not driving forward with it
|
||||
if (is_backwards) {
|
||||
prim_pose._theta = std::fmod(it->_theta + M_PI, 2.0 * M_PI);
|
||||
} else {
|
||||
prim_pose._theta = it->_theta;
|
||||
}
|
||||
if (collision_checker->inCollision(
|
||||
prim_pose._x,
|
||||
prim_pose._y,
|
||||
prim_pose._theta / bin_size /*bin in collision checker*/,
|
||||
traverse_unknown))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
max_cell_cost = std::max(max_cell_cost, collision_checker->getCost());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
_cell_cost = max_cell_cost;
|
||||
return true;
|
||||
}
|
||||
|
||||
float NodeLattice::getTraversalCost(const NodePtr & child)
|
||||
{
|
||||
const float normalized_cost = child->getCost() / 252.0;
|
||||
if (std::isnan(normalized_cost)) {
|
||||
throw std::runtime_error(
|
||||
"Node attempted to get traversal "
|
||||
"cost without a known collision cost!");
|
||||
}
|
||||
|
||||
// this is the first node
|
||||
MotionPrimitive * prim = this->getMotionPrimitive();
|
||||
MotionPrimitive * transition_prim = child->getMotionPrimitive();
|
||||
const float prim_length =
|
||||
transition_prim->trajectory_length / motion_table.lattice_metadata.grid_resolution;
|
||||
if (prim == nullptr) {
|
||||
return prim_length;
|
||||
}
|
||||
|
||||
// Pure rotation in place 1 angular bin in either direction
|
||||
if (transition_prim->trajectory_length < 1e-4) {
|
||||
return motion_table.rotation_penalty * (1.0 + motion_table.cost_penalty * normalized_cost);
|
||||
}
|
||||
|
||||
float travel_cost = 0.0;
|
||||
float travel_cost_raw = prim_length *
|
||||
(motion_table.travel_distance_reward + motion_table.cost_penalty * normalized_cost);
|
||||
|
||||
if (transition_prim->arc_length < 0.001) {
|
||||
// New motion is a straight motion, no additional costs to be applied
|
||||
travel_cost = travel_cost_raw;
|
||||
} else {
|
||||
if (prim->left_turn == transition_prim->left_turn) {
|
||||
// Turning motion but keeps in same general direction: encourages to commit to actions
|
||||
travel_cost = travel_cost_raw * motion_table.non_straight_penalty;
|
||||
} else {
|
||||
// Turning motion and velocity directions: penalizes wiggling.
|
||||
travel_cost = travel_cost_raw *
|
||||
(motion_table.non_straight_penalty + motion_table.change_penalty);
|
||||
}
|
||||
}
|
||||
|
||||
// If backwards flag is set, this primitive is moving in reverse
|
||||
if (child->isBackward()) {
|
||||
// reverse direction
|
||||
travel_cost *= motion_table.reverse_penalty;
|
||||
}
|
||||
|
||||
return travel_cost;
|
||||
}
|
||||
|
||||
float NodeLattice::getHeuristicCost(
|
||||
const Coordinates & node_coords,
|
||||
const Coordinates & goal_coords,
|
||||
const nav2_costmap_2d::Costmap2D * costmap)
|
||||
{
|
||||
// get obstacle heuristic value
|
||||
const float obstacle_heuristic = getObstacleHeuristic(
|
||||
node_coords, goal_coords, motion_table.cost_penalty);
|
||||
const float distance_heuristic =
|
||||
getDistanceHeuristic(node_coords, goal_coords, obstacle_heuristic);
|
||||
return std::max(obstacle_heuristic, distance_heuristic);
|
||||
}
|
||||
|
||||
void NodeLattice::initMotionModel(
|
||||
const MotionModel & motion_model,
|
||||
unsigned int & size_x,
|
||||
unsigned int & /*size_y*/,
|
||||
unsigned int & /*num_angle_quantization*/,
|
||||
SearchInfo & search_info)
|
||||
{
|
||||
if (motion_model != MotionModel::STATE_LATTICE) {
|
||||
throw std::runtime_error(
|
||||
"Invalid motion model for Lattice node. Please select"
|
||||
" STATE_LATTICE and provide a valid lattice file.");
|
||||
}
|
||||
|
||||
motion_table.initMotionModel(size_x, search_info);
|
||||
}
|
||||
|
||||
float NodeLattice::getDistanceHeuristic(
|
||||
const Coordinates & node_coords,
|
||||
const Coordinates & goal_coords,
|
||||
const float & obstacle_heuristic)
|
||||
{
|
||||
// rotate and translate node_coords such that goal_coords relative is (0,0,0)
|
||||
// Due to the rounding involved in exact cell increments for caching,
|
||||
// this is not an exact replica of a live heuristic, but has bounded error.
|
||||
// (Usually less than 1 cell length)
|
||||
|
||||
// This angle is negative since we are de-rotating the current node
|
||||
// by the goal angle; cos(-th) = cos(th) & sin(-th) = -sin(th)
|
||||
const TrigValues & trig_vals = motion_table.trig_values[goal_coords.theta];
|
||||
const float cos_th = trig_vals.first;
|
||||
const float sin_th = -trig_vals.second;
|
||||
const float dx = node_coords.x - goal_coords.x;
|
||||
const float dy = node_coords.y - goal_coords.y;
|
||||
|
||||
double dtheta_bin = node_coords.theta - goal_coords.theta;
|
||||
if (dtheta_bin < 0) {
|
||||
dtheta_bin += motion_table.num_angle_quantization;
|
||||
}
|
||||
if (dtheta_bin > motion_table.num_angle_quantization) {
|
||||
dtheta_bin -= motion_table.num_angle_quantization;
|
||||
}
|
||||
|
||||
Coordinates node_coords_relative(
|
||||
round(dx * cos_th - dy * sin_th),
|
||||
round(dx * sin_th + dy * cos_th),
|
||||
round(dtheta_bin));
|
||||
|
||||
// Check if the relative node coordinate is within the localized window around the goal
|
||||
// to apply the distance heuristic. Since the lookup table is contains only the positive
|
||||
// X axis, we mirror the Y and theta values across the X axis to find the heuristic values.
|
||||
float motion_heuristic = 0.0;
|
||||
const int floored_size = floor(size_lookup / 2.0);
|
||||
const int ceiling_size = ceil(size_lookup / 2.0);
|
||||
const float mirrored_relative_y = abs(node_coords_relative.y);
|
||||
if (abs(node_coords_relative.x) < floored_size && mirrored_relative_y < floored_size) {
|
||||
// Need to mirror angle if Y coordinate was mirrored
|
||||
int theta_pos;
|
||||
if (node_coords_relative.y < 0.0) {
|
||||
theta_pos = motion_table.num_angle_quantization - node_coords_relative.theta;
|
||||
} else {
|
||||
theta_pos = node_coords_relative.theta;
|
||||
}
|
||||
const int x_pos = node_coords_relative.x + floored_size;
|
||||
const int y_pos = static_cast<int>(mirrored_relative_y);
|
||||
const int index =
|
||||
x_pos * ceiling_size * motion_table.num_angle_quantization +
|
||||
y_pos * motion_table.num_angle_quantization +
|
||||
theta_pos;
|
||||
motion_heuristic = dist_heuristic_lookup_table[index];
|
||||
} else if (obstacle_heuristic == 0.0) {
|
||||
static ompl::base::ScopedState<> from(motion_table.state_space), to(motion_table.state_space);
|
||||
to[0] = goal_coords.x;
|
||||
to[1] = goal_coords.y;
|
||||
to[2] = motion_table.getAngleFromBin(goal_coords.theta);
|
||||
from[0] = node_coords.x;
|
||||
from[1] = node_coords.y;
|
||||
from[2] = motion_table.getAngleFromBin(node_coords.theta);
|
||||
motion_heuristic = motion_table.state_space->distance(from(), to());
|
||||
}
|
||||
|
||||
return motion_heuristic;
|
||||
}
|
||||
|
||||
void NodeLattice::precomputeDistanceHeuristic(
|
||||
const float & lookup_table_dim,
|
||||
const MotionModel & motion_model,
|
||||
const unsigned int & dim_3_size,
|
||||
const SearchInfo & search_info)
|
||||
{
|
||||
// Dubin or Reeds-Shepp shortest distances
|
||||
if (!search_info.allow_reverse_expansion) {
|
||||
motion_table.state_space = std::make_unique<ompl::base::DubinsStateSpace>(
|
||||
search_info.minimum_turning_radius);
|
||||
} else {
|
||||
motion_table.state_space = std::make_unique<ompl::base::ReedsSheppStateSpace>(
|
||||
search_info.minimum_turning_radius);
|
||||
}
|
||||
motion_table.lattice_metadata =
|
||||
LatticeMotionTable::getLatticeMetadata(search_info.lattice_filepath);
|
||||
|
||||
ompl::base::ScopedState<> from(motion_table.state_space), to(motion_table.state_space);
|
||||
to[0] = 0.0;
|
||||
to[1] = 0.0;
|
||||
to[2] = 0.0;
|
||||
size_lookup = lookup_table_dim;
|
||||
float motion_heuristic = 0.0;
|
||||
unsigned int index = 0;
|
||||
int dim_3_size_int = static_cast<int>(dim_3_size);
|
||||
|
||||
// Create a lookup table of Dubin/Reeds-Shepp distances in a window around the goal
|
||||
// to help drive the search towards admissible approaches. Deu to symmetries in the
|
||||
// Heuristic space, we need to only store 2 of the 4 quadrants and simply mirror
|
||||
// around the X axis any relative node lookup. This reduces memory overhead and increases
|
||||
// the size of a window a platform can store in memory.
|
||||
dist_heuristic_lookup_table.resize(size_lookup * ceil(size_lookup / 2.0) * dim_3_size_int);
|
||||
for (float x = ceil(-size_lookup / 2.0); x <= floor(size_lookup / 2.0); x += 1.0) {
|
||||
for (float y = 0.0; y <= floor(size_lookup / 2.0); y += 1.0) {
|
||||
for (int heading = 0; heading != dim_3_size_int; heading++) {
|
||||
from[0] = x;
|
||||
from[1] = y;
|
||||
from[2] = motion_table.getAngleFromBin(heading);
|
||||
motion_heuristic = motion_table.state_space->distance(from(), to());
|
||||
dist_heuristic_lookup_table[index] = motion_heuristic;
|
||||
index++;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void NodeLattice::getNeighbors(
|
||||
std::function<bool(const unsigned int &, nav2_smac_planner::NodeLattice * &)> & NeighborGetter,
|
||||
GridCollisionChecker * collision_checker,
|
||||
const bool & traverse_unknown,
|
||||
NodeVector & neighbors)
|
||||
{
|
||||
unsigned int index = 0;
|
||||
bool backwards = false;
|
||||
NodePtr neighbor = nullptr;
|
||||
Coordinates initial_node_coords, motion_projection;
|
||||
MotionPrimitivePtrs motion_primitives = motion_table.getMotionPrimitives(this);
|
||||
const float & grid_resolution = motion_table.lattice_metadata.grid_resolution;
|
||||
|
||||
unsigned int direction_change_idx = 1e9;
|
||||
for (unsigned int i = 0; i != motion_primitives.size(); i++) {
|
||||
if (motion_primitives[0]->start_angle != motion_primitives[i]->start_angle) {
|
||||
direction_change_idx = i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
for (unsigned int i = 0; i != motion_primitives.size(); i++) {
|
||||
const MotionPose & end_pose = motion_primitives[i]->poses.back();
|
||||
motion_projection.x = this->pose.x + (end_pose._x / grid_resolution);
|
||||
motion_projection.y = this->pose.y + (end_pose._y / grid_resolution);
|
||||
motion_projection.theta = motion_primitives[i]->end_angle /*this is the ending angular bin*/;
|
||||
|
||||
// if i >= idx, then we're in a reversing primitive. In that situation,
|
||||
// the orientation of the robot is mirrored from what it would otherwise
|
||||
// appear to be from the motion primitives file. We want to take this into
|
||||
// account in case the robot base footprint is asymmetric.
|
||||
backwards = false;
|
||||
if (i >= direction_change_idx) {
|
||||
backwards = true;
|
||||
float opposite_heading_theta =
|
||||
motion_projection.theta - (motion_table.num_angle_quantization / 2);
|
||||
if (opposite_heading_theta < 0) {
|
||||
opposite_heading_theta += motion_table.num_angle_quantization;
|
||||
}
|
||||
if (opposite_heading_theta > motion_table.num_angle_quantization) {
|
||||
opposite_heading_theta -= motion_table.num_angle_quantization;
|
||||
}
|
||||
motion_projection.theta = opposite_heading_theta;
|
||||
}
|
||||
|
||||
index = NodeLattice::getIndex(
|
||||
static_cast<unsigned int>(motion_projection.x),
|
||||
static_cast<unsigned int>(motion_projection.y),
|
||||
static_cast<unsigned int>(motion_projection.theta));
|
||||
|
||||
if (NeighborGetter(index, neighbor) && !neighbor->wasVisited()) {
|
||||
// Cache the initial pose in case it was visited but valid
|
||||
// don't want to disrupt continuous coordinate expansion
|
||||
initial_node_coords = neighbor->pose;
|
||||
neighbor->setPose(
|
||||
Coordinates(
|
||||
motion_projection.x,
|
||||
motion_projection.y,
|
||||
motion_projection.theta));
|
||||
|
||||
// Using a special isNodeValid API here, giving the motion primitive to use to
|
||||
// validity check the transition of the current node to the new node over
|
||||
if (neighbor->isNodeValid(
|
||||
traverse_unknown, collision_checker, motion_primitives[i], backwards))
|
||||
{
|
||||
neighbor->setMotionPrimitive(motion_primitives[i]);
|
||||
// Marking if this search was obtained in the reverse direction
|
||||
neighbor->backwards(backwards);
|
||||
neighbors.push_back(neighbor);
|
||||
} else {
|
||||
neighbor->setPose(initial_node_coords);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool NodeLattice::backtracePath(CoordinateVector & path)
|
||||
{
|
||||
if (!this->parent) {
|
||||
return false;
|
||||
}
|
||||
|
||||
NodePtr current_node = this;
|
||||
|
||||
while (current_node->parent) {
|
||||
addNodeToPath(current_node, path);
|
||||
current_node = current_node->parent;
|
||||
}
|
||||
|
||||
// add start to path
|
||||
addNodeToPath(current_node, path);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void NodeLattice::addNodeToPath(
|
||||
NodeLattice::NodePtr current_node,
|
||||
NodeLattice::CoordinateVector & path)
|
||||
{
|
||||
Coordinates initial_pose, prim_pose;
|
||||
MotionPrimitive * prim = nullptr;
|
||||
const float & grid_resolution = NodeLattice::motion_table.lattice_metadata.grid_resolution;
|
||||
prim = current_node->getMotionPrimitive();
|
||||
// if motion primitive is valid, then was searched (rather than analytically expanded),
|
||||
// include dense path of subpoints making up the primitive at grid resolution
|
||||
if (prim) {
|
||||
initial_pose.x = current_node->pose.x - (prim->poses.back()._x / grid_resolution);
|
||||
initial_pose.y = current_node->pose.y - (prim->poses.back()._y / grid_resolution);
|
||||
initial_pose.theta = NodeLattice::motion_table.getAngleFromBin(prim->start_angle);
|
||||
|
||||
for (auto it = prim->poses.crbegin(); it != prim->poses.crend(); ++it) {
|
||||
// Convert primitive pose into grid space if it should be checked
|
||||
prim_pose.x = initial_pose.x + (it->_x / grid_resolution);
|
||||
prim_pose.y = initial_pose.y + (it->_y / grid_resolution);
|
||||
// If reversing, invert the angle because the robot is backing into the primitive
|
||||
// not driving forward with it
|
||||
if (current_node->isBackward()) {
|
||||
prim_pose.theta = std::fmod(it->_theta + M_PI, 2.0 * M_PI);
|
||||
} else {
|
||||
prim_pose.theta = it->_theta;
|
||||
}
|
||||
path.push_back(prim_pose);
|
||||
}
|
||||
} else {
|
||||
// For analytic expansion nodes where there is no valid motion primitive
|
||||
path.push_back(current_node->pose);
|
||||
path.back().theta = NodeLattice::motion_table.getAngleFromBin(path.back().theta);
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace nav2_smac_planner
|
||||
@@ -0,0 +1,419 @@
|
||||
// Copyright (c) 2020, 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. Reserved.
|
||||
|
||||
#include <string>
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
#include <limits>
|
||||
#include <algorithm>
|
||||
|
||||
#include "nav2_smac_planner/smac_planner_2d.hpp"
|
||||
#include "nav2_util/geometry_utils.hpp"
|
||||
|
||||
// #define BENCHMARK_TESTING
|
||||
|
||||
namespace nav2_smac_planner
|
||||
{
|
||||
using namespace std::chrono; // NOLINT
|
||||
using rcl_interfaces::msg::ParameterType;
|
||||
using std::placeholders::_1;
|
||||
|
||||
SmacPlanner2D::SmacPlanner2D()
|
||||
: _a_star(nullptr),
|
||||
_collision_checker(nullptr, 1, nullptr),
|
||||
_smoother(nullptr),
|
||||
_costmap(nullptr),
|
||||
_costmap_downsampler(nullptr)
|
||||
{
|
||||
}
|
||||
|
||||
SmacPlanner2D::~SmacPlanner2D()
|
||||
{
|
||||
RCLCPP_INFO(
|
||||
_logger, "Destroying plugin %s of type SmacPlanner2D",
|
||||
_name.c_str());
|
||||
}
|
||||
|
||||
void SmacPlanner2D::configure(
|
||||
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
|
||||
std::string name, std::shared_ptr<tf2_ros::Buffer>/*tf*/,
|
||||
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
|
||||
{
|
||||
_node = parent;
|
||||
auto node = parent.lock();
|
||||
_logger = node->get_logger();
|
||||
_clock = node->get_clock();
|
||||
_costmap = costmap_ros->getCostmap();
|
||||
_name = name;
|
||||
_global_frame = costmap_ros->getGlobalFrameID();
|
||||
|
||||
RCLCPP_INFO(_logger, "Configuring %s of type SmacPlanner2D", name.c_str());
|
||||
|
||||
// General planner params
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, name + ".tolerance", rclcpp::ParameterValue(0.125));
|
||||
_tolerance = static_cast<float>(node->get_parameter(name + ".tolerance").as_double());
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, name + ".downsample_costmap", rclcpp::ParameterValue(false));
|
||||
node->get_parameter(name + ".downsample_costmap", _downsample_costmap);
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, name + ".downsampling_factor", rclcpp::ParameterValue(1));
|
||||
node->get_parameter(name + ".downsampling_factor", _downsampling_factor);
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, name + ".cost_travel_multiplier", rclcpp::ParameterValue(1.0));
|
||||
node->get_parameter(name + ".cost_travel_multiplier", _search_info.cost_penalty);
|
||||
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, name + ".allow_unknown", rclcpp::ParameterValue(true));
|
||||
node->get_parameter(name + ".allow_unknown", _allow_unknown);
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, name + ".max_iterations", rclcpp::ParameterValue(1000000));
|
||||
node->get_parameter(name + ".max_iterations", _max_iterations);
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, name + ".max_on_approach_iterations", rclcpp::ParameterValue(1000));
|
||||
node->get_parameter(name + ".max_on_approach_iterations", _max_on_approach_iterations);
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, name + ".use_final_approach_orientation", rclcpp::ParameterValue(false));
|
||||
node->get_parameter(name + ".use_final_approach_orientation", _use_final_approach_orientation);
|
||||
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, name + ".max_planning_time", rclcpp::ParameterValue(2.0));
|
||||
node->get_parameter(name + ".max_planning_time", _max_planning_time);
|
||||
|
||||
_motion_model = MotionModel::TWOD;
|
||||
|
||||
if (_max_on_approach_iterations <= 0) {
|
||||
RCLCPP_INFO(
|
||||
_logger, "On approach iteration selected as <= 0, "
|
||||
"disabling tolerance and on approach iterations.");
|
||||
_max_on_approach_iterations = std::numeric_limits<int>::max();
|
||||
}
|
||||
|
||||
if (_max_iterations <= 0) {
|
||||
RCLCPP_INFO(
|
||||
_logger, "maximum iteration selected as <= 0, "
|
||||
"disabling maximum iterations.");
|
||||
_max_iterations = std::numeric_limits<int>::max();
|
||||
}
|
||||
|
||||
// Initialize collision checker
|
||||
_collision_checker = GridCollisionChecker(_costmap, 1 /*for 2D, most be 1*/, node);
|
||||
_collision_checker.setFootprint(
|
||||
costmap_ros->getRobotFootprint(),
|
||||
true /*for 2D, most use radius*/,
|
||||
0.0 /*for 2D cost at inscribed isn't relevent*/);
|
||||
|
||||
// Initialize A* template
|
||||
_a_star = std::make_unique<AStarAlgorithm<Node2D>>(_motion_model, _search_info);
|
||||
_a_star->initialize(
|
||||
_allow_unknown,
|
||||
_max_iterations,
|
||||
_max_on_approach_iterations,
|
||||
_max_planning_time,
|
||||
0.0 /*unused for 2D*/,
|
||||
1.0 /*unused for 2D*/);
|
||||
|
||||
// Initialize path smoother
|
||||
SmootherParams params;
|
||||
params.get(node, name);
|
||||
params.holonomic_ = true; // So smoother will treat this as a grid search
|
||||
_smoother = std::make_unique<Smoother>(params);
|
||||
_smoother->initialize(1e-50 /*No valid minimum turning radius for 2D*/);
|
||||
|
||||
// Initialize costmap downsampler
|
||||
if (_downsample_costmap && _downsampling_factor > 1) {
|
||||
std::string topic_name = "downsampled_costmap";
|
||||
_costmap_downsampler = std::make_unique<CostmapDownsampler>();
|
||||
_costmap_downsampler->on_configure(
|
||||
node, _global_frame, topic_name, _costmap, _downsampling_factor);
|
||||
}
|
||||
|
||||
_raw_plan_publisher = node->create_publisher<nav_msgs::msg::Path>("unsmoothed_plan", 1);
|
||||
|
||||
RCLCPP_INFO(
|
||||
_logger, "Configured plugin %s of type SmacPlanner2D with "
|
||||
"tolerance %.2f, maximum iterations %i, "
|
||||
"max on approach iterations %i, and %s.",
|
||||
_name.c_str(), _tolerance, _max_iterations, _max_on_approach_iterations,
|
||||
_allow_unknown ? "allowing unknown traversal" : "not allowing unknown traversal");
|
||||
}
|
||||
|
||||
void SmacPlanner2D::activate()
|
||||
{
|
||||
RCLCPP_INFO(
|
||||
_logger, "Activating plugin %s of type SmacPlanner2D",
|
||||
_name.c_str());
|
||||
_raw_plan_publisher->on_activate();
|
||||
if (_costmap_downsampler) {
|
||||
_costmap_downsampler->on_activate();
|
||||
}
|
||||
auto node = _node.lock();
|
||||
// Add callback for dynamic parameters
|
||||
_dyn_params_handler = node->add_on_set_parameters_callback(
|
||||
std::bind(&SmacPlanner2D::dynamicParametersCallback, this, _1));
|
||||
}
|
||||
|
||||
void SmacPlanner2D::deactivate()
|
||||
{
|
||||
RCLCPP_INFO(
|
||||
_logger, "Deactivating plugin %s of type SmacPlanner2D",
|
||||
_name.c_str());
|
||||
_raw_plan_publisher->on_deactivate();
|
||||
if (_costmap_downsampler) {
|
||||
_costmap_downsampler->on_deactivate();
|
||||
}
|
||||
_dyn_params_handler.reset();
|
||||
}
|
||||
|
||||
void SmacPlanner2D::cleanup()
|
||||
{
|
||||
RCLCPP_INFO(
|
||||
_logger, "Cleaning up plugin %s of type SmacPlanner2D",
|
||||
_name.c_str());
|
||||
_a_star.reset();
|
||||
_smoother.reset();
|
||||
if (_costmap_downsampler) {
|
||||
_costmap_downsampler->on_cleanup();
|
||||
_costmap_downsampler.reset();
|
||||
}
|
||||
_raw_plan_publisher.reset();
|
||||
}
|
||||
|
||||
nav_msgs::msg::Path SmacPlanner2D::createPlan(
|
||||
const geometry_msgs::msg::PoseStamped & start,
|
||||
const geometry_msgs::msg::PoseStamped & goal)
|
||||
{
|
||||
std::lock_guard<std::mutex> lock_reinit(_mutex);
|
||||
steady_clock::time_point a = steady_clock::now();
|
||||
|
||||
std::unique_lock<nav2_costmap_2d::Costmap2D::mutex_t> lock(*(_costmap->getMutex()));
|
||||
|
||||
// Downsample costmap, if required
|
||||
nav2_costmap_2d::Costmap2D * costmap = _costmap;
|
||||
if (_costmap_downsampler) {
|
||||
costmap = _costmap_downsampler->downsample(_downsampling_factor);
|
||||
_collision_checker.setCostmap(costmap);
|
||||
}
|
||||
|
||||
// Set collision checker and costmap information
|
||||
_a_star->setCollisionChecker(&_collision_checker);
|
||||
|
||||
// Set starting point
|
||||
unsigned int mx_start, my_start, mx_goal, my_goal;
|
||||
costmap->worldToMap(start.pose.position.x, start.pose.position.y, mx_start, my_start);
|
||||
_a_star->setStart(mx_start, my_start, 0);
|
||||
|
||||
// Set goal point
|
||||
costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx_goal, my_goal);
|
||||
_a_star->setGoal(mx_goal, my_goal, 0);
|
||||
|
||||
// Setup message
|
||||
nav_msgs::msg::Path plan;
|
||||
plan.header.stamp = _clock->now();
|
||||
plan.header.frame_id = _global_frame;
|
||||
geometry_msgs::msg::PoseStamped pose;
|
||||
pose.header = plan.header;
|
||||
pose.pose.position.z = 0.0;
|
||||
pose.pose.orientation.x = 0.0;
|
||||
pose.pose.orientation.y = 0.0;
|
||||
pose.pose.orientation.z = 0.0;
|
||||
pose.pose.orientation.w = 1.0;
|
||||
|
||||
// Corner case of start and goal beeing on the same cell
|
||||
if (mx_start == mx_goal && my_start == my_goal) {
|
||||
if (costmap->getCost(mx_start, my_start) == nav2_costmap_2d::LETHAL_OBSTACLE) {
|
||||
RCLCPP_WARN(_logger, "Failed to create a unique pose path because of obstacles");
|
||||
return plan;
|
||||
}
|
||||
pose.pose = start.pose;
|
||||
// if we have a different start and goal orientation, set the unique path pose to the goal
|
||||
// orientation, unless use_final_approach_orientation=true where we need it to be the start
|
||||
// orientation to avoid movement from the local planner
|
||||
if (start.pose.orientation != goal.pose.orientation && !_use_final_approach_orientation) {
|
||||
pose.pose.orientation = goal.pose.orientation;
|
||||
}
|
||||
plan.poses.push_back(pose);
|
||||
return plan;
|
||||
}
|
||||
|
||||
// Compute plan
|
||||
Node2D::CoordinateVector path;
|
||||
int num_iterations = 0;
|
||||
std::string error;
|
||||
try {
|
||||
if (!_a_star->createPath(
|
||||
path, num_iterations, _tolerance / static_cast<float>(costmap->getResolution())))
|
||||
{
|
||||
if (num_iterations < _a_star->getMaxIterations()) {
|
||||
error = std::string("no valid path found");
|
||||
} else {
|
||||
error = std::string("exceeded maximum iterations");
|
||||
}
|
||||
}
|
||||
} catch (const std::runtime_error & e) {
|
||||
error = "invalid use: ";
|
||||
error += e.what();
|
||||
}
|
||||
|
||||
if (!error.empty()) {
|
||||
RCLCPP_WARN(
|
||||
_logger,
|
||||
"%s: failed to create plan, %s.",
|
||||
_name.c_str(), error.c_str());
|
||||
return plan;
|
||||
}
|
||||
|
||||
// Convert to world coordinates
|
||||
plan.poses.reserve(path.size());
|
||||
for (int i = path.size() - 1; i >= 0; --i) {
|
||||
pose.pose = getWorldCoords(path[i].x, path[i].y, costmap);
|
||||
plan.poses.push_back(pose);
|
||||
}
|
||||
|
||||
// Publish raw path for debug
|
||||
if (_raw_plan_publisher->get_subscription_count() > 0) {
|
||||
_raw_plan_publisher->publish(plan);
|
||||
}
|
||||
|
||||
// Find how much time we have left to do smoothing
|
||||
steady_clock::time_point b = steady_clock::now();
|
||||
duration<double> time_span = duration_cast<duration<double>>(b - a);
|
||||
double time_remaining = _max_planning_time - static_cast<double>(time_span.count());
|
||||
|
||||
#ifdef BENCHMARK_TESTING
|
||||
std::cout << "It took " << time_span.count() * 1000 <<
|
||||
" milliseconds with " << num_iterations << " iterations." << std::endl;
|
||||
#endif
|
||||
|
||||
// Smooth plan
|
||||
_smoother->smooth(plan, costmap, time_remaining);
|
||||
|
||||
// If use_final_approach_orientation=true, interpolate the last pose orientation from the
|
||||
// previous pose to set the orientation to the 'final approach' orientation of the robot so
|
||||
// it does not rotate.
|
||||
// And deal with corner case of plan of length 1
|
||||
// If use_final_approach_orientation=false (default), override last pose orientation to match goal
|
||||
size_t plan_size = plan.poses.size();
|
||||
if (_use_final_approach_orientation) {
|
||||
if (plan_size == 1) {
|
||||
plan.poses.back().pose.orientation = start.pose.orientation;
|
||||
} else if (plan_size > 1) {
|
||||
double dx, dy, theta;
|
||||
auto last_pose = plan.poses.back().pose.position;
|
||||
auto approach_pose = plan.poses[plan_size - 2].pose.position;
|
||||
dx = last_pose.x - approach_pose.x;
|
||||
dy = last_pose.y - approach_pose.y;
|
||||
theta = atan2(dy, dx);
|
||||
plan.poses.back().pose.orientation =
|
||||
nav2_util::geometry_utils::orientationAroundZAxis(theta);
|
||||
}
|
||||
} else if (plan_size > 0) {
|
||||
plan.poses.back().pose.orientation = goal.pose.orientation;
|
||||
}
|
||||
|
||||
return plan;
|
||||
}
|
||||
|
||||
rcl_interfaces::msg::SetParametersResult
|
||||
SmacPlanner2D::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters)
|
||||
{
|
||||
rcl_interfaces::msg::SetParametersResult result;
|
||||
std::lock_guard<std::mutex> lock_reinit(_mutex);
|
||||
|
||||
bool reinit_a_star = false;
|
||||
bool reinit_downsampler = false;
|
||||
|
||||
for (auto parameter : parameters) {
|
||||
const auto & type = parameter.get_type();
|
||||
const auto & name = parameter.get_name();
|
||||
|
||||
if (type == ParameterType::PARAMETER_DOUBLE) {
|
||||
if (name == _name + ".tolerance") {
|
||||
_tolerance = static_cast<float>(parameter.as_double());
|
||||
} else if (name == _name + ".cost_travel_multiplier") {
|
||||
reinit_a_star = true;
|
||||
_search_info.cost_penalty = parameter.as_double();
|
||||
} else if (name == _name + ".max_planning_time") {
|
||||
reinit_a_star = true;
|
||||
_max_planning_time = parameter.as_double();
|
||||
}
|
||||
} else if (type == ParameterType::PARAMETER_BOOL) {
|
||||
if (name == _name + ".downsample_costmap") {
|
||||
reinit_downsampler = true;
|
||||
_downsample_costmap = parameter.as_bool();
|
||||
} else if (name == _name + ".allow_unknown") {
|
||||
reinit_a_star = true;
|
||||
_allow_unknown = parameter.as_bool();
|
||||
} else if (name == _name + ".use_final_approach_orientation") {
|
||||
_use_final_approach_orientation = parameter.as_bool();
|
||||
}
|
||||
} else if (type == ParameterType::PARAMETER_INTEGER) {
|
||||
if (name == _name + ".downsampling_factor") {
|
||||
reinit_downsampler = true;
|
||||
_downsampling_factor = parameter.as_int();
|
||||
} else if (name == _name + ".max_iterations") {
|
||||
reinit_a_star = true;
|
||||
_max_iterations = parameter.as_int();
|
||||
if (_max_iterations <= 0) {
|
||||
RCLCPP_INFO(
|
||||
_logger, "maximum iteration selected as <= 0, "
|
||||
"disabling maximum iterations.");
|
||||
_max_iterations = std::numeric_limits<int>::max();
|
||||
}
|
||||
} else if (name == _name + ".max_on_approach_iterations") {
|
||||
reinit_a_star = true;
|
||||
_max_on_approach_iterations = parameter.as_int();
|
||||
if (_max_on_approach_iterations <= 0) {
|
||||
RCLCPP_INFO(
|
||||
_logger, "On approach iteration selected as <= 0, "
|
||||
"disabling tolerance and on approach iterations.");
|
||||
_max_on_approach_iterations = std::numeric_limits<int>::max();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Re-init if needed with mutex lock (to avoid re-init while creating a plan)
|
||||
if (reinit_a_star || reinit_downsampler) {
|
||||
// Re-Initialize A* template
|
||||
if (reinit_a_star) {
|
||||
_a_star = std::make_unique<AStarAlgorithm<Node2D>>(_motion_model, _search_info);
|
||||
_a_star->initialize(
|
||||
_allow_unknown,
|
||||
_max_iterations,
|
||||
_max_on_approach_iterations,
|
||||
_max_planning_time,
|
||||
0.0 /*unused for 2D*/,
|
||||
1.0 /*unused for 2D*/);
|
||||
}
|
||||
|
||||
// Re-Initialize costmap downsampler
|
||||
if (reinit_downsampler) {
|
||||
if (_downsample_costmap && _downsampling_factor > 1) {
|
||||
auto node = _node.lock();
|
||||
std::string topic_name = "downsampled_costmap";
|
||||
_costmap_downsampler = std::make_unique<CostmapDownsampler>();
|
||||
_costmap_downsampler->on_configure(
|
||||
node, _global_frame, topic_name, _costmap, _downsampling_factor);
|
||||
}
|
||||
}
|
||||
}
|
||||
result.successful = true;
|
||||
return result;
|
||||
}
|
||||
|
||||
} // namespace nav2_smac_planner
|
||||
|
||||
#include "pluginlib/class_list_macros.hpp"
|
||||
PLUGINLIB_EXPORT_CLASS(nav2_smac_planner::SmacPlanner2D, nav2_core::GlobalPlanner)
|
||||
@@ -0,0 +1,583 @@
|
||||
// Copyright (c) 2020, 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. Reserved.
|
||||
|
||||
#include <string>
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
#include <algorithm>
|
||||
#include <limits>
|
||||
|
||||
#include "Eigen/Core"
|
||||
#include "nav2_smac_planner/smac_planner_hybrid.hpp"
|
||||
|
||||
// #define BENCHMARK_TESTING
|
||||
|
||||
namespace nav2_smac_planner
|
||||
{
|
||||
|
||||
using namespace std::chrono; // NOLINT
|
||||
using rcl_interfaces::msg::ParameterType;
|
||||
using std::placeholders::_1;
|
||||
|
||||
SmacPlannerHybrid::SmacPlannerHybrid()
|
||||
: _a_star(nullptr),
|
||||
_collision_checker(nullptr, 1, nullptr),
|
||||
_smoother(nullptr),
|
||||
_costmap(nullptr),
|
||||
_costmap_downsampler(nullptr)
|
||||
{
|
||||
}
|
||||
|
||||
SmacPlannerHybrid::~SmacPlannerHybrid()
|
||||
{
|
||||
RCLCPP_INFO(
|
||||
_logger, "Destroying plugin %s of type SmacPlannerHybrid",
|
||||
_name.c_str());
|
||||
}
|
||||
|
||||
void SmacPlannerHybrid::configure(
|
||||
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
|
||||
std::string name, std::shared_ptr<tf2_ros::Buffer>/*tf*/,
|
||||
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
|
||||
{
|
||||
_node = parent;
|
||||
auto node = parent.lock();
|
||||
_logger = node->get_logger();
|
||||
_clock = node->get_clock();
|
||||
_costmap = costmap_ros->getCostmap();
|
||||
_costmap_ros = costmap_ros;
|
||||
_name = name;
|
||||
_global_frame = costmap_ros->getGlobalFrameID();
|
||||
|
||||
RCLCPP_INFO(_logger, "Configuring %s of type SmacPlannerHybrid", name.c_str());
|
||||
|
||||
int angle_quantizations;
|
||||
double analytic_expansion_max_length_m;
|
||||
bool smooth_path;
|
||||
|
||||
// General planner params
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, name + ".downsample_costmap", rclcpp::ParameterValue(false));
|
||||
node->get_parameter(name + ".downsample_costmap", _downsample_costmap);
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, name + ".downsampling_factor", rclcpp::ParameterValue(1));
|
||||
node->get_parameter(name + ".downsampling_factor", _downsampling_factor);
|
||||
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, name + ".angle_quantization_bins", rclcpp::ParameterValue(72));
|
||||
node->get_parameter(name + ".angle_quantization_bins", angle_quantizations);
|
||||
_angle_bin_size = 2.0 * M_PI / angle_quantizations;
|
||||
_angle_quantizations = static_cast<unsigned int>(angle_quantizations);
|
||||
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, name + ".tolerance", rclcpp::ParameterValue(0.25));
|
||||
_tolerance = static_cast<float>(node->get_parameter(name + ".tolerance").as_double());
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, name + ".allow_unknown", rclcpp::ParameterValue(true));
|
||||
node->get_parameter(name + ".allow_unknown", _allow_unknown);
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, name + ".max_iterations", rclcpp::ParameterValue(1000000));
|
||||
node->get_parameter(name + ".max_iterations", _max_iterations);
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, name + ".max_on_approach_iterations", rclcpp::ParameterValue(1000));
|
||||
node->get_parameter(name + ".max_on_approach_iterations", _max_on_approach_iterations);
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, name + ".smooth_path", rclcpp::ParameterValue(true));
|
||||
node->get_parameter(name + ".smooth_path", smooth_path);
|
||||
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, name + ".minimum_turning_radius", rclcpp::ParameterValue(0.4));
|
||||
node->get_parameter(name + ".minimum_turning_radius", _minimum_turning_radius_global_coords);
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, name + ".cache_obstacle_heuristic", rclcpp::ParameterValue(false));
|
||||
node->get_parameter(name + ".cache_obstacle_heuristic", _search_info.cache_obstacle_heuristic);
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, name + ".reverse_penalty", rclcpp::ParameterValue(2.0));
|
||||
node->get_parameter(name + ".reverse_penalty", _search_info.reverse_penalty);
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, name + ".change_penalty", rclcpp::ParameterValue(0.0));
|
||||
node->get_parameter(name + ".change_penalty", _search_info.change_penalty);
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, name + ".non_straight_penalty", rclcpp::ParameterValue(1.2));
|
||||
node->get_parameter(name + ".non_straight_penalty", _search_info.non_straight_penalty);
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, name + ".cost_penalty", rclcpp::ParameterValue(2.0));
|
||||
node->get_parameter(name + ".cost_penalty", _search_info.cost_penalty);
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, name + ".retrospective_penalty", rclcpp::ParameterValue(0.015));
|
||||
node->get_parameter(name + ".retrospective_penalty", _search_info.retrospective_penalty);
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, name + ".analytic_expansion_ratio", rclcpp::ParameterValue(3.5));
|
||||
node->get_parameter(name + ".analytic_expansion_ratio", _search_info.analytic_expansion_ratio);
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, name + ".analytic_expansion_max_length", rclcpp::ParameterValue(3.0));
|
||||
node->get_parameter(name + ".analytic_expansion_max_length", analytic_expansion_max_length_m);
|
||||
_search_info.analytic_expansion_max_length =
|
||||
analytic_expansion_max_length_m / _costmap->getResolution();
|
||||
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, name + ".max_planning_time", rclcpp::ParameterValue(5.0));
|
||||
node->get_parameter(name + ".max_planning_time", _max_planning_time);
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
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 + ".motion_model_for_search", rclcpp::ParameterValue(std::string("DUBIN")));
|
||||
node->get_parameter(name + ".motion_model_for_search", _motion_model_for_search);
|
||||
_motion_model = fromString(_motion_model_for_search);
|
||||
if (_motion_model == MotionModel::UNKNOWN) {
|
||||
RCLCPP_WARN(
|
||||
_logger,
|
||||
"Unable to get MotionModel search type. Given '%s', "
|
||||
"valid options are MOORE, VON_NEUMANN, DUBIN, REEDS_SHEPP, STATE_LATTICE.",
|
||||
_motion_model_for_search.c_str());
|
||||
}
|
||||
|
||||
if (_max_on_approach_iterations <= 0) {
|
||||
RCLCPP_INFO(
|
||||
_logger, "On approach iteration selected as <= 0, "
|
||||
"disabling tolerance and on approach iterations.");
|
||||
_max_on_approach_iterations = std::numeric_limits<int>::max();
|
||||
}
|
||||
|
||||
if (_max_iterations <= 0) {
|
||||
RCLCPP_INFO(
|
||||
_logger, "maximum iteration selected as <= 0, "
|
||||
"disabling maximum iterations.");
|
||||
_max_iterations = std::numeric_limits<int>::max();
|
||||
}
|
||||
|
||||
// convert to grid coordinates
|
||||
if (!_downsample_costmap) {
|
||||
_downsampling_factor = 1;
|
||||
}
|
||||
_search_info.minimum_turning_radius =
|
||||
_minimum_turning_radius_global_coords / (_costmap->getResolution() * _downsampling_factor);
|
||||
_lookup_table_dim =
|
||||
static_cast<float>(_lookup_table_size) /
|
||||
static_cast<float>(_costmap->getResolution() * _downsampling_factor);
|
||||
|
||||
// Make sure its a whole number
|
||||
_lookup_table_dim = static_cast<float>(static_cast<int>(_lookup_table_dim));
|
||||
|
||||
// Make sure its an odd number
|
||||
if (static_cast<int>(_lookup_table_dim) % 2 == 0) {
|
||||
RCLCPP_INFO(
|
||||
_logger,
|
||||
"Even sized heuristic lookup table size set %f, increasing size by 1 to make odd",
|
||||
_lookup_table_dim);
|
||||
_lookup_table_dim += 1.0;
|
||||
}
|
||||
|
||||
// Initialize collision checker
|
||||
_collision_checker = GridCollisionChecker(_costmap, _angle_quantizations, node);
|
||||
_collision_checker.setFootprint(
|
||||
_costmap_ros->getRobotFootprint(),
|
||||
_costmap_ros->getUseRadius(),
|
||||
findCircumscribedCost(_costmap_ros));
|
||||
|
||||
// Initialize A* template
|
||||
_a_star = std::make_unique<AStarAlgorithm<NodeHybrid>>(_motion_model, _search_info);
|
||||
_a_star->initialize(
|
||||
_allow_unknown,
|
||||
_max_iterations,
|
||||
_max_on_approach_iterations,
|
||||
_max_planning_time,
|
||||
_lookup_table_dim,
|
||||
_angle_quantizations);
|
||||
|
||||
// Initialize path smoother
|
||||
if (smooth_path) {
|
||||
SmootherParams params;
|
||||
params.get(node, name);
|
||||
_smoother = std::make_unique<Smoother>(params);
|
||||
_smoother->initialize(_minimum_turning_radius_global_coords);
|
||||
}
|
||||
|
||||
// Initialize costmap downsampler
|
||||
if (_downsample_costmap && _downsampling_factor > 1) {
|
||||
_costmap_downsampler = std::make_unique<CostmapDownsampler>();
|
||||
std::string topic_name = "downsampled_costmap";
|
||||
_costmap_downsampler->on_configure(
|
||||
node, _global_frame, topic_name, _costmap, _downsampling_factor);
|
||||
}
|
||||
|
||||
_raw_plan_publisher = node->create_publisher<nav_msgs::msg::Path>("unsmoothed_plan", 1);
|
||||
|
||||
RCLCPP_INFO(
|
||||
_logger, "Configured plugin %s of type SmacPlannerHybrid with "
|
||||
"maximum iterations %i, max on approach iterations %i, and %s. Tolerance %.2f."
|
||||
"Using motion model: %s.",
|
||||
_name.c_str(), _max_iterations, _max_on_approach_iterations,
|
||||
_allow_unknown ? "allowing unknown traversal" : "not allowing unknown traversal",
|
||||
_tolerance, toString(_motion_model).c_str());
|
||||
}
|
||||
|
||||
void SmacPlannerHybrid::activate()
|
||||
{
|
||||
RCLCPP_INFO(
|
||||
_logger, "Activating plugin %s of type SmacPlannerHybrid",
|
||||
_name.c_str());
|
||||
_raw_plan_publisher->on_activate();
|
||||
if (_costmap_downsampler) {
|
||||
_costmap_downsampler->on_activate();
|
||||
}
|
||||
auto node = _node.lock();
|
||||
// Add callback for dynamic parameters
|
||||
_dyn_params_handler = node->add_on_set_parameters_callback(
|
||||
std::bind(&SmacPlannerHybrid::dynamicParametersCallback, this, _1));
|
||||
}
|
||||
|
||||
void SmacPlannerHybrid::deactivate()
|
||||
{
|
||||
RCLCPP_INFO(
|
||||
_logger, "Deactivating plugin %s of type SmacPlannerHybrid",
|
||||
_name.c_str());
|
||||
_raw_plan_publisher->on_deactivate();
|
||||
if (_costmap_downsampler) {
|
||||
_costmap_downsampler->on_deactivate();
|
||||
}
|
||||
_dyn_params_handler.reset();
|
||||
}
|
||||
|
||||
void SmacPlannerHybrid::cleanup()
|
||||
{
|
||||
RCLCPP_INFO(
|
||||
_logger, "Cleaning up plugin %s of type SmacPlannerHybrid",
|
||||
_name.c_str());
|
||||
_a_star.reset();
|
||||
_smoother.reset();
|
||||
if (_costmap_downsampler) {
|
||||
_costmap_downsampler->on_cleanup();
|
||||
_costmap_downsampler.reset();
|
||||
}
|
||||
_raw_plan_publisher.reset();
|
||||
}
|
||||
|
||||
nav_msgs::msg::Path SmacPlannerHybrid::createPlan(
|
||||
const geometry_msgs::msg::PoseStamped & start,
|
||||
const geometry_msgs::msg::PoseStamped & goal)
|
||||
{
|
||||
std::lock_guard<std::mutex> lock_reinit(_mutex);
|
||||
steady_clock::time_point a = steady_clock::now();
|
||||
|
||||
std::unique_lock<nav2_costmap_2d::Costmap2D::mutex_t> lock(*(_costmap->getMutex()));
|
||||
|
||||
// Downsample costmap, if required
|
||||
nav2_costmap_2d::Costmap2D * costmap = _costmap;
|
||||
if (_costmap_downsampler) {
|
||||
costmap = _costmap_downsampler->downsample(_downsampling_factor);
|
||||
_collision_checker.setCostmap(costmap);
|
||||
}
|
||||
|
||||
// Set collision checker and costmap information
|
||||
_collision_checker.setFootprint(
|
||||
_costmap_ros->getRobotFootprint(),
|
||||
_costmap_ros->getUseRadius(),
|
||||
findCircumscribedCost(_costmap_ros));
|
||||
_a_star->setCollisionChecker(&_collision_checker);
|
||||
|
||||
// Set starting point, in A* bin search coordinates
|
||||
unsigned int mx, my;
|
||||
if (!costmap->worldToMap(start.pose.position.x, start.pose.position.y, mx, my)) {
|
||||
throw std::runtime_error("Start pose is out of costmap!");
|
||||
}
|
||||
|
||||
double orientation_bin = std::round(tf2::getYaw(start.pose.orientation) / _angle_bin_size);
|
||||
while (orientation_bin < 0.0) {
|
||||
orientation_bin += static_cast<float>(_angle_quantizations);
|
||||
}
|
||||
// This is needed to handle precision issues
|
||||
if (orientation_bin >= static_cast<float>(_angle_quantizations)) {
|
||||
orientation_bin -= static_cast<float>(_angle_quantizations);
|
||||
}
|
||||
_a_star->setStart(mx, my, static_cast<unsigned int>(orientation_bin));
|
||||
|
||||
// Set goal point, in A* bin search coordinates
|
||||
if (!costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx, my)) {
|
||||
throw std::runtime_error("Goal pose is out of costmap!");
|
||||
}
|
||||
orientation_bin = std::round(tf2::getYaw(goal.pose.orientation) / _angle_bin_size);
|
||||
while (orientation_bin < 0.0) {
|
||||
orientation_bin += static_cast<float>(_angle_quantizations);
|
||||
}
|
||||
// This is needed to handle precision issues
|
||||
if (orientation_bin >= static_cast<float>(_angle_quantizations)) {
|
||||
orientation_bin -= static_cast<float>(_angle_quantizations);
|
||||
}
|
||||
_a_star->setGoal(mx, my, static_cast<unsigned int>(orientation_bin));
|
||||
|
||||
// Setup message
|
||||
nav_msgs::msg::Path plan;
|
||||
plan.header.stamp = _clock->now();
|
||||
plan.header.frame_id = _global_frame;
|
||||
geometry_msgs::msg::PoseStamped pose;
|
||||
pose.header = plan.header;
|
||||
pose.pose.position.z = 0.0;
|
||||
pose.pose.orientation.x = 0.0;
|
||||
pose.pose.orientation.y = 0.0;
|
||||
pose.pose.orientation.z = 0.0;
|
||||
pose.pose.orientation.w = 1.0;
|
||||
|
||||
// Compute plan
|
||||
NodeHybrid::CoordinateVector path;
|
||||
int num_iterations = 0;
|
||||
std::string error;
|
||||
try {
|
||||
if (!_a_star->createPath(
|
||||
path, num_iterations, _tolerance / static_cast<float>(costmap->getResolution())))
|
||||
{
|
||||
if (num_iterations < _a_star->getMaxIterations()) {
|
||||
error = std::string("no valid path found");
|
||||
} else {
|
||||
error = std::string("exceeded maximum iterations");
|
||||
}
|
||||
}
|
||||
} catch (const std::runtime_error & e) {
|
||||
error = "invalid use: ";
|
||||
error += e.what();
|
||||
}
|
||||
|
||||
if (!error.empty()) {
|
||||
RCLCPP_WARN(
|
||||
_logger,
|
||||
"%s: failed to create plan, %s.",
|
||||
_name.c_str(), error.c_str());
|
||||
return plan;
|
||||
}
|
||||
|
||||
// Convert to world coordinates
|
||||
plan.poses.reserve(path.size());
|
||||
for (int i = path.size() - 1; i >= 0; --i) {
|
||||
pose.pose = getWorldCoords(path[i].x, path[i].y, costmap);
|
||||
pose.pose.orientation = getWorldOrientation(path[i].theta);
|
||||
plan.poses.push_back(pose);
|
||||
}
|
||||
|
||||
// Publish raw path for debug
|
||||
if (_raw_plan_publisher->get_subscription_count() > 0) {
|
||||
_raw_plan_publisher->publish(plan);
|
||||
}
|
||||
|
||||
// Find how much time we have left to do smoothing
|
||||
steady_clock::time_point b = steady_clock::now();
|
||||
duration<double> time_span = duration_cast<duration<double>>(b - a);
|
||||
double time_remaining = _max_planning_time - static_cast<double>(time_span.count());
|
||||
|
||||
#ifdef BENCHMARK_TESTING
|
||||
std::cout << "It took " << time_span.count() * 1000 <<
|
||||
" milliseconds with " << num_iterations << " iterations." << std::endl;
|
||||
#endif
|
||||
|
||||
// Smooth plan
|
||||
if (_smoother && num_iterations > 1) {
|
||||
_smoother->smooth(plan, costmap, time_remaining);
|
||||
}
|
||||
|
||||
#ifdef BENCHMARK_TESTING
|
||||
steady_clock::time_point c = steady_clock::now();
|
||||
duration<double> time_span2 = duration_cast<duration<double>>(c - b);
|
||||
std::cout << "It took " << time_span2.count() * 1000 <<
|
||||
" milliseconds to smooth path." << std::endl;
|
||||
#endif
|
||||
|
||||
return plan;
|
||||
}
|
||||
|
||||
rcl_interfaces::msg::SetParametersResult
|
||||
SmacPlannerHybrid::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters)
|
||||
{
|
||||
rcl_interfaces::msg::SetParametersResult result;
|
||||
std::lock_guard<std::mutex> lock_reinit(_mutex);
|
||||
|
||||
bool reinit_collision_checker = false;
|
||||
bool reinit_a_star = false;
|
||||
bool reinit_downsampler = false;
|
||||
bool reinit_smoother = false;
|
||||
|
||||
for (auto parameter : parameters) {
|
||||
const auto & type = parameter.get_type();
|
||||
const auto & name = parameter.get_name();
|
||||
|
||||
if (type == ParameterType::PARAMETER_DOUBLE) {
|
||||
if (name == _name + ".max_planning_time") {
|
||||
reinit_a_star = true;
|
||||
_max_planning_time = parameter.as_double();
|
||||
} else if (name == _name + ".tolerance") {
|
||||
_tolerance = static_cast<float>(parameter.as_double());
|
||||
} else if (name == _name + ".lookup_table_size") {
|
||||
reinit_a_star = true;
|
||||
_lookup_table_size = parameter.as_double();
|
||||
} else if (name == _name + ".minimum_turning_radius") {
|
||||
reinit_a_star = true;
|
||||
if (_smoother) {
|
||||
reinit_smoother = true;
|
||||
}
|
||||
_minimum_turning_radius_global_coords = static_cast<float>(parameter.as_double());
|
||||
} else if (name == _name + ".reverse_penalty") {
|
||||
reinit_a_star = true;
|
||||
_search_info.reverse_penalty = static_cast<float>(parameter.as_double());
|
||||
} else if (name == _name + ".change_penalty") {
|
||||
reinit_a_star = true;
|
||||
_search_info.change_penalty = static_cast<float>(parameter.as_double());
|
||||
} else if (name == _name + ".non_straight_penalty") {
|
||||
reinit_a_star = true;
|
||||
_search_info.non_straight_penalty = static_cast<float>(parameter.as_double());
|
||||
} else if (name == _name + ".cost_penalty") {
|
||||
reinit_a_star = true;
|
||||
_search_info.cost_penalty = static_cast<float>(parameter.as_double());
|
||||
} else if (name == _name + ".analytic_expansion_ratio") {
|
||||
reinit_a_star = true;
|
||||
_search_info.analytic_expansion_ratio = static_cast<float>(parameter.as_double());
|
||||
} else if (name == _name + ".analytic_expansion_max_length") {
|
||||
reinit_a_star = true;
|
||||
_search_info.analytic_expansion_max_length =
|
||||
static_cast<float>(parameter.as_double()) / _costmap->getResolution();
|
||||
}
|
||||
} else if (type == ParameterType::PARAMETER_BOOL) {
|
||||
if (name == _name + ".downsample_costmap") {
|
||||
reinit_downsampler = true;
|
||||
_downsample_costmap = parameter.as_bool();
|
||||
} else if (name == _name + ".allow_unknown") {
|
||||
reinit_a_star = true;
|
||||
_allow_unknown = parameter.as_bool();
|
||||
} else if (name == _name + ".cache_obstacle_heuristic") {
|
||||
reinit_a_star = true;
|
||||
_search_info.cache_obstacle_heuristic = parameter.as_bool();
|
||||
} else if (name == _name + ".smooth_path") {
|
||||
if (parameter.as_bool()) {
|
||||
reinit_smoother = true;
|
||||
} else {
|
||||
_smoother.reset();
|
||||
}
|
||||
}
|
||||
} else if (type == ParameterType::PARAMETER_INTEGER) {
|
||||
if (name == _name + ".downsampling_factor") {
|
||||
reinit_a_star = true;
|
||||
reinit_downsampler = true;
|
||||
_downsampling_factor = parameter.as_int();
|
||||
} else if (name == _name + ".max_iterations") {
|
||||
reinit_a_star = true;
|
||||
_max_iterations = parameter.as_int();
|
||||
if (_max_iterations <= 0) {
|
||||
RCLCPP_INFO(
|
||||
_logger, "maximum iteration selected as <= 0, "
|
||||
"disabling maximum iterations.");
|
||||
_max_iterations = std::numeric_limits<int>::max();
|
||||
}
|
||||
} else if (name == _name + ".max_on_approach_iterations") {
|
||||
reinit_a_star = true;
|
||||
_max_on_approach_iterations = parameter.as_int();
|
||||
if (_max_on_approach_iterations <= 0) {
|
||||
RCLCPP_INFO(
|
||||
_logger, "On approach iteration selected as <= 0, "
|
||||
"disabling tolerance and on approach iterations.");
|
||||
_max_on_approach_iterations = std::numeric_limits<int>::max();
|
||||
}
|
||||
} else if (name == _name + ".angle_quantization_bins") {
|
||||
reinit_collision_checker = true;
|
||||
reinit_a_star = true;
|
||||
int angle_quantizations = parameter.as_int();
|
||||
_angle_bin_size = 2.0 * M_PI / angle_quantizations;
|
||||
_angle_quantizations = static_cast<unsigned int>(angle_quantizations);
|
||||
}
|
||||
} else if (type == ParameterType::PARAMETER_STRING) {
|
||||
if (name == _name + ".motion_model_for_search") {
|
||||
reinit_a_star = true;
|
||||
_motion_model = fromString(parameter.as_string());
|
||||
if (_motion_model == MotionModel::UNKNOWN) {
|
||||
RCLCPP_WARN(
|
||||
_logger,
|
||||
"Unable to get MotionModel search type. Given '%s', "
|
||||
"valid options are MOORE, VON_NEUMANN, DUBIN, REEDS_SHEPP.",
|
||||
_motion_model_for_search.c_str());
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Re-init if needed with mutex lock (to avoid re-init while creating a plan)
|
||||
if (reinit_a_star || reinit_downsampler || reinit_collision_checker || reinit_smoother) {
|
||||
// convert to grid coordinates
|
||||
if (!_downsample_costmap) {
|
||||
_downsampling_factor = 1;
|
||||
}
|
||||
_search_info.minimum_turning_radius =
|
||||
_minimum_turning_radius_global_coords / (_costmap->getResolution() * _downsampling_factor);
|
||||
_lookup_table_dim =
|
||||
static_cast<float>(_lookup_table_size) /
|
||||
static_cast<float>(_costmap->getResolution() * _downsampling_factor);
|
||||
|
||||
// Make sure its a whole number
|
||||
_lookup_table_dim = static_cast<float>(static_cast<int>(_lookup_table_dim));
|
||||
|
||||
// Make sure its an odd number
|
||||
if (static_cast<int>(_lookup_table_dim) % 2 == 0) {
|
||||
RCLCPP_INFO(
|
||||
_logger,
|
||||
"Even sized heuristic lookup table size set %f, increasing size by 1 to make odd",
|
||||
_lookup_table_dim);
|
||||
_lookup_table_dim += 1.0;
|
||||
}
|
||||
|
||||
auto node = _node.lock();
|
||||
|
||||
// Re-Initialize A* template
|
||||
if (reinit_a_star) {
|
||||
_a_star = std::make_unique<AStarAlgorithm<NodeHybrid>>(_motion_model, _search_info);
|
||||
_a_star->initialize(
|
||||
_allow_unknown,
|
||||
_max_iterations,
|
||||
_max_on_approach_iterations,
|
||||
_max_planning_time,
|
||||
_lookup_table_dim,
|
||||
_angle_quantizations);
|
||||
}
|
||||
|
||||
// Re-Initialize costmap downsampler
|
||||
if (reinit_downsampler) {
|
||||
if (_downsample_costmap && _downsampling_factor > 1) {
|
||||
std::string topic_name = "downsampled_costmap";
|
||||
_costmap_downsampler = std::make_unique<CostmapDownsampler>();
|
||||
_costmap_downsampler->on_configure(
|
||||
node, _global_frame, topic_name, _costmap, _downsampling_factor);
|
||||
}
|
||||
}
|
||||
|
||||
// Re-Initialize collision checker
|
||||
if (reinit_collision_checker) {
|
||||
_collision_checker = GridCollisionChecker(_costmap, _angle_quantizations, node);
|
||||
_collision_checker.setFootprint(
|
||||
_costmap_ros->getRobotFootprint(),
|
||||
_costmap_ros->getUseRadius(),
|
||||
findCircumscribedCost(_costmap_ros));
|
||||
}
|
||||
|
||||
// Re-Initialize smoother
|
||||
if (reinit_smoother) {
|
||||
SmootherParams params;
|
||||
params.get(node, _name);
|
||||
_smoother = std::make_unique<Smoother>(params);
|
||||
_smoother->initialize(_minimum_turning_radius_global_coords);
|
||||
}
|
||||
}
|
||||
result.successful = true;
|
||||
return result;
|
||||
}
|
||||
|
||||
} // namespace nav2_smac_planner
|
||||
|
||||
#include "pluginlib/class_list_macros.hpp"
|
||||
PLUGINLIB_EXPORT_CLASS(nav2_smac_planner::SmacPlannerHybrid, nav2_core::GlobalPlanner)
|
||||
@@ -0,0 +1,500 @@
|
||||
// Copyright (c) 2021, 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. Reserved.
|
||||
|
||||
#include <string>
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
#include <algorithm>
|
||||
#include <limits>
|
||||
|
||||
#include "Eigen/Core"
|
||||
#include "nav2_smac_planner/smac_planner_lattice.hpp"
|
||||
|
||||
// #define BENCHMARK_TESTING
|
||||
|
||||
namespace nav2_smac_planner
|
||||
{
|
||||
|
||||
using namespace std::chrono; // NOLINT
|
||||
using rcl_interfaces::msg::ParameterType;
|
||||
|
||||
SmacPlannerLattice::SmacPlannerLattice()
|
||||
: _a_star(nullptr),
|
||||
_collision_checker(nullptr, 1, nullptr),
|
||||
_smoother(nullptr),
|
||||
_costmap(nullptr)
|
||||
{
|
||||
}
|
||||
|
||||
SmacPlannerLattice::~SmacPlannerLattice()
|
||||
{
|
||||
RCLCPP_INFO(
|
||||
_logger, "Destroying plugin %s of type SmacPlannerLattice",
|
||||
_name.c_str());
|
||||
}
|
||||
|
||||
void SmacPlannerLattice::configure(
|
||||
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
|
||||
std::string name, std::shared_ptr<tf2_ros::Buffer>/*tf*/,
|
||||
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
|
||||
{
|
||||
_node = parent;
|
||||
auto node = parent.lock();
|
||||
_logger = node->get_logger();
|
||||
_clock = node->get_clock();
|
||||
_costmap = costmap_ros->getCostmap();
|
||||
_costmap_ros = costmap_ros;
|
||||
_name = name;
|
||||
_global_frame = costmap_ros->getGlobalFrameID();
|
||||
_raw_plan_publisher = node->create_publisher<nav_msgs::msg::Path>("unsmoothed_plan", 1);
|
||||
|
||||
RCLCPP_INFO(_logger, "Configuring %s of type SmacPlannerLattice", name.c_str());
|
||||
|
||||
// General planner params
|
||||
double analytic_expansion_max_length_m;
|
||||
bool smooth_path;
|
||||
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, name + ".tolerance", rclcpp::ParameterValue(0.25));
|
||||
_tolerance = static_cast<float>(node->get_parameter(name + ".tolerance").as_double());
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, name + ".allow_unknown", rclcpp::ParameterValue(true));
|
||||
node->get_parameter(name + ".allow_unknown", _allow_unknown);
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, name + ".max_iterations", rclcpp::ParameterValue(1000000));
|
||||
node->get_parameter(name + ".max_iterations", _max_iterations);
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, name + ".max_on_approach_iterations", rclcpp::ParameterValue(1000));
|
||||
node->get_parameter(name + ".max_on_approach_iterations", _max_on_approach_iterations);
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, name + ".smooth_path", rclcpp::ParameterValue(true));
|
||||
node->get_parameter(name + ".smooth_path", smooth_path);
|
||||
|
||||
// Default to a well rounded model: 16 bin, 0.4m turning radius, ackermann model
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, name + ".lattice_filepath", rclcpp::ParameterValue(
|
||||
ament_index_cpp::get_package_share_directory("nav2_smac_planner") +
|
||||
"/sample_primitives/5cm_resolution/0.5m_turning_radius/ackermann/output.json"));
|
||||
node->get_parameter(name + ".lattice_filepath", _search_info.lattice_filepath);
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, name + ".cache_obstacle_heuristic", rclcpp::ParameterValue(false));
|
||||
node->get_parameter(name + ".cache_obstacle_heuristic", _search_info.cache_obstacle_heuristic);
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, name + ".reverse_penalty", rclcpp::ParameterValue(2.0));
|
||||
node->get_parameter(name + ".reverse_penalty", _search_info.reverse_penalty);
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, name + ".change_penalty", rclcpp::ParameterValue(0.05));
|
||||
node->get_parameter(name + ".change_penalty", _search_info.change_penalty);
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, name + ".non_straight_penalty", rclcpp::ParameterValue(1.05));
|
||||
node->get_parameter(name + ".non_straight_penalty", _search_info.non_straight_penalty);
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, name + ".cost_penalty", rclcpp::ParameterValue(2.0));
|
||||
node->get_parameter(name + ".cost_penalty", _search_info.cost_penalty);
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, name + ".retrospective_penalty", rclcpp::ParameterValue(0.015));
|
||||
node->get_parameter(name + ".retrospective_penalty", _search_info.retrospective_penalty);
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, name + ".rotation_penalty", rclcpp::ParameterValue(5.0));
|
||||
node->get_parameter(name + ".rotation_penalty", _search_info.rotation_penalty);
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, name + ".analytic_expansion_ratio", rclcpp::ParameterValue(3.5));
|
||||
node->get_parameter(name + ".analytic_expansion_ratio", _search_info.analytic_expansion_ratio);
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, name + ".analytic_expansion_max_length", rclcpp::ParameterValue(3.0));
|
||||
node->get_parameter(name + ".analytic_expansion_max_length", analytic_expansion_max_length_m);
|
||||
_search_info.analytic_expansion_max_length =
|
||||
analytic_expansion_max_length_m / _costmap->getResolution();
|
||||
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, name + ".max_planning_time", rclcpp::ParameterValue(5.0));
|
||||
node->get_parameter(name + ".max_planning_time", _max_planning_time);
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
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 + ".allow_reverse_expansion", rclcpp::ParameterValue(false));
|
||||
node->get_parameter(name + ".allow_reverse_expansion", _search_info.allow_reverse_expansion);
|
||||
|
||||
_metadata = LatticeMotionTable::getLatticeMetadata(_search_info.lattice_filepath);
|
||||
_search_info.minimum_turning_radius =
|
||||
_metadata.min_turning_radius / (_costmap->getResolution());
|
||||
_motion_model = MotionModel::STATE_LATTICE;
|
||||
|
||||
if (_max_on_approach_iterations <= 0) {
|
||||
RCLCPP_INFO(
|
||||
_logger, "On approach iteration selected as <= 0, "
|
||||
"disabling tolerance and on approach iterations.");
|
||||
_max_on_approach_iterations = std::numeric_limits<int>::max();
|
||||
}
|
||||
|
||||
if (_max_iterations <= 0) {
|
||||
RCLCPP_INFO(
|
||||
_logger, "maximum iteration selected as <= 0, "
|
||||
"disabling maximum iterations.");
|
||||
_max_iterations = std::numeric_limits<int>::max();
|
||||
}
|
||||
|
||||
float lookup_table_dim =
|
||||
static_cast<float>(_lookup_table_size) /
|
||||
static_cast<float>(_costmap->getResolution());
|
||||
|
||||
// Make sure its a whole number
|
||||
lookup_table_dim = static_cast<float>(static_cast<int>(lookup_table_dim));
|
||||
|
||||
// Make sure its an odd number
|
||||
if (static_cast<int>(lookup_table_dim) % 2 == 0) {
|
||||
RCLCPP_INFO(
|
||||
_logger,
|
||||
"Even sized heuristic lookup table size set %f, increasing size by 1 to make odd",
|
||||
lookup_table_dim);
|
||||
lookup_table_dim += 1.0;
|
||||
}
|
||||
|
||||
// Initialize collision checker using 72 evenly sized bins instead of the lattice
|
||||
// heading angles. This is done so that we have precomputed angles every 5 degrees.
|
||||
// If we used the sparse lattice headings (usually 16), then when we attempt to collision
|
||||
// check for intermediary points of the primitives, we're forced to round to one of the 16
|
||||
// increments causing "wobbly" checks that could cause larger robots to virtually show collisions
|
||||
// in valid configurations. This approximation helps to bound orientation error for all checks
|
||||
// in exchange for slight inaccuracies in the collision headings in terminal search states.
|
||||
_collision_checker = GridCollisionChecker(_costmap, 72u, node);
|
||||
_collision_checker.setFootprint(
|
||||
costmap_ros->getRobotFootprint(),
|
||||
costmap_ros->getUseRadius(),
|
||||
findCircumscribedCost(costmap_ros));
|
||||
|
||||
// Initialize A* template
|
||||
_a_star = std::make_unique<AStarAlgorithm<NodeLattice>>(_motion_model, _search_info);
|
||||
_a_star->initialize(
|
||||
_allow_unknown,
|
||||
_max_iterations,
|
||||
_max_on_approach_iterations,
|
||||
_max_planning_time,
|
||||
lookup_table_dim,
|
||||
_metadata.number_of_headings);
|
||||
|
||||
// Initialize path smoother
|
||||
if (smooth_path) {
|
||||
SmootherParams params;
|
||||
params.get(node, name);
|
||||
_smoother = std::make_unique<Smoother>(params);
|
||||
_smoother->initialize(_metadata.min_turning_radius);
|
||||
}
|
||||
|
||||
RCLCPP_INFO(
|
||||
_logger, "Configured plugin %s of type SmacPlannerLattice with "
|
||||
"maximum iterations %i, max on approach iterations %i, "
|
||||
"and %s. Tolerance %.2f. Using motion model: %s. State lattice file: %s.",
|
||||
_name.c_str(), _max_iterations, _max_on_approach_iterations,
|
||||
_allow_unknown ? "allowing unknown traversal" : "not allowing unknown traversal",
|
||||
_tolerance, toString(_motion_model).c_str(), _search_info.lattice_filepath.c_str());
|
||||
}
|
||||
|
||||
void SmacPlannerLattice::activate()
|
||||
{
|
||||
RCLCPP_INFO(
|
||||
_logger, "Activating plugin %s of type SmacPlannerLattice",
|
||||
_name.c_str());
|
||||
_raw_plan_publisher->on_activate();
|
||||
auto node = _node.lock();
|
||||
// Add callback for dynamic parameters
|
||||
_dyn_params_handler = node->add_on_set_parameters_callback(
|
||||
std::bind(&SmacPlannerLattice::dynamicParametersCallback, this, std::placeholders::_1));
|
||||
}
|
||||
|
||||
void SmacPlannerLattice::deactivate()
|
||||
{
|
||||
RCLCPP_INFO(
|
||||
_logger, "Deactivating plugin %s of type SmacPlannerLattice",
|
||||
_name.c_str());
|
||||
_raw_plan_publisher->on_deactivate();
|
||||
_dyn_params_handler.reset();
|
||||
}
|
||||
|
||||
void SmacPlannerLattice::cleanup()
|
||||
{
|
||||
RCLCPP_INFO(
|
||||
_logger, "Cleaning up plugin %s of type SmacPlannerLattice",
|
||||
_name.c_str());
|
||||
_a_star.reset();
|
||||
_smoother.reset();
|
||||
_raw_plan_publisher.reset();
|
||||
}
|
||||
|
||||
nav_msgs::msg::Path SmacPlannerLattice::createPlan(
|
||||
const geometry_msgs::msg::PoseStamped & start,
|
||||
const geometry_msgs::msg::PoseStamped & goal)
|
||||
{
|
||||
std::lock_guard<std::mutex> lock_reinit(_mutex);
|
||||
steady_clock::time_point a = steady_clock::now();
|
||||
|
||||
std::unique_lock<nav2_costmap_2d::Costmap2D::mutex_t> lock(*(_costmap->getMutex()));
|
||||
|
||||
// Set collision checker and costmap information
|
||||
_collision_checker.setFootprint(
|
||||
_costmap_ros->getRobotFootprint(),
|
||||
_costmap_ros->getUseRadius(),
|
||||
findCircumscribedCost(_costmap_ros));
|
||||
_a_star->setCollisionChecker(&_collision_checker);
|
||||
|
||||
// Set starting point, in A* bin search coordinates
|
||||
unsigned int mx, my;
|
||||
_costmap->worldToMap(start.pose.position.x, start.pose.position.y, mx, my);
|
||||
_a_star->setStart(
|
||||
mx, my,
|
||||
NodeLattice::motion_table.getClosestAngularBin(tf2::getYaw(start.pose.orientation)));
|
||||
|
||||
// Set goal point, in A* bin search coordinates
|
||||
_costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx, my);
|
||||
_a_star->setGoal(
|
||||
mx, my,
|
||||
NodeLattice::motion_table.getClosestAngularBin(tf2::getYaw(goal.pose.orientation)));
|
||||
|
||||
// Setup message
|
||||
nav_msgs::msg::Path plan;
|
||||
plan.header.stamp = _clock->now();
|
||||
plan.header.frame_id = _global_frame;
|
||||
geometry_msgs::msg::PoseStamped pose;
|
||||
pose.header = plan.header;
|
||||
pose.pose.position.z = 0.0;
|
||||
pose.pose.orientation.x = 0.0;
|
||||
pose.pose.orientation.y = 0.0;
|
||||
pose.pose.orientation.z = 0.0;
|
||||
pose.pose.orientation.w = 1.0;
|
||||
|
||||
// Compute plan
|
||||
NodeLattice::CoordinateVector path;
|
||||
int num_iterations = 0;
|
||||
std::string error;
|
||||
try {
|
||||
if (!_a_star->createPath(
|
||||
path, num_iterations, _tolerance / static_cast<float>(_costmap->getResolution())))
|
||||
{
|
||||
if (num_iterations < _a_star->getMaxIterations()) {
|
||||
error = std::string("no valid path found");
|
||||
} else {
|
||||
error = std::string("exceeded maximum iterations");
|
||||
}
|
||||
}
|
||||
} catch (const std::runtime_error & e) {
|
||||
error = "invalid use: ";
|
||||
error += e.what();
|
||||
}
|
||||
|
||||
if (!error.empty()) {
|
||||
RCLCPP_WARN(
|
||||
_logger,
|
||||
"%s: failed to create plan, %s.",
|
||||
_name.c_str(), error.c_str());
|
||||
return plan;
|
||||
}
|
||||
|
||||
// Convert to world coordinates
|
||||
plan.poses.reserve(path.size());
|
||||
geometry_msgs::msg::PoseStamped last_pose = pose;
|
||||
for (int i = path.size() - 1; i >= 0; --i) {
|
||||
pose.pose = getWorldCoords(path[i].x, path[i].y, _costmap);
|
||||
pose.pose.orientation = getWorldOrientation(path[i].theta);
|
||||
if (fabs(pose.pose.position.x - last_pose.pose.position.x) < 1e-4 &&
|
||||
fabs(pose.pose.position.y - last_pose.pose.position.y) < 1e-4 &&
|
||||
fabs(tf2::getYaw(pose.pose.orientation) - tf2::getYaw(last_pose.pose.orientation)) < 1e-4)
|
||||
{
|
||||
RCLCPP_DEBUG(
|
||||
_logger,
|
||||
"Removed a path from the path due to replication. "
|
||||
"Make sure your minimum control set does not contain duplicate values!");
|
||||
continue;
|
||||
}
|
||||
last_pose = pose;
|
||||
plan.poses.push_back(pose);
|
||||
}
|
||||
|
||||
// Publish raw path for debug
|
||||
if (_raw_plan_publisher->get_subscription_count() > 0) {
|
||||
_raw_plan_publisher->publish(plan);
|
||||
}
|
||||
|
||||
// Find how much time we have left to do smoothing
|
||||
steady_clock::time_point b = steady_clock::now();
|
||||
duration<double> time_span = duration_cast<duration<double>>(b - a);
|
||||
double time_remaining = _max_planning_time - static_cast<double>(time_span.count());
|
||||
|
||||
#ifdef BENCHMARK_TESTING
|
||||
std::cout << "It took " << time_span.count() * 1000 <<
|
||||
" milliseconds with " << num_iterations << " iterations." << std::endl;
|
||||
#endif
|
||||
|
||||
// Smooth plan
|
||||
if (_smoother && num_iterations > 1) {
|
||||
_smoother->smooth(plan, _costmap, time_remaining);
|
||||
}
|
||||
|
||||
#ifdef BENCHMARK_TESTING
|
||||
steady_clock::time_point c = steady_clock::now();
|
||||
duration<double> time_span2 = duration_cast<duration<double>>(c - b);
|
||||
std::cout << "It took " << time_span2.count() * 1000 <<
|
||||
" milliseconds to smooth path." << std::endl;
|
||||
#endif
|
||||
|
||||
return plan;
|
||||
}
|
||||
|
||||
rcl_interfaces::msg::SetParametersResult
|
||||
SmacPlannerLattice::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters)
|
||||
{
|
||||
rcl_interfaces::msg::SetParametersResult result;
|
||||
std::lock_guard<std::mutex> lock_reinit(_mutex);
|
||||
|
||||
bool reinit_a_star = false;
|
||||
bool reinit_smoother = false;
|
||||
|
||||
for (auto parameter : parameters) {
|
||||
const auto & type = parameter.get_type();
|
||||
const auto & name = parameter.get_name();
|
||||
|
||||
if (type == ParameterType::PARAMETER_DOUBLE) {
|
||||
if (name == _name + ".max_planning_time") {
|
||||
reinit_a_star = true;
|
||||
_max_planning_time = parameter.as_double();
|
||||
} else if (name == _name + ".tolerance") {
|
||||
_tolerance = static_cast<float>(parameter.as_double());
|
||||
} else if (name == _name + ".lookup_table_size") {
|
||||
reinit_a_star = true;
|
||||
_lookup_table_size = parameter.as_double();
|
||||
} else if (name == _name + ".reverse_penalty") {
|
||||
reinit_a_star = true;
|
||||
_search_info.reverse_penalty = static_cast<float>(parameter.as_double());
|
||||
} else if (name == _name + ".change_penalty") {
|
||||
reinit_a_star = true;
|
||||
_search_info.change_penalty = static_cast<float>(parameter.as_double());
|
||||
} else if (name == _name + ".non_straight_penalty") {
|
||||
reinit_a_star = true;
|
||||
_search_info.non_straight_penalty = static_cast<float>(parameter.as_double());
|
||||
} else if (name == _name + ".cost_penalty") {
|
||||
reinit_a_star = true;
|
||||
_search_info.cost_penalty = static_cast<float>(parameter.as_double());
|
||||
} else if (name == _name + ".rotation_penalty") {
|
||||
reinit_a_star = true;
|
||||
_search_info.rotation_penalty = static_cast<float>(parameter.as_double());
|
||||
} else if (name == _name + ".analytic_expansion_ratio") {
|
||||
reinit_a_star = true;
|
||||
_search_info.analytic_expansion_ratio = static_cast<float>(parameter.as_double());
|
||||
} else if (name == _name + ".analytic_expansion_max_length") {
|
||||
reinit_a_star = true;
|
||||
_search_info.analytic_expansion_max_length =
|
||||
static_cast<float>(parameter.as_double()) / _costmap->getResolution();
|
||||
}
|
||||
} else if (type == ParameterType::PARAMETER_BOOL) {
|
||||
if (name == _name + ".allow_unknown") {
|
||||
reinit_a_star = true;
|
||||
_allow_unknown = parameter.as_bool();
|
||||
} else if (name == _name + ".cache_obstacle_heuristic") {
|
||||
reinit_a_star = true;
|
||||
_search_info.cache_obstacle_heuristic = parameter.as_bool();
|
||||
} else if (name == _name + ".allow_reverse_expansion") {
|
||||
reinit_a_star = true;
|
||||
_search_info.allow_reverse_expansion = parameter.as_bool();
|
||||
} else if (name == _name + ".smooth_path") {
|
||||
if (parameter.as_bool()) {
|
||||
reinit_smoother = true;
|
||||
} else {
|
||||
_smoother.reset();
|
||||
}
|
||||
}
|
||||
} else if (type == ParameterType::PARAMETER_INTEGER) {
|
||||
if (name == _name + ".max_iterations") {
|
||||
reinit_a_star = true;
|
||||
_max_iterations = parameter.as_int();
|
||||
if (_max_iterations <= 0) {
|
||||
RCLCPP_INFO(
|
||||
_logger, "maximum iteration selected as <= 0, "
|
||||
"disabling maximum iterations.");
|
||||
_max_iterations = std::numeric_limits<int>::max();
|
||||
}
|
||||
}
|
||||
} else if (name == _name + ".max_on_approach_iterations") {
|
||||
reinit_a_star = true;
|
||||
_max_on_approach_iterations = parameter.as_int();
|
||||
if (_max_on_approach_iterations <= 0) {
|
||||
RCLCPP_INFO(
|
||||
_logger, "On approach iteration selected as <= 0, "
|
||||
"disabling tolerance and on approach iterations.");
|
||||
_max_on_approach_iterations = std::numeric_limits<int>::max();
|
||||
}
|
||||
} else if (type == ParameterType::PARAMETER_STRING) {
|
||||
if (name == _name + ".lattice_filepath") {
|
||||
reinit_a_star = true;
|
||||
if (_smoother) {
|
||||
reinit_smoother = true;
|
||||
}
|
||||
_search_info.lattice_filepath = parameter.as_string();
|
||||
_metadata = LatticeMotionTable::getLatticeMetadata(_search_info.lattice_filepath);
|
||||
_search_info.minimum_turning_radius =
|
||||
_metadata.min_turning_radius / (_costmap->getResolution());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Re-init if needed with mutex lock (to avoid re-init while creating a plan)
|
||||
if (reinit_a_star || reinit_smoother) {
|
||||
// convert to grid coordinates
|
||||
_search_info.minimum_turning_radius =
|
||||
_metadata.min_turning_radius / (_costmap->getResolution());
|
||||
float lookup_table_dim =
|
||||
static_cast<float>(_lookup_table_size) /
|
||||
static_cast<float>(_costmap->getResolution());
|
||||
|
||||
// Make sure its a whole number
|
||||
lookup_table_dim = static_cast<float>(static_cast<int>(lookup_table_dim));
|
||||
|
||||
// Make sure its an odd number
|
||||
if (static_cast<int>(lookup_table_dim) % 2 == 0) {
|
||||
RCLCPP_INFO(
|
||||
_logger,
|
||||
"Even sized heuristic lookup table size set %f, increasing size by 1 to make odd",
|
||||
lookup_table_dim);
|
||||
lookup_table_dim += 1.0;
|
||||
}
|
||||
|
||||
// Re-Initialize smoother
|
||||
if (reinit_smoother) {
|
||||
auto node = _node.lock();
|
||||
SmootherParams params;
|
||||
params.get(node, _name);
|
||||
_smoother = std::make_unique<Smoother>(params);
|
||||
_smoother->initialize(_metadata.min_turning_radius);
|
||||
}
|
||||
|
||||
// Re-Initialize A* template
|
||||
if (reinit_a_star) {
|
||||
_a_star = std::make_unique<AStarAlgorithm<NodeLattice>>(_motion_model, _search_info);
|
||||
_a_star->initialize(
|
||||
_allow_unknown,
|
||||
_max_iterations,
|
||||
_max_on_approach_iterations,
|
||||
_max_planning_time,
|
||||
lookup_table_dim,
|
||||
_metadata.number_of_headings);
|
||||
}
|
||||
}
|
||||
|
||||
result.successful = true;
|
||||
return result;
|
||||
}
|
||||
|
||||
} // namespace nav2_smac_planner
|
||||
|
||||
#include "pluginlib/class_list_macros.hpp"
|
||||
PLUGINLIB_EXPORT_CLASS(nav2_smac_planner::SmacPlannerLattice, nav2_core::GlobalPlanner)
|
||||
@@ -0,0 +1,512 @@
|
||||
// Copyright (c) 2021, 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. Reserved.
|
||||
|
||||
#include <ompl/base/ScopedState.h>
|
||||
#include <ompl/base/spaces/DubinsStateSpace.h>
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
#include "nav2_smac_planner/smoother.hpp"
|
||||
|
||||
namespace nav2_smac_planner
|
||||
{
|
||||
using namespace nav2_util::geometry_utils; // NOLINT
|
||||
using namespace std::chrono; // NOLINT
|
||||
|
||||
Smoother::Smoother(const SmootherParams & params)
|
||||
{
|
||||
tolerance_ = params.tolerance_;
|
||||
max_its_ = params.max_its_;
|
||||
data_w_ = params.w_data_;
|
||||
smooth_w_ = params.w_smooth_;
|
||||
is_holonomic_ = params.holonomic_;
|
||||
do_refinement_ = params.do_refinement_;
|
||||
}
|
||||
|
||||
void Smoother::initialize(const double & min_turning_radius)
|
||||
{
|
||||
min_turning_rad_ = min_turning_radius;
|
||||
state_space_ = std::make_unique<ompl::base::DubinsStateSpace>(min_turning_rad_);
|
||||
}
|
||||
|
||||
bool Smoother::smooth(
|
||||
nav_msgs::msg::Path & path,
|
||||
const nav2_costmap_2d::Costmap2D * costmap,
|
||||
const double & max_time)
|
||||
{
|
||||
// by-pass path orientations approximation when skipping smac smoother
|
||||
if (max_its_ == 0) {
|
||||
return false;
|
||||
}
|
||||
|
||||
refinement_ctr_ = 0;
|
||||
steady_clock::time_point start = steady_clock::now();
|
||||
double time_remaining = max_time;
|
||||
bool success = true, reversing_segment;
|
||||
nav_msgs::msg::Path curr_path_segment;
|
||||
curr_path_segment.header = path.header;
|
||||
std::vector<PathSegment> path_segments = findDirectionalPathSegments(path);
|
||||
|
||||
for (unsigned int i = 0; i != path_segments.size(); i++) {
|
||||
if (path_segments[i].end - path_segments[i].start > 10) {
|
||||
// Populate path segment
|
||||
curr_path_segment.poses.clear();
|
||||
std::copy(
|
||||
path.poses.begin() + path_segments[i].start,
|
||||
path.poses.begin() + path_segments[i].end + 1,
|
||||
std::back_inserter(curr_path_segment.poses));
|
||||
|
||||
// Make sure we're still able to smooth with time remaining
|
||||
steady_clock::time_point now = steady_clock::now();
|
||||
time_remaining = max_time - duration_cast<duration<double>>(now - start).count();
|
||||
|
||||
// Smooth path segment naively
|
||||
const geometry_msgs::msg::Pose start_pose = curr_path_segment.poses.front().pose;
|
||||
const geometry_msgs::msg::Pose goal_pose = curr_path_segment.poses.back().pose;
|
||||
bool local_success =
|
||||
smoothImpl(curr_path_segment, reversing_segment, costmap, time_remaining);
|
||||
success = success && local_success;
|
||||
|
||||
// Enforce boundary conditions
|
||||
if (!is_holonomic_ && local_success) {
|
||||
enforceStartBoundaryConditions(start_pose, curr_path_segment, costmap, reversing_segment);
|
||||
enforceEndBoundaryConditions(goal_pose, curr_path_segment, costmap, reversing_segment);
|
||||
}
|
||||
|
||||
// Assemble the path changes to the main path
|
||||
std::copy(
|
||||
curr_path_segment.poses.begin(),
|
||||
curr_path_segment.poses.end(),
|
||||
path.poses.begin() + path_segments[i].start);
|
||||
}
|
||||
}
|
||||
|
||||
return success;
|
||||
}
|
||||
|
||||
bool Smoother::smoothImpl(
|
||||
nav_msgs::msg::Path & path,
|
||||
bool & reversing_segment,
|
||||
const nav2_costmap_2d::Costmap2D * costmap,
|
||||
const double & max_time)
|
||||
{
|
||||
steady_clock::time_point a = steady_clock::now();
|
||||
rclcpp::Duration max_dur = rclcpp::Duration::from_seconds(max_time);
|
||||
|
||||
int its = 0;
|
||||
double change = tolerance_;
|
||||
const unsigned int & path_size = path.poses.size();
|
||||
double x_i, y_i, y_m1, y_ip1, y_i_org;
|
||||
unsigned int mx, my;
|
||||
|
||||
nav_msgs::msg::Path new_path = path;
|
||||
nav_msgs::msg::Path last_path = path;
|
||||
|
||||
while (change >= tolerance_) {
|
||||
its += 1;
|
||||
change = 0.0;
|
||||
|
||||
// Make sure the smoothing function will converge
|
||||
if (its >= max_its_) {
|
||||
RCLCPP_DEBUG(
|
||||
rclcpp::get_logger("SmacPlannerSmoother"),
|
||||
"Number of iterations has exceeded limit of %i.", max_its_);
|
||||
path = last_path;
|
||||
updateApproximatePathOrientations(path, reversing_segment);
|
||||
return false;
|
||||
}
|
||||
|
||||
// Make sure still have time left to process
|
||||
steady_clock::time_point b = steady_clock::now();
|
||||
rclcpp::Duration timespan(duration_cast<duration<double>>(b - a));
|
||||
if (timespan > max_dur) {
|
||||
RCLCPP_DEBUG(
|
||||
rclcpp::get_logger("SmacPlannerSmoother"),
|
||||
"Smoothing time exceeded allowed duration of %0.2f.", max_time);
|
||||
path = last_path;
|
||||
updateApproximatePathOrientations(path, reversing_segment);
|
||||
return false;
|
||||
}
|
||||
|
||||
for (unsigned int i = 1; i != path_size - 1; i++) {
|
||||
for (unsigned int j = 0; j != 2; j++) {
|
||||
x_i = getFieldByDim(path.poses[i], j);
|
||||
y_i = getFieldByDim(new_path.poses[i], j);
|
||||
y_m1 = getFieldByDim(new_path.poses[i - 1], j);
|
||||
y_ip1 = getFieldByDim(new_path.poses[i + 1], j);
|
||||
y_i_org = y_i;
|
||||
|
||||
// Smooth based on local 3 point neighborhood and original data locations
|
||||
y_i += data_w_ * (x_i - y_i) + smooth_w_ * (y_ip1 + y_m1 - (2.0 * y_i));
|
||||
setFieldByDim(new_path.poses[i], j, y_i);
|
||||
change += abs(y_i - y_i_org);
|
||||
}
|
||||
|
||||
// validate update is admissible, only checks cost if a valid costmap pointer is provided
|
||||
float cost = 0.0;
|
||||
if (costmap) {
|
||||
costmap->worldToMap(
|
||||
getFieldByDim(new_path.poses[i], 0),
|
||||
getFieldByDim(new_path.poses[i], 1),
|
||||
mx, my);
|
||||
cost = static_cast<float>(costmap->getCost(mx, my));
|
||||
}
|
||||
|
||||
if (cost > MAX_NON_OBSTACLE && cost != UNKNOWN) {
|
||||
RCLCPP_DEBUG(
|
||||
rclcpp::get_logger("SmacPlannerSmoother"),
|
||||
"Smoothing process resulted in an infeasible collision. "
|
||||
"Returning the last path before the infeasibility was introduced.");
|
||||
path = last_path;
|
||||
updateApproximatePathOrientations(path, reversing_segment);
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
last_path = new_path;
|
||||
}
|
||||
|
||||
// Lets do additional refinement, it shouldn't take more than a couple milliseconds
|
||||
// but really puts the path quality over the top.
|
||||
if (do_refinement_ && refinement_ctr_ < 4) {
|
||||
refinement_ctr_++;
|
||||
smoothImpl(new_path, reversing_segment, costmap, max_time);
|
||||
}
|
||||
|
||||
updateApproximatePathOrientations(new_path, reversing_segment);
|
||||
path = new_path;
|
||||
return true;
|
||||
}
|
||||
|
||||
double Smoother::getFieldByDim(
|
||||
const geometry_msgs::msg::PoseStamped & msg, const unsigned int & dim)
|
||||
{
|
||||
if (dim == 0) {
|
||||
return msg.pose.position.x;
|
||||
} else if (dim == 1) {
|
||||
return msg.pose.position.y;
|
||||
} else {
|
||||
return msg.pose.position.z;
|
||||
}
|
||||
}
|
||||
|
||||
void Smoother::setFieldByDim(
|
||||
geometry_msgs::msg::PoseStamped & msg, const unsigned int dim,
|
||||
const double & value)
|
||||
{
|
||||
if (dim == 0) {
|
||||
msg.pose.position.x = value;
|
||||
} else if (dim == 1) {
|
||||
msg.pose.position.y = value;
|
||||
} else {
|
||||
msg.pose.position.z = value;
|
||||
}
|
||||
}
|
||||
|
||||
std::vector<PathSegment> Smoother::findDirectionalPathSegments(const nav_msgs::msg::Path & path)
|
||||
{
|
||||
std::vector<PathSegment> segments;
|
||||
PathSegment curr_segment;
|
||||
curr_segment.start = 0;
|
||||
|
||||
// If holonomic, no directional changes and
|
||||
// may have abrupt angular changes from naive grid search
|
||||
if (is_holonomic_) {
|
||||
curr_segment.end = path.poses.size() - 1;
|
||||
segments.push_back(curr_segment);
|
||||
return segments;
|
||||
}
|
||||
|
||||
// Iterating through the path to determine the position of the cusp
|
||||
for (unsigned int idx = 1; idx < path.poses.size() - 1; ++idx) {
|
||||
// We have two vectors for the dot product OA and AB. Determining the vectors.
|
||||
double oa_x = path.poses[idx].pose.position.x -
|
||||
path.poses[idx - 1].pose.position.x;
|
||||
double oa_y = path.poses[idx].pose.position.y -
|
||||
path.poses[idx - 1].pose.position.y;
|
||||
double ab_x = path.poses[idx + 1].pose.position.x -
|
||||
path.poses[idx].pose.position.x;
|
||||
double ab_y = path.poses[idx + 1].pose.position.y -
|
||||
path.poses[idx].pose.position.y;
|
||||
|
||||
// Checking for the existance of cusp, in the path, using the dot product.
|
||||
double dot_product = (oa_x * ab_x) + (oa_y * ab_y);
|
||||
if (dot_product < 0.0) {
|
||||
curr_segment.end = idx;
|
||||
segments.push_back(curr_segment);
|
||||
curr_segment.start = idx;
|
||||
}
|
||||
|
||||
// Checking for the existance of a differential rotation in place.
|
||||
double cur_theta = tf2::getYaw(path.poses[idx].pose.orientation);
|
||||
double next_theta = tf2::getYaw(path.poses[idx + 1].pose.orientation);
|
||||
double dtheta = angles::shortest_angular_distance(cur_theta, next_theta);
|
||||
if (fabs(ab_x) < 1e-4 && fabs(ab_y) < 1e-4 && fabs(dtheta) > 1e-4) {
|
||||
curr_segment.end = idx;
|
||||
segments.push_back(curr_segment);
|
||||
curr_segment.start = idx;
|
||||
}
|
||||
}
|
||||
|
||||
curr_segment.end = path.poses.size() - 1;
|
||||
segments.push_back(curr_segment);
|
||||
return segments;
|
||||
}
|
||||
|
||||
void Smoother::updateApproximatePathOrientations(
|
||||
nav_msgs::msg::Path & path,
|
||||
bool & reversing_segment)
|
||||
{
|
||||
double dx, dy, theta, pt_yaw;
|
||||
reversing_segment = false;
|
||||
|
||||
// Find if this path segment is in reverse
|
||||
dx = path.poses[2].pose.position.x - path.poses[1].pose.position.x;
|
||||
dy = path.poses[2].pose.position.y - path.poses[1].pose.position.y;
|
||||
theta = atan2(dy, dx);
|
||||
pt_yaw = tf2::getYaw(path.poses[1].pose.orientation);
|
||||
if (!is_holonomic_ && fabs(angles::shortest_angular_distance(pt_yaw, theta)) > M_PI_2) {
|
||||
reversing_segment = true;
|
||||
}
|
||||
|
||||
// Find the angle relative the path position vectors
|
||||
for (unsigned int i = 0; i != path.poses.size() - 1; i++) {
|
||||
dx = path.poses[i + 1].pose.position.x - path.poses[i].pose.position.x;
|
||||
dy = path.poses[i + 1].pose.position.y - path.poses[i].pose.position.y;
|
||||
theta = atan2(dy, dx);
|
||||
|
||||
// If points are overlapping, pass
|
||||
if (fabs(dx) < 1e-4 && fabs(dy) < 1e-4) {
|
||||
continue;
|
||||
}
|
||||
|
||||
// Flip the angle if this path segment is in reverse
|
||||
if (reversing_segment) {
|
||||
theta += M_PI; // orientationAroundZAxis will normalize
|
||||
}
|
||||
|
||||
path.poses[i].pose.orientation = orientationAroundZAxis(theta);
|
||||
}
|
||||
}
|
||||
|
||||
unsigned int Smoother::findShortestBoundaryExpansionIdx(
|
||||
const BoundaryExpansions & boundary_expansions)
|
||||
{
|
||||
// Check which is valid with the minimum integrated length such that
|
||||
// shorter end-points away that are infeasible to achieve without
|
||||
// a loop-de-loop are punished
|
||||
double min_length = 1e9;
|
||||
int shortest_boundary_expansion_idx = 1e9;
|
||||
for (unsigned int idx = 0; idx != boundary_expansions.size(); idx++) {
|
||||
if (boundary_expansions[idx].expansion_path_length<min_length &&
|
||||
!boundary_expansions[idx].in_collision &&
|
||||
boundary_expansions[idx].path_end_idx>0.0 &&
|
||||
boundary_expansions[idx].expansion_path_length > 0.0)
|
||||
{
|
||||
min_length = boundary_expansions[idx].expansion_path_length;
|
||||
shortest_boundary_expansion_idx = idx;
|
||||
}
|
||||
}
|
||||
|
||||
return shortest_boundary_expansion_idx;
|
||||
}
|
||||
|
||||
void Smoother::findBoundaryExpansion(
|
||||
const geometry_msgs::msg::Pose & start,
|
||||
const geometry_msgs::msg::Pose & end,
|
||||
BoundaryExpansion & expansion,
|
||||
const nav2_costmap_2d::Costmap2D * costmap)
|
||||
{
|
||||
static ompl::base::ScopedState<> from(state_space_), to(state_space_), s(state_space_);
|
||||
|
||||
from[0] = start.position.x;
|
||||
from[1] = start.position.y;
|
||||
from[2] = tf2::getYaw(start.orientation);
|
||||
to[0] = end.position.x;
|
||||
to[1] = end.position.y;
|
||||
to[2] = tf2::getYaw(end.orientation);
|
||||
|
||||
double d = state_space_->distance(from(), to());
|
||||
// If this path is too long compared to the original, then this is probably
|
||||
// a loop-de-loop, treat as invalid as to not deviate too far from the original path.
|
||||
// 2.0 selected from prinicipled choice of boundary test points
|
||||
// r, 2 * r, r * PI, and 2 * PI * r. If there is a loop, it will be
|
||||
// approximately 2 * PI * r, which is 2 * PI > r, PI > 2 * r, and 2 > r * PI.
|
||||
// For all but the last backup test point, a loop would be approximately
|
||||
// 2x greater than any of the selections.
|
||||
if (d > 2.0 * expansion.original_path_length) {
|
||||
return;
|
||||
}
|
||||
|
||||
std::vector<double> reals;
|
||||
double theta(0.0), x(0.0), y(0.0);
|
||||
double x_m = start.position.x;
|
||||
double y_m = start.position.y;
|
||||
|
||||
// Get intermediary poses
|
||||
for (double i = 0; i <= expansion.path_end_idx; i++) {
|
||||
state_space_->interpolate(from(), to(), i / expansion.path_end_idx, s());
|
||||
reals = s.reals();
|
||||
// Make sure in range [0, 2PI)
|
||||
theta = (reals[2] < 0.0) ? (reals[2] + 2.0 * M_PI) : reals[2];
|
||||
theta = (theta > 2.0 * M_PI) ? (theta - 2.0 * M_PI) : theta;
|
||||
x = reals[0];
|
||||
y = reals[1];
|
||||
|
||||
// Check for collision
|
||||
unsigned int mx, my;
|
||||
costmap->worldToMap(x, y, mx, my);
|
||||
if (static_cast<float>(costmap->getCost(mx, my)) >= INSCRIBED) {
|
||||
expansion.in_collision = true;
|
||||
}
|
||||
|
||||
// Integrate path length
|
||||
expansion.expansion_path_length += hypot(x - x_m, y - y_m);
|
||||
x_m = x;
|
||||
y_m = y;
|
||||
|
||||
// Store point
|
||||
expansion.pts.emplace_back(x, y, theta);
|
||||
}
|
||||
}
|
||||
|
||||
template<typename IteratorT>
|
||||
BoundaryExpansions Smoother::generateBoundaryExpansionPoints(IteratorT start, IteratorT end)
|
||||
{
|
||||
std::vector<double> distances = {
|
||||
min_turning_rad_, // Radius
|
||||
2.0 * min_turning_rad_, // Diameter
|
||||
M_PI * min_turning_rad_, // 50% Circumference
|
||||
2.0 * M_PI * min_turning_rad_ // Circumference
|
||||
};
|
||||
|
||||
BoundaryExpansions boundary_expansions;
|
||||
boundary_expansions.resize(distances.size());
|
||||
double curr_dist = 0.0;
|
||||
double x_last = start->pose.position.x;
|
||||
double y_last = start->pose.position.y;
|
||||
geometry_msgs::msg::Point pt;
|
||||
unsigned int curr_dist_idx = 0;
|
||||
|
||||
for (IteratorT iter = start; iter != end; iter++) {
|
||||
pt = iter->pose.position;
|
||||
curr_dist += hypot(pt.x - x_last, pt.y - y_last);
|
||||
x_last = pt.x;
|
||||
y_last = pt.y;
|
||||
|
||||
if (curr_dist >= distances[curr_dist_idx]) {
|
||||
boundary_expansions[curr_dist_idx].path_end_idx = iter - start;
|
||||
boundary_expansions[curr_dist_idx].original_path_length = curr_dist;
|
||||
curr_dist_idx++;
|
||||
}
|
||||
|
||||
if (curr_dist_idx == boundary_expansions.size()) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
return boundary_expansions;
|
||||
}
|
||||
|
||||
void Smoother::enforceStartBoundaryConditions(
|
||||
const geometry_msgs::msg::Pose & start_pose,
|
||||
nav_msgs::msg::Path & path,
|
||||
const nav2_costmap_2d::Costmap2D * costmap,
|
||||
const bool & reversing_segment)
|
||||
{
|
||||
// Find range of points for testing
|
||||
BoundaryExpansions boundary_expansions =
|
||||
generateBoundaryExpansionPoints<PathIterator>(path.poses.begin(), path.poses.end());
|
||||
|
||||
// Generate the motion model and metadata from start -> test points
|
||||
for (unsigned int i = 0; i != boundary_expansions.size(); i++) {
|
||||
BoundaryExpansion & expansion = boundary_expansions[i];
|
||||
if (expansion.path_end_idx == 0.0) {
|
||||
continue;
|
||||
}
|
||||
|
||||
if (!reversing_segment) {
|
||||
findBoundaryExpansion(
|
||||
start_pose, path.poses[expansion.path_end_idx].pose, expansion,
|
||||
costmap);
|
||||
} else {
|
||||
findBoundaryExpansion(
|
||||
path.poses[expansion.path_end_idx].pose, start_pose, expansion,
|
||||
costmap);
|
||||
}
|
||||
}
|
||||
|
||||
// Find the shortest kinematically feasible boundary expansion
|
||||
unsigned int best_expansion_idx = findShortestBoundaryExpansionIdx(boundary_expansions);
|
||||
if (best_expansion_idx > boundary_expansions.size()) {
|
||||
return;
|
||||
}
|
||||
|
||||
// Override values to match curve
|
||||
BoundaryExpansion & best_expansion = boundary_expansions[best_expansion_idx];
|
||||
if (reversing_segment) {
|
||||
std::reverse(best_expansion.pts.begin(), best_expansion.pts.end());
|
||||
}
|
||||
for (unsigned int i = 0; i != best_expansion.pts.size(); i++) {
|
||||
path.poses[i].pose.position.x = best_expansion.pts[i].x;
|
||||
path.poses[i].pose.position.y = best_expansion.pts[i].y;
|
||||
path.poses[i].pose.orientation = orientationAroundZAxis(best_expansion.pts[i].theta);
|
||||
}
|
||||
}
|
||||
|
||||
void Smoother::enforceEndBoundaryConditions(
|
||||
const geometry_msgs::msg::Pose & end_pose,
|
||||
nav_msgs::msg::Path & path,
|
||||
const nav2_costmap_2d::Costmap2D * costmap,
|
||||
const bool & reversing_segment)
|
||||
{
|
||||
// Find range of points for testing
|
||||
BoundaryExpansions boundary_expansions =
|
||||
generateBoundaryExpansionPoints<ReversePathIterator>(path.poses.rbegin(), path.poses.rend());
|
||||
|
||||
// Generate the motion model and metadata from start -> test points
|
||||
unsigned int expansion_starting_idx;
|
||||
for (unsigned int i = 0; i != boundary_expansions.size(); i++) {
|
||||
BoundaryExpansion & expansion = boundary_expansions[i];
|
||||
if (expansion.path_end_idx == 0.0) {
|
||||
continue;
|
||||
}
|
||||
expansion_starting_idx = path.poses.size() - expansion.path_end_idx - 1;
|
||||
if (!reversing_segment) {
|
||||
findBoundaryExpansion(path.poses[expansion_starting_idx].pose, end_pose, expansion, costmap);
|
||||
} else {
|
||||
findBoundaryExpansion(end_pose, path.poses[expansion_starting_idx].pose, expansion, costmap);
|
||||
}
|
||||
}
|
||||
|
||||
// Find the shortest kinematically feasible boundary expansion
|
||||
unsigned int best_expansion_idx = findShortestBoundaryExpansionIdx(boundary_expansions);
|
||||
if (best_expansion_idx > boundary_expansions.size()) {
|
||||
return;
|
||||
}
|
||||
|
||||
// Override values to match curve
|
||||
BoundaryExpansion & best_expansion = boundary_expansions[best_expansion_idx];
|
||||
if (reversing_segment) {
|
||||
std::reverse(best_expansion.pts.begin(), best_expansion.pts.end());
|
||||
}
|
||||
expansion_starting_idx = path.poses.size() - best_expansion.path_end_idx - 1;
|
||||
for (unsigned int i = 0; i != best_expansion.pts.size(); i++) {
|
||||
path.poses[expansion_starting_idx + i].pose.position.x = best_expansion.pts[i].x;
|
||||
path.poses[expansion_starting_idx + i].pose.position.y = best_expansion.pts[i].y;
|
||||
path.poses[expansion_starting_idx + i].pose.orientation = orientationAroundZAxis(
|
||||
best_expansion.pts[i].theta);
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace nav2_smac_planner
|
||||
Reference in New Issue
Block a user