diff --git a/mrpt_path_planning/include/mpp/algos/TPS_Astar.h b/mrpt_path_planning/include/mpp/algos/TPS_Astar.h index f3b64de..836b5fe 100644 --- a/mrpt_path_planning/include/mpp/algos/TPS_Astar.h +++ b/mrpt_path_planning/include/mpp/algos/TPS_Astar.h @@ -10,7 +10,6 @@ #include #include #include // 0.0_deg -#include #include #include @@ -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 @@ -181,14 +179,6 @@ class TPS_Astar : virtual public mrpt::system::COutputLogger, public Planner using nodes_with_desired_speed_t = std::unordered_map; - 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, @@ -227,32 +217,31 @@ class TPS_Astar : virtual public mrpt::system::COutputLogger, public Planner bool visited = false; }; - mrpt::poses::CPose2DGridTemplate grid_; + using SE2_Lattice = std::unordered_map; + + 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 @@ -308,7 +297,9 @@ class TPS_Astar : virtual public mrpt::system::COutputLogger, public Planner const SE2orR2_KinState& goalState, const std::vector& 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, diff --git a/mrpt_path_planning/src/algos/TPS_Astar.cpp b/mrpt_path_planning/src/algos/TPS_Astar.cpp index b277595..e6b039d 100644 --- a/mrpt_path_planning/src/algos/TPS_Astar.cpp +++ b/mrpt_path_planning/src/algos/TPS_Astar.cpp @@ -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(); // ---------------------------------------- // @@ -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() @@ -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 { @@ -521,7 +525,9 @@ TPS_Astar::list_paths_to_neighbors_t const SE2orR2_KinState& goalState, const std::vector& 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"); @@ -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 bestPaths; + std::unordered_map + bestPaths; +#if 0 size_t totalConsidered = 0, totalCollided = 0; - +#endif // For each PTG: for (size_t ptgIdx = 0; ptgIdx < trs.ptgs.size(); ptgIdx++) { @@ -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; @@ -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); @@ -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; } @@ -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?