Skip to content

Commit

Permalink
A* grid reimplemented as unordered_map for maximum efficiency
Browse files Browse the repository at this point in the history
  • Loading branch information
@jlblancoc
jlblancoc committed Sep 23, 2024
1 parent ee38778 commit 352bb7e
Show file tree
Hide file tree
Showing 2 changed files with 52 additions and 49 deletions.
47 changes: 19 additions & 28 deletions mrpt_path_planning/include/mpp/algos/TPS_Astar.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,6 @@
#include <mpp/algos/Planner.h>
#include <mpp/data/MotionPrimitivesTree.h>
#include <mrpt/core/bits_math.h> // 0.0_deg
#include <mrpt/poses/CPose2DGridTemplate.h>
#include <mrpt/system/COutputLogger.h>
#include <mrpt/system/CTimeLogger.h>

Expand Down Expand Up @@ -93,9 +92,8 @@ class TPS_Astar : virtual public mrpt::system::COutputLogger, public Planner
const SE2_KinState& from, const mrpt::math::TPoint2D& goal) const;

astar_heuristic_t heuristic = astar_heuristic_t(
[this](const SE2_KinState& from, const SE2orR2_KinState& goal) {
return this->default_heuristic(from, goal);
});
[this](const SE2_KinState& from, const SE2orR2_KinState& goal)
{ return this->default_heuristic(from, goal); });

private:
struct NodeCoords
Expand Down Expand Up @@ -181,14 +179,6 @@ class TPS_Astar : virtual public mrpt::system::COutputLogger, public Planner
using nodes_with_desired_speed_t =
std::unordered_map<NodeCoords, normalized_speed_t, NodeCoordsHash>;

using absolute_cell_index_t = size_t;

absolute_cell_index_t nodeCoordsToAbsIndex(const NodeCoords& n) const
{
return grid_.getSizeX() * grid_.getSizeY() * n.idxYaw.value() +
grid_.getSizeX() * n.idxY + n.idxX;
}

#if 0
mrpt::math::TPose2D nodeCoordsToPose(
const NodeCoords& n,
Expand Down Expand Up @@ -227,32 +217,31 @@ class TPS_Astar : virtual public mrpt::system::COutputLogger, public Planner
bool visited = false;
};

mrpt::poses::CPose2DGridTemplate<Node> grid_;
using SE2_Lattice = std::unordered_map<NodeCoords, Node, NodeCoordsHash>;

SE2_Lattice grid_;

int32_t x2idx(float x) const { return x / params_.grid_resolution_xy; }
int32_t y2idx(float y) const { return y / params_.grid_resolution_xy; }
int32_t phi2idx(float yaw) const
{
auto phi = mrpt::math::wrapToPi(yaw);
return phi / params_.grid_resolution_yaw;
}

/// throws on out of grid limits.
/// Returns a ref to the node.
Node& getOrCreateNodeByPose(
const mpp::SE2_KinState& p, mrpt::graphs::TNodeID& nextFreeId)
{
Node& n = *grid_.getByPos(p.pose.x, p.pose.y, p.pose.phi);
if (!n.id.has_value())
{
n.id = nextFreeId++;
n.state = p;
}

return n;
}
const mpp::SE2_KinState& p, mrpt::graphs::TNodeID& nextFreeId);

/// throws on out of grid limits.
NodeCoords nodeGridCoords(const mrpt::math::TPose2D& p) const
{
return NodeCoords(
grid_.x2idx(p.x), grid_.y2idx(p.y), grid_.phi2idx(p.phi));
return NodeCoords(x2idx(p.x), y2idx(p.y), phi2idx(p.phi));
}
NodeCoords nodeGridCoords(const mrpt::math::TPoint2D& p) const
{
return NodeCoords(grid_.x2idx(p.x), grid_.y2idx(p.y));
return NodeCoords(x2idx(p.x), y2idx(p.y));
}

struct NodePtr
Expand Down Expand Up @@ -308,7 +297,9 @@ class TPS_Astar : virtual public mrpt::system::COutputLogger, public Planner
const SE2orR2_KinState& goalState,
const std::vector<mrpt::maps::CPointsMap::Ptr>& globalObstacles,
double MAX_XY_OBSTACLES_CLIPPING_DIST,
const nodes_with_desired_speed_t& nodesWithSpeed);
const nodes_with_desired_speed_t& nodesWithSpeed,
const mrpt::math::TPose2D& worldBboxMin,
const mrpt::math::TPose2D& worldBboxMax);

mrpt::maps::CPointsMap::Ptr cached_local_obstacles(
const mrpt::math::TPose2D& queryPose,
Expand Down
54 changes: 33 additions & 21 deletions mrpt_path_planning/src/algos/TPS_Astar.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -128,16 +128,7 @@ PlannerOutput TPS_Astar::plan(const PlannerInput& in)
// ------------------------------------------------------------------
tree.edges_to_children.clear();

MRPT_LOG_DEBUG_STREAM("Initializing lattice...");
mrpt::system::CTimeLoggerEntry tle_lat(profiler_(), "plan.lattice");
grid_.setSize(
in.worldBboxMin.x, in.worldBboxMax.x, // x
in.worldBboxMin.y, in.worldBboxMax.y, // y
params_.grid_resolution_xy, params_.grid_resolution_yaw, // res
in.worldBboxMin.phi, in.worldBboxMax.phi // phi / yaw
);
tle_lat.stop();
MRPT_LOG_DEBUG_STREAM("Lattice init done.");
grid_.clear();

// ----------------------------------------
//
Expand Down Expand Up @@ -254,7 +245,7 @@ PlannerOutput TPS_Astar::plan(const PlannerInput& in)
// for each neighbor of current:
const auto neighbors = find_feasible_paths_to_neighbors(
current, in.ptgs, in.stateGoal, obstaclePoints, MAX_XY_DIST,
nodesWithDesiredSpeed);
nodesWithDesiredSpeed, in.worldBboxMin, in.worldBboxMax);

#if 0
std::cout << " cur : " << nodeGridCoords(current.state.pose).asString()
Expand Down Expand Up @@ -504,6 +495,19 @@ cost_t TPS_Astar::default_heuristic_R2(
return distR2 + params_.heuristic_heading_weight * distHeading;
}

TPS_Astar::Node& TPS_Astar::getOrCreateNodeByPose(
const SE2_KinState& p, mrpt::graphs::TNodeID& nextFreeId)
{
auto& n = grid_[NodeCoords(nodeGridCoords(p.pose))];

if (!n.id.has_value())
{
n.id = nextFreeId++;
n.state = p;
}
return n;
}

cost_t TPS_Astar::default_heuristic(
const SE2_KinState& from, const SE2orR2_KinState& goal) const
{
Expand All @@ -521,7 +525,9 @@ TPS_Astar::list_paths_to_neighbors_t
const SE2orR2_KinState& goalState,
const std::vector<mrpt::maps::CPointsMap::Ptr>& globalObstacles,
double MAX_XY_OBSTACLES_CLIPPING_DIST,
const nodes_with_desired_speed_t& nodesWithSpeed)
const nodes_with_desired_speed_t& nodesWithSpeed,
const mrpt::math::TPose2D& worldBboxMin,
const mrpt::math::TPose2D& worldBboxMax)
{
mrpt::system::CTimeLoggerEntry tle(profiler_(), "find_feasible");

Expand All @@ -533,17 +539,19 @@ TPS_Astar::list_paths_to_neighbors_t

const auto relGoal = goalState.asSE2KinState().pose - from.state.pose;

const double halfCell = grid_.getResolutionXY() * 0.5;
const double halfCell = params_.grid_resolution_xy * 0.5;

// local obstacles as seen from this "from" pose:
const auto localObstacles = cached_local_obstacles(
from.state.pose, globalObstacles, MAX_XY_OBSTACLES_CLIPPING_DIST);

// If two PTGs reach the same cell, keep the shortest/best:
std::map<absolute_cell_index_t, path_to_neighbor_t> bestPaths;
std::unordered_map<NodeCoords, path_to_neighbor_t, NodeCoordsHash>
bestPaths;

#if 0
size_t totalConsidered = 0, totalCollided = 0;

#endif
// For each PTG:
for (size_t ptgIdx = 0; ptgIdx < trs.ptgs.size(); ptgIdx++)
{
Expand Down Expand Up @@ -686,7 +694,9 @@ TPS_Astar::list_paths_to_neighbors_t
{
const auto& tpsPt = tpsPointsToConsider[tpsPtIdx];

#if 0
totalConsidered++;
#endif

// solution is a no-motion: skip.
if (tpsPt.step == 0) continue;
Expand All @@ -701,12 +711,12 @@ TPS_Astar::list_paths_to_neighbors_t
const auto absPose = from.state.pose + relReconstrPose;

// out of lattice limits?
if (absPose.x < grid_.getXMin() || absPose.y < grid_.getYMin() ||
absPose.phi < grid_.getPhiMin())
if (absPose.x < worldBboxMin.x || absPose.y < worldBboxMin.y ||
absPose.phi < worldBboxMin.phi)
continue;
if (absPose.x > grid_.getXMax() - halfCell ||
absPose.y > grid_.getYMax() - halfCell ||
absPose.phi > grid_.getPhiMax())
if (absPose.x > worldBboxMax.x - halfCell ||
absPose.y > worldBboxMax.y - halfCell ||
absPose.phi > worldBboxMax.phi)
continue;

const NodeCoords nc = nodeGridCoords(absPose);
Expand All @@ -724,7 +734,9 @@ TPS_Astar::list_paths_to_neighbors_t
{
// we would need to move farther away than what is possible
// without colliding: discard this trajectory.
#if 0
totalCollided++;
#endif
continue;
}

Expand Down Expand Up @@ -752,7 +764,7 @@ TPS_Astar::list_paths_to_neighbors_t
// ok, it's a good potential path, add it.
// It will be later on scored by the A* algo.

auto& path = bestPaths[nodeCoordsToAbsIndex(nc)];
auto& path = bestPaths[nc];

// Ok, it's a valid new neighbor with this PTG.
// Is it shorter with this PTG than with others?
Expand Down

0 comments on commit 352bb7e

Please sign in to comment.