Program Listing for File node_hybrid.hpp
↰ Return to documentation for file (include/nav2_smac_planner/node_hybrid.hpp
)
// 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.
#ifndef NAV2_SMAC_PLANNER__NODE_HYBRID_HPP_
#define NAV2_SMAC_PLANNER__NODE_HYBRID_HPP_
#include <math.h>
#include <vector>
#include <cmath>
#include <iostream>
#include <functional>
#include <queue>
#include <memory>
#include <utility>
#include <limits>
#include "ompl/base/StateSpace.h"
#include "nav2_smac_planner/constants.hpp"
#include "nav2_smac_planner/types.hpp"
#include "nav2_smac_planner/collision_checker.hpp"
#include "nav2_smac_planner/costmap_downsampler.hpp"
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
#include "nav2_costmap_2d/inflation_layer.hpp"
namespace nav2_smac_planner
{
typedef std::vector<float> LookupTable;
typedef std::pair<double, double> TrigValues;
typedef std::pair<float, uint64_t> ObstacleHeuristicElement;
struct ObstacleHeuristicComparator
{
bool operator()(const ObstacleHeuristicElement & a, const ObstacleHeuristicElement & b) const
{
return a.first > b.first;
}
};
typedef std::vector<ObstacleHeuristicElement> ObstacleHeuristicQueue;
// Must forward declare
class NodeHybrid;
struct HybridMotionTable
{
HybridMotionTable() {}
void initDubin(
unsigned int & size_x_in,
unsigned int & size_y_in,
unsigned int & angle_quantization_in,
SearchInfo & search_info);
void initReedsShepp(
unsigned int & size_x_in,
unsigned int & size_y_in,
unsigned int & angle_quantization_in,
SearchInfo & search_info);
MotionPoses getProjections(const NodeHybrid * node);
unsigned int getClosestAngularBin(const double & theta);
float getAngleFromBin(const unsigned int & bin_idx);
MotionModel motion_model = MotionModel::UNKNOWN;
MotionPoses projections;
unsigned int size_x;
unsigned int num_angle_quantization;
float num_angle_quantization_float;
float min_turning_radius;
float bin_size;
float change_penalty;
float non_straight_penalty;
float cost_penalty;
float reverse_penalty;
float travel_distance_reward;
bool downsample_obstacle_heuristic;
bool use_quadratic_cost_penalty;
ompl::base::StateSpacePtr state_space;
std::vector<std::vector<double>> delta_xs;
std::vector<std::vector<double>> delta_ys;
std::vector<TrigValues> trig_values;
std::vector<float> travel_costs;
};
class NodeHybrid
{
public:
typedef NodeHybrid * NodePtr;
typedef std::unique_ptr<std::vector<NodeHybrid>> Graph;
typedef std::vector<NodePtr> NodeVector;
struct Coordinates
{
Coordinates() {}
Coordinates(const float & x_in, const float & y_in, const float & theta_in)
: x(x_in), y(y_in), theta(theta_in)
{}
inline bool operator==(const Coordinates & rhs)
{
return this->x == rhs.x && this->y == rhs.y && this->theta == rhs.theta;
}
inline bool operator!=(const Coordinates & rhs)
{
return !(*this == rhs);
}
float x, y, theta;
};
typedef std::vector<Coordinates> CoordinateVector;
explicit NodeHybrid(const uint64_t index);
~NodeHybrid();
bool operator==(const NodeHybrid & rhs)
{
return this->_index == rhs._index;
}
inline void setPose(const Coordinates & pose_in)
{
pose = pose_in;
}
void reset();
inline float getAccumulatedCost()
{
return _accumulated_cost;
}
inline void setAccumulatedCost(const float & cost_in)
{
_accumulated_cost = cost_in;
}
inline void setMotionPrimitiveIndex(const unsigned int & idx, const TurnDirection & turn_dir)
{
_motion_primitive_index = idx;
_turn_dir = turn_dir;
}
inline unsigned int & getMotionPrimitiveIndex()
{
return _motion_primitive_index;
}
inline TurnDirection & getTurnDirection()
{
return _turn_dir;
}
inline float getCost()
{
return _cell_cost;
}
inline bool wasVisited()
{
return _was_visited;
}
inline void visited()
{
_was_visited = true;
}
inline uint64_t getIndex()
{
return _index;
}
bool isNodeValid(const bool & traverse_unknown, GridCollisionChecker * collision_checker);
float getTraversalCost(const NodePtr & child);
static inline uint64_t getIndex(
const unsigned int & x, const unsigned int & y, const unsigned int & angle,
const unsigned int & width, const unsigned int & angle_quantization)
{
return static_cast<uint64_t>(angle) + static_cast<uint64_t>(x) *
static_cast<uint64_t>(angle_quantization) +
static_cast<uint64_t>(y) * static_cast<uint64_t>(width) *
static_cast<uint64_t>(angle_quantization);
}
static inline uint64_t getIndex(
const unsigned int & x, const unsigned int & y, const unsigned int & angle)
{
return getIndex(
x, y, angle, motion_table.size_x,
motion_table.num_angle_quantization);
}
static inline Coordinates getCoords(
const uint64_t & index,
const unsigned int & width, const unsigned int & angle_quantization)
{
return Coordinates(
(index / angle_quantization) % width, // x
index / (angle_quantization * width), // y
index % angle_quantization); // theta
}
static float getHeuristicCost(
const Coordinates & node_coords,
const Coordinates & goal_coordinates);
static void initMotionModel(
const MotionModel & motion_model,
unsigned int & size_x,
unsigned int & size_y,
unsigned int & angle_quantization,
SearchInfo & search_info);
static void precomputeDistanceHeuristic(
const float & lookup_table_dim,
const MotionModel & motion_model,
const unsigned int & dim_3_size,
const SearchInfo & search_info);
static float getObstacleHeuristic(
const Coordinates & node_coords,
const Coordinates & goal_coords,
const float & cost_penalty);
static float getDistanceHeuristic(
const Coordinates & node_coords,
const Coordinates & goal_coords,
const float & obstacle_heuristic);
static void resetObstacleHeuristic(
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros,
const unsigned int & start_x, const unsigned int & start_y,
const unsigned int & goal_x, const unsigned int & goal_y);
static float adjustedFootprintCost(const float & cost);
void getNeighbors(
std::function<bool(const uint64_t &,
nav2_smac_planner::NodeHybrid * &)> & validity_checker,
GridCollisionChecker * collision_checker,
const bool & traverse_unknown,
NodeVector & neighbors);
bool backtracePath(CoordinateVector & path);
static void destroyStaticAssets()
{
inflation_layer.reset();
costmap_ros.reset();
}
NodeHybrid * parent;
Coordinates pose;
// Constants required across all nodes but don't want to allocate more than once
static float travel_distance_cost;
static HybridMotionTable motion_table;
// Wavefront lookup and queue for continuing to expand as needed
static LookupTable obstacle_heuristic_lookup_table;
static ObstacleHeuristicQueue obstacle_heuristic_queue;
static std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros;
static std::shared_ptr<nav2_costmap_2d::InflationLayer> inflation_layer;
// Dubin / Reeds-Shepp lookup and size for dereferencing
static LookupTable dist_heuristic_lookup_table;
static float size_lookup;
private:
float _cell_cost;
float _accumulated_cost;
uint64_t _index;
bool _was_visited;
unsigned int _motion_primitive_index;
TurnDirection _turn_dir;
};
} // namespace nav2_smac_planner
#endif // NAV2_SMAC_PLANNER__NODE_HYBRID_HPP_