Skip to content

Commit

Permalink
faster method; added a to-do note
Browse files Browse the repository at this point in the history
  • Loading branch information
@jlblancoc
jlblancoc committed Apr 5, 2021
1 parent e8eebc2 commit b917d4a
Show file tree
Hide file tree
Showing 3 changed files with 139 additions and 66 deletions.
43 changes: 33 additions & 10 deletions libselfdriving/include/selfdriving/TPS_RRTstar.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -70,12 +70,18 @@ class TPS_RRTstar : public mrpt::system::COutputLogger
const TNodeID goalNodeId_;
};

mrpt::math::TPose2D draw_random_free_pose(const DrawFreePoseParams& p);
using closest_lie_nodes_list_t = std::map<
distance_t,
std::reference_wrapper<const MotionPrimitivesTreeSE2::node_t>>;

using draw_pose_return_t =
std::tuple<mrpt::math::TPose2D, closest_lie_nodes_list_t>;

mrpt::math::TPose2D draw_random_tps(const DrawFreePoseParams& p);
mrpt::math::TPose2D draw_random_euclidean(const DrawFreePoseParams& p);
draw_pose_return_t draw_random_free_pose(const DrawFreePoseParams& p);
draw_pose_return_t draw_random_tps(const DrawFreePoseParams& p);
draw_pose_return_t draw_random_euclidean(const DrawFreePoseParams& p);

using closest_nodes_list_t = std::map<
using path_to_nodes_list_t = std::map<
distance_t,
std::tuple<TNodeID, ptg_index_t, trajectory_index_t, distance_t>>;

Expand All @@ -84,14 +90,30 @@ class TPS_RRTstar : public mrpt::system::COutputLogger
* such motions. Note that, at this point, `query` does not have a velocity
* state: it will be determined by the best motion primitive.
*
* This method is used in the EXTEND stage of RRT*.
* This method is used in the EXTEND stage of RRT*, and uses the TP-Space
* motion primitives (PTGs).
*
* \sa find_reachable_nodes_from()
* \sa find_reachable_nodes_from(), find_nearby_nodes()
*/
closest_nodes_list_t find_source_nodes_towards(
path_to_nodes_list_t find_source_nodes_towards(
const MotionPrimitivesTreeSE2& tree, const mrpt::math::TPose2D& query,
const double maxDistance, const TrajectoriesAndRobotShape& trs,
const TNodeID goalNodeToIgnore);
const TNodeID goalNodeToIgnore,
const closest_lie_nodes_list_t& hintCloseNodes);

/** Find all existing nodes "x" in the tree within a given ball, given by
* the metric on the Lie group, i.e. *not* following any particular PTG
* trajectory.
*
* \sa find_reachable_nodes_from(), find_source_nodes_towards()
*/
closest_lie_nodes_list_t find_nearby_nodes(
const MotionPrimitivesTreeSE2& tree, const mrpt::math::TPose2D& query,
const double maxDistance);

distance_t find_closest_node(
const MotionPrimitivesTreeSE2& tree,
const mrpt::math::TPose2D& query) const;

/** Find all existing nodes "x" in the tree that are **reachable from**
* `query` (i.e. `query` ==> `other nodes`), and the motion primitives for
Expand All @@ -101,9 +123,10 @@ class TPS_RRTstar : public mrpt::system::COutputLogger
*
* \sa find_source_nodes_towards()
*/
closest_nodes_list_t find_reachable_nodes_from(
path_to_nodes_list_t find_reachable_nodes_from(
const MotionPrimitivesTreeSE2& tree, const TNodeID queryNodeId,
const double maxDistance, const TrajectoriesAndRobotShape& trs);
const double maxDistance, const TrajectoriesAndRobotShape& trs,
const closest_lie_nodes_list_t& hintCloseNodes);

/** Returns local obstacles as seen from a given pose, clipped to a maximum
* distance. */
Expand Down
160 changes: 105 additions & 55 deletions libselfdriving/src/TPS_RRTstar.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -157,13 +157,13 @@ PlannerOutput TPS_RRTstar::plan(const PlannerInput& in)
// 4 | q_i ← SAMPLE( Q_free )
// ------------------------------------------------------------------
// issue: What about dynamic obstacles that depend on time?
const mrpt::math::TPose2D qi = draw_random_free_pose(drawParams);
const auto [qi, qiNearbyNodes] = draw_random_free_pose(drawParams);

// 5 | {x_best, x_i} ← argmin{x ∈ Tree | cost[x, q_i ] < r ∧
// CollisionFree(pi(x,q_i)}( cost[x] + cost[x,x_i] )
// ------------------------------------------------------------------
const closest_nodes_list_t closeNodes = find_source_nodes_towards(
tree, qi, searchRadius, in.ptgs, goalNodeId);
const path_to_nodes_list_t closeNodes = find_source_nodes_towards(
tree, qi, searchRadius, in.ptgs, goalNodeId, qiNearbyNodes);

if (closeNodes.empty()) continue; // No body around?

Expand Down Expand Up @@ -298,8 +298,8 @@ PlannerOutput TPS_RRTstar::plan(const PlannerInput& in)
// 9 | cost[x] ← cost[x_i] + cost[x_i, x]
// 10 | parent[x] ← x_i
// ------------------------------------------------------------------
const closest_nodes_list_t reachableNodes =
find_reachable_nodes_from(tree, newNodeId, searchRadius, in.ptgs);
const path_to_nodes_list_t reachableNodes = find_reachable_nodes_from(
tree, newNodeId, searchRadius, in.ptgs, qiNearbyNodes);

// Check collisions:
const auto& localObstaclesNewNode = cached_local_obstacles(
Expand Down Expand Up @@ -394,11 +394,12 @@ PlannerOutput TPS_RRTstar::plan(const PlannerInput& in)
}
}

MRPT_LOG_DEBUG_STREAM(
"iter: " << rrtIter << ", " << closeNodes.size()
<< " candidate nodes near qi=" << qi.asString() << ", "
<< nValidCandidateSourceNodes << " valid, rewired "
<< nRewired);
MRPT_LOG_DEBUG_FMT(
"iter: %5u qi=%40s candidates/evaluated/rewired= %3u/%3u/%3u",
static_cast<unsigned int>(rrtIter), qi.asString().c_str(),
static_cast<unsigned int>(closeNodes.size()),
static_cast<unsigned int>(nValidCandidateSourceNodes),
static_cast<unsigned int>(nRewired));

// Debug log files:
if (params_.saveDebugVisualizationDecimation > 0 &&
Expand Down Expand Up @@ -503,7 +504,7 @@ distance_t TPS_RRTstar::tp_obstacles_single_path(
MRPT_END
}

mrpt::math::TPose2D TPS_RRTstar::draw_random_free_pose(
TPS_RRTstar::draw_pose_return_t TPS_RRTstar::draw_random_free_pose(
const TPS_RRTstar::DrawFreePoseParams& p)
{
auto tle =
Expand All @@ -515,7 +516,7 @@ mrpt::math::TPose2D TPS_RRTstar::draw_random_free_pose(
return draw_random_euclidean(p);
}

mrpt::math::TPose2D TPS_RRTstar::draw_random_euclidean(
TPS_RRTstar::draw_pose_return_t TPS_RRTstar::draw_random_euclidean(
const TPS_RRTstar::DrawFreePoseParams& p)
{
auto tle = mrpt::system::CTimeLoggerEntry(
Expand All @@ -538,6 +539,16 @@ mrpt::math::TPose2D TPS_RRTstar::draw_random_euclidean(
rng.drawUniform(bbMin.y, bbMax.y),
rng.drawUniform(bbMin.phi, bbMax.phi));

closest_lie_nodes_list_t closeNodes =
find_nearby_nodes(p.tree_, q, p.searchRadius_ * 1.2);

const double minFoundDistance = closeNodes.empty()
? params_.minStepLength
: closeNodes.begin()->first;

// We must ensure a minimum clearance:
if (minFoundDistance < params_.minStepLength) continue;

// TODO: More flexible check? Variable no. of points?
mrpt::math::TPoint2D closestObs;
float closestDistSqr;
Expand All @@ -548,12 +559,12 @@ mrpt::math::TPose2D TPS_RRTstar::draw_random_euclidean(
const bool isCollision =
selfdriving::obstaclePointCollides(closestObsWrtRobot, p.pi_.ptgs);

if (!isCollision) return q;
if (!isCollision) return {q, closeNodes};
}
THROW_EXCEPTION("Could not draw collision-free random pose!");
}

mrpt::math::TPose2D TPS_RRTstar::draw_random_tps(
TPS_RRTstar::draw_pose_return_t TPS_RRTstar::draw_random_tps(
const TPS_RRTstar::DrawFreePoseParams& p)
{
auto tle =
Expand Down Expand Up @@ -615,31 +626,18 @@ mrpt::math::TPose2D TPS_RRTstar::draw_random_tps(
// In this case, do NOT use TPS, but the real SE(2) metric space,
// to avoid the lack of existing paths to hide nodes that are really
// close to this tentative pose sample:
// TODO: Use KD-tree with nanoflann!

double minFoundDistance = params_.minStepLength;

PoseDistanceMetric_Lie<SE2_KinState> distEvaluator;
const distance_t minFoundDistance = find_closest_node(p.tree_, q);

for (const auto& node : p.tree_.nodes())
// We must ensure a minimum clearance:
if (minFoundDistance < params_.minStepLength)
{
const SE2_KinState& nodeState = node.second;
auto& de = distEvaluator;

// Skip the more expensive calculation of exact distance:
if (de.cannotBeNearerThan(q, nodeState.pose, minFoundDistance))
{
// It's too far, skip:
continue;
}

const auto d = de.distance(q, nodeState.pose);
mrpt::keep_min(minFoundDistance, d);
MRPT_TODO("Return the ID of the existing node and rethink it.");
MRPT_LOG_DEBUG_FMT(
"Draw TPS pose: min. distance: %f", minFoundDistance);
continue;
}

// We must ensure a minimum clearance:
if (minFoundDistance < params_.minStepLength) continue;

// Approximate check for collisions:
// TODO: More flexible check? Variable no. of points?
mrpt::math::TPoint2D closestObs;
Expand All @@ -654,17 +652,21 @@ mrpt::math::TPose2D TPS_RRTstar::draw_random_tps(
if (!isCollision)
{
// Ok, good sample has been drawn:
return q;
closest_lie_nodes_list_t closeNodes =
find_nearby_nodes(p.tree_, q, p.searchRadius_ * 1.2);

return {q, closeNodes};
}
}
THROW_EXCEPTION("Could not draw collision-free random pose!");
}

// See docs in .h
TPS_RRTstar::closest_nodes_list_t TPS_RRTstar::find_source_nodes_towards(
TPS_RRTstar::path_to_nodes_list_t TPS_RRTstar::find_source_nodes_towards(
const MotionPrimitivesTreeSE2& tree, const mrpt::math::TPose2D& query,
const double maxDistance, const TrajectoriesAndRobotShape& trs,
const TNodeID goalNodeToIgnore)
const TNodeID goalNodeToIgnore,
const closest_lie_nodes_list_t& hintCloseNodes)
{
auto tle =
mrpt::system::CTimeLoggerEntry(profiler_, "find_source_nodes_towards");
Expand All @@ -680,15 +682,15 @@ TPS_RRTstar::closest_nodes_list_t TPS_RRTstar::find_source_nodes_towards(
for (auto& ptg : trs.ptgs)
distEvaluators.emplace_back(*ptg, params_.headingToleranceMetric);

closest_nodes_list_t closestNodes;
path_to_nodes_list_t closestNodes;

// TODO: Use KD-tree with nanoflann!

for (const auto& node : nodes)
for (const auto& distNodeId : hintCloseNodes)
{
if (node.first == goalNodeToIgnore) continue; // ignore
const auto nodeId = distNodeId.second.get().nodeID_;

const SE2_KinState& nodeState = node.second;
if (nodeId == goalNodeToIgnore) continue; // ignore

const SE2_KinState& nodeState = tree.nodes().at(nodeId);

for (ptg_index_t ptgIdx = 0; ptgIdx < distEvaluators.size(); ptgIdx++)
{
Expand Down Expand Up @@ -724,8 +726,8 @@ TPS_RRTstar::closest_nodes_list_t TPS_RRTstar::find_source_nodes_towards(
}
// Ok, accept it:
closestNodes.emplace(
distance, closest_nodes_list_t::mapped_type(
node.first, ptgIdx, trajIndex, distance));
distance, path_to_nodes_list_t::mapped_type(
nodeId, ptgIdx, trajIndex, distance));
}
}
return closestNodes;
Expand All @@ -734,9 +736,10 @@ TPS_RRTstar::closest_nodes_list_t TPS_RRTstar::find_source_nodes_towards(
// See docs in .h
// This is mostly similar to find_source_nodes_towards(), but with
// reversed order between source and target states.
TPS_RRTstar::closest_nodes_list_t TPS_RRTstar::find_reachable_nodes_from(
TPS_RRTstar::path_to_nodes_list_t TPS_RRTstar::find_reachable_nodes_from(
const MotionPrimitivesTreeSE2& tree, const TNodeID queryNodeId,
const double maxDistance, const TrajectoriesAndRobotShape& trs)
const double maxDistance, const TrajectoriesAndRobotShape& trs,
const closest_lie_nodes_list_t& hintCloseNodes)
{
auto tle =
mrpt::system::CTimeLoggerEntry(profiler_, "find_reachable_nodes_from");
Expand All @@ -756,16 +759,17 @@ TPS_RRTstar::closest_nodes_list_t TPS_RRTstar::find_reachable_nodes_from(
for (auto& ptg : trs.ptgs)
distEvaluators.emplace_back(*ptg, params_.headingToleranceMetric);

closest_nodes_list_t closestNodes;

// TODO: Use KD-tree with nanoflann!
path_to_nodes_list_t closestNodes;

for (const auto& node : nodes)
for (const auto& distNodeId : hintCloseNodes)
{
const SE2_KinState& nodeState = node.second;
const auto& node = distNodeId.second.get();
const auto nodeId = node.nodeID_;

const SE2_KinState& nodeState = node;

// Don't rewire to myself ;-)
if (node.first == queryNodeId) continue;
if (nodeId == queryNodeId) continue;

for (ptg_index_t ptgIdx = 0; ptgIdx < distEvaluators.size(); ptgIdx++)
{
Expand Down Expand Up @@ -796,8 +800,8 @@ TPS_RRTstar::closest_nodes_list_t TPS_RRTstar::find_reachable_nodes_from(
}
// Ok, accept it:
closestNodes.emplace(
distance, closest_nodes_list_t::mapped_type(
node.first, ptgIdx, trajIndex, distance));
distance, path_to_nodes_list_t::mapped_type(
nodeId, ptgIdx, trajIndex, distance));
}
}
return closestNodes;
Expand Down Expand Up @@ -834,3 +838,49 @@ cost_t TPS_RRTstar::cost_path_segment(const MoveEdgeSE2_TPS& edge) const
{
return edge.ptgDist;
}

TPS_RRTstar::closest_lie_nodes_list_t TPS_RRTstar::find_nearby_nodes(
const MotionPrimitivesTreeSE2& tree, const mrpt::math::TPose2D& query,
const double maxDistance)
{
auto tle = mrpt::system::CTimeLoggerEntry(profiler_, "find_nearby_nodes");

closest_lie_nodes_list_t out;
PoseDistanceMetric_Lie<SE2_KinState> de;

// TODO: Use KD-tree with nanoflann!
for (const auto& node : tree.nodes())
{
const SE2_KinState& nodeState = node.second;

// Skip the more expensive calculation of exact distance:
if (de.cannotBeNearerThan(query, nodeState.pose, maxDistance))
continue; // It's too far, skip:

if (auto d = de.distance(query, nodeState.pose); d < maxDistance)
out.emplace(d, std::ref(node.second));
}
return out;
}

distance_t TPS_RRTstar::find_closest_node(
const MotionPrimitivesTreeSE2& tree, const mrpt::math::TPose2D& query) const
{
ASSERT_(!tree.nodes().empty());

distance_t minDist = std::numeric_limits<distance_t>::max();
PoseDistanceMetric_Lie<SE2_KinState> de;

for (const auto& node : tree.nodes())
{
const SE2_KinState& nodeState = node.second;

// Skip the more expensive calculation of exact distance:
if (de.cannotBeNearerThan(query, nodeState.pose, minDist))
continue; // It's too far, skip:

const auto d = de.distance(query, nodeState.pose);
mrpt::keep_min(minDist, d);
}
return minDist;
}
2 changes: 1 addition & 1 deletion share/ptgs_holonomic_robot.ini
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,7 @@
# Max angular vel (deg/s):
@define ROBOT_MAX_W 60.0
# Max distance to "foresee" obstacles (m):
@define NAV_MAX_REF_DIST 3.0
@define NAV_MAX_REF_DIST 1.5


[SelfDriving]
Expand Down

0 comments on commit b917d4a

Please sign in to comment.