diff --git a/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/abstract_algorithm.hpp b/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/abstract_algorithm.hpp index 17171882f0bda..b59888fa52281 100644 --- a/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/abstract_algorithm.hpp +++ b/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/abstract_algorithm.hpp @@ -114,7 +114,9 @@ class AbstractPlanningAlgorithm virtual ~AbstractPlanningAlgorithm() {} protected: - void computeCollisionIndexes(int theta_index, std::vector & indexes) const; + void computeCollisionIndexes( + int theta_index, std::vector & indexes, + std::vector & vertex_indexes_2d) const; bool detectCollision(const IndexXYT & base_index) const; inline bool isOutOfRange(const IndexXYT & index) const { @@ -143,6 +145,9 @@ class AbstractPlanningAlgorithm // collision indexes cache std::vector> coll_indexes_table_; + // vehicle vertex indexes cache + std::vector> vertex_indexes_table_; + // is_obstacle's table std::vector> is_obstacle_table_; diff --git a/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/astar_search.hpp b/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/astar_search.hpp index 2f2328c1a43da..9ccec85bfc185 100644 --- a/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/astar_search.hpp +++ b/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/astar_search.hpp @@ -29,6 +29,7 @@ #include #include #include +#include #include namespace freespace_planning_algorithms @@ -130,6 +131,11 @@ class AstarSearch : public AbstractPlanningAlgorithm const PlannerWaypoints & getWaypoints() const { return waypoints_; } + inline int getKey(const IndexXYT & index) + { + return (index.theta + (index.y * x_scale_ + index.x) * y_scale_); + } + private: bool search(); void clearNodes(); @@ -139,14 +145,18 @@ class AstarSearch : public AbstractPlanningAlgorithm double estimateCost(const geometry_msgs::msg::Pose & pose) const; bool isGoal(const AstarNode & node) const; - AstarNode * getNodeRef(const IndexXYT & index) { return &nodes_[index.y][index.x][index.theta]; } + AstarNode * getNodeRef(const IndexXYT & index) + { + return &(graph_.emplace(getKey(index), AstarNode()).first->second); + } // Algorithm specific param AstarParam astar_param_; // hybrid astar variables TransitionTable transition_table_; - std::vector>> nodes_; + std::unordered_map graph_; + std::priority_queue, NodeComparison> openlist_; // goal node, which may helpful in testing and debugging @@ -154,6 +164,9 @@ class AstarSearch : public AbstractPlanningAlgorithm // distance metric option (removed when the reeds_shepp gets stable) bool use_reeds_shepp_; + + int x_scale_; + int y_scale_; }; } // namespace freespace_planning_algorithms diff --git a/planning/freespace_planning_algorithms/src/abstract_algorithm.cpp b/planning/freespace_planning_algorithms/src/abstract_algorithm.cpp index b4ab7e62750d9..143f147b79eee 100644 --- a/planning/freespace_planning_algorithms/src/abstract_algorithm.cpp +++ b/planning/freespace_planning_algorithms/src/abstract_algorithm.cpp @@ -121,14 +121,16 @@ void AbstractPlanningAlgorithm::setMap(const nav_msgs::msg::OccupancyGrid & cost // construct collision indexes table for (int i = 0; i < planner_common_param_.theta_size; i++) { - std::vector indexes_2d; - computeCollisionIndexes(i, indexes_2d); + std::vector indexes_2d, vertex_indexes_2d; + computeCollisionIndexes(i, indexes_2d, vertex_indexes_2d); coll_indexes_table_.push_back(indexes_2d); + vertex_indexes_table_.push_back(vertex_indexes_2d); } } void AbstractPlanningAlgorithm::computeCollisionIndexes( - int theta_index, std::vector & indexes_2d) const + int theta_index, std::vector & indexes_2d, + std::vector & vertex_indexes_2d) const { IndexXYT base_index{0, 0, theta_index}; const VehicleShape & vehicle_shape = collision_vehicle_shape_; @@ -143,7 +145,8 @@ void AbstractPlanningAlgorithm::computeCollisionIndexes( const auto base_theta = tf2::getYaw(base_pose.orientation); // Convert each point to index and check if the node is Obstacle - const auto addIndex2d = [&](const double x, const double y) { + const auto addIndex2d = [&]( + const double x, const double y, std::vector & indexes_cache) { // Calculate offset in rotated frame const double offset_x = std::cos(base_theta) * x - std::sin(base_theta) * y; const double offset_y = std::sin(base_theta) * x + std::cos(base_theta) * y; @@ -154,19 +157,41 @@ void AbstractPlanningAlgorithm::computeCollisionIndexes( const auto index = pose2index(costmap_, pose_local, planner_common_param_.theta_size); const auto index_2d = IndexXY{index.x, index.y}; - indexes_2d.push_back(index_2d); + indexes_cache.push_back(index_2d); }; for (double x = back; x <= front; x += costmap_.info.resolution / 2) { for (double y = right; y <= left; y += costmap_.info.resolution / 2) { - addIndex2d(x, y); + addIndex2d(x, y, indexes_2d); } - addIndex2d(x, left); + addIndex2d(x, left, indexes_2d); } for (double y = right; y <= left; y += costmap_.info.resolution / 2) { - addIndex2d(front, y); + addIndex2d(front, y, indexes_2d); } - addIndex2d(front, left); + addIndex2d(front, left, indexes_2d); + + const auto compareIndex2d = [](const IndexXY & left, const IndexXY & right) { + if (left.x != right.x) { + return (left.x < right.x); + } else { + return (left.y < right.y); + } + }; + + const auto equalIndex2d = [](const IndexXY & left, const IndexXY & right) { + return ((left.x == right.x) && (left.y == right.y)); + }; + + // remove duplicate indexes + std::sort(indexes_2d.begin(), indexes_2d.end(), compareIndex2d); + indexes_2d.erase( + std::unique(indexes_2d.begin(), indexes_2d.end(), equalIndex2d), indexes_2d.end()); + + addIndex2d(front, left, vertex_indexes_2d); + addIndex2d(front, right, vertex_indexes_2d); + addIndex2d(back, right, vertex_indexes_2d); + addIndex2d(back, left, vertex_indexes_2d); } bool AbstractPlanningAlgorithm::detectCollision(const IndexXYT & base_index) const @@ -175,6 +200,18 @@ bool AbstractPlanningAlgorithm::detectCollision(const IndexXYT & base_index) con std::cerr << "[abstract_algorithm] setMap has not yet been done." << std::endl; return false; } + + const auto & vertex_indexes_2d = vertex_indexes_table_[base_index.theta]; + for (const auto & vertex_index_2d : vertex_indexes_2d) { + IndexXYT vertex_index{vertex_index_2d.x, vertex_index_2d.y, 0}; + // must slide to current base position + vertex_index.x += base_index.x; + vertex_index.y += base_index.y; + if (isOutOfRange(vertex_index)) { + return true; + } + } + const auto & coll_indexes_2d = coll_indexes_table_[base_index.theta]; for (const auto & coll_index_2d : coll_indexes_2d) { int idx_theta = 0; // whatever. Yaw is nothing to do with collision detection between grids. @@ -183,10 +220,11 @@ bool AbstractPlanningAlgorithm::detectCollision(const IndexXYT & base_index) con coll_index.x += base_index.x; coll_index.y += base_index.y; - if (isOutOfRange(coll_index) || isObs(coll_index)) { + if (isObs(coll_index)) { return true; } } + return false; } diff --git a/planning/freespace_planning_algorithms/src/astar_search.cpp b/planning/freespace_planning_algorithms/src/astar_search.cpp index 315b9e06cf812..8d419c3daa13f 100644 --- a/planning/freespace_planning_algorithms/src/astar_search.cpp +++ b/planning/freespace_planning_algorithms/src/astar_search.cpp @@ -133,6 +133,8 @@ AstarSearch::AstarSearch( planner_common_param_.minimum_turning_radius, planner_common_param_.maximum_turning_radius, planner_common_param_.turning_radius_size, planner_common_param_.theta_size, astar_param_.use_back); + + y_scale_ = planner_common_param.theta_size; } void AstarSearch::setMap(const nav_msgs::msg::OccupancyGrid & costmap) @@ -141,17 +143,8 @@ void AstarSearch::setMap(const nav_msgs::msg::OccupancyGrid & costmap) clearNodes(); - const auto height = costmap_.info.height; - const auto width = costmap_.info.width; - - // Initialize nodes - nodes_.resize(height); - for (size_t i = 0; i < height; i++) { - nodes_[i].resize(width); - for (size_t j = 0; j < width; j++) { - nodes_[i][j].resize(planner_common_param_.theta_size); - } - } + x_scale_ = costmap_.info.height; + graph_.reserve(100000); } bool AstarSearch::makePlan( @@ -175,8 +168,9 @@ void AstarSearch::clearNodes() { // clearing openlist is necessary because otherwise remaining elements of openlist // point to deleted node. - nodes_.clear(); openlist_ = std::priority_queue, NodeComparison>(); + + graph_ = std::unordered_map(); } bool AstarSearch::setStartNode()