Program Listing for File node_2d.hpp
↰ Return to documentation for file (include/nav2_smac_planner/node_2d.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_2D_HPP_
#define NAV2_SMAC_PLANNER__NODE_2D_HPP_
#include <math.h>
#include <vector>
#include <iostream>
#include <memory>
#include <queue>
#include <limits>
#include <utility>
#include <functional>
#include "nav2_smac_planner/types.hpp"
#include "nav2_smac_planner/constants.hpp"
#include "nav2_smac_planner/collision_checker.hpp"
#include "nav2_smac_planner/node_hybrid.hpp"
namespace nav2_smac_planner
{
class Node2D
{
public:
typedef Node2D * NodePtr;
typedef std::unique_ptr<std::vector<Node2D>> Graph;
typedef std::vector<NodePtr> NodeVector;
struct Coordinates
{
Coordinates() {}
Coordinates(const float & x_in, const float & y_in)
: x(x_in), y(y_in)
{}
float x, y;
};
typedef std::vector<Coordinates> CoordinateVector;
explicit Node2D(const uint64_t index);
~Node2D();
bool operator==(const Node2D & rhs)
{
return this->_index == rhs._index;
}
void reset();
inline float getAccumulatedCost()
{
return _accumulated_cost;
}
inline void setAccumulatedCost(const float & cost_in)
{
_accumulated_cost = cost_in;
}
inline float getCost()
{
return _cell_cost;
}
inline void setCost(const float & cost)
{
_cell_cost = cost;
}
inline bool wasVisited()
{
return _was_visited;
}
inline void visited()
{
_was_visited = true;
_is_queued = false;
}
inline bool & isQueued()
{
return _is_queued;
}
inline void queued()
{
_is_queued = 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 & width)
{
return static_cast<uint64_t>(x) + static_cast<uint64_t>(y) *
static_cast<uint64_t>(width);
}
static inline Coordinates getCoords(
const uint64_t & index, const unsigned int & width, const unsigned int & angles)
{
if (angles != 1) {
throw std::runtime_error("Node type Node2D does not have a valid angle quantization.");
}
return Coordinates(index % width, index / width);
}
static inline Coordinates getCoords(const uint64_t & index)
{
const unsigned int & size_x = _neighbors_grid_offsets[3];
return Coordinates(index % size_x, index / size_x);
}
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 & num_angle_quantization,
SearchInfo & search_info);
void getNeighbors(
std::function<bool(const uint64_t &,
nav2_smac_planner::Node2D * &)> & validity_checker,
GridCollisionChecker * collision_checker,
const bool & traverse_unknown,
NodeVector & neighbors);
bool backtracePath(CoordinateVector & path);
Node2D * parent;
static float cost_travel_multiplier;
static std::vector<int> _neighbors_grid_offsets;
private:
float _cell_cost;
float _accumulated_cost;
uint64_t _index;
bool _was_visited;
bool _is_queued;
};
} // namespace nav2_smac_planner
#endif // NAV2_SMAC_PLANNER__NODE_2D_HPP_