gazebo
[Turtlebot4][RRT Star] kd-tree로 rrt star다시 작성
정지홍
2025. 8. 8. 13:53
이거는 cpp
#include "rrt_star/rrt_star.hpp"
#include <random>
#include <nanoflann.hpp> // nano , Fast Library for Approximate Nearest Neighbors
#include <vector>
namespace rrt_star
{
// nanoflann을 위한 PointCloud 데이터 구조
// 비유 한다면, nano flann에 " 나의 데이터는 이렇게 생겼으니 , 여기를 이렇게 읽어주세요"라고 하는 느낌
struct PointCloud
{
struct Point{
double x, y;
};
std::vector<Point> pts; // 여러 point를 vector자료구조에 저장한다. vector는 동적 배열
// const가 붙는 이유는 "pts에 대한 정보는 읽겠지만, 정보는 변경하지 않겠다"라고 하는거
inline size_t kdtree_get_point_count() const { return pts.size(); }
inline double kdtree_get_pt( const size_t idx , int dim ) const
{
return dim == 0 ? pts[idx].x : pts[idx].y;
}
// function template ==> "함수를 하나만 작성해두고, 그 함수가 처리할 자료형type을 나중에 매게변수처럼 받아서 쓰는"방법
// 임의의 타임 BBOX를 입력받아서 동작하는 함수 템플릿
template <class BBOX> // 즉, BBOX라는 이름의 매개변수를 가진 템플릿 함수이다.
bool kdtree_get_bbox(BBOX&) const { return false; }
};
// KDTree Single Index Adaptor는 nano flann이 제공하는 KD-Tree 구현 템플릿 클래스이다.
// point cloud를 KD-Tree로 indexing해서, 최근접 이웃 탐색을 한다.
// L2는 유클리드 거리 공식을 쓰되, 내부 연산은 double로 처리하라는 말. PointCloud는 실제 데이터를 담고 있는 타입.
// 2차원을 의미.
// 별칭을 KDTree라고 선언.
typedef nanoflann::KDTreeSingleIndexAdaptor<
nanoflann::L2_Simple_Adaptor<double, PointCloud>,
PointCloud,
2
> KDTree;
void RRTStar::configure(
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
std::string name,
std::shared_ptr<tf2_ros::Buffer> tf,
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
{
node_ = parent.lock();
costmap_ros_ = costmap_ros;
RCLCPP_INFO(logger_, "RRT Star Algorithm is configured");
}
void RRTStar::cleanup() { RCLCPP_INFO(logger_,"RRT star is cleanup."); }
void RRTStar::activate() { RCLCPP_INFO(logger_,"RRT star is activated"); }
void RRTStar::deactivate() { RCLCPP_INFO(logger_,"RRT star is deactivated"); }
bool RRTStar::isCollisionFree(double x0, double y0, double x1, double y1)
{
unsigned int costmap_x, costmap_y;
double distance = hypot(x1 - x0, y1 - y0);
int steps = static_cast<int>(distance / 0.05);
for (int i = 0; i <= steps; i++) {
double x = x0 + (x1 - x0) * i / steps;
double y = y0 + (y1 - y0) * i / steps;
if (!costmap_ros_->getCostmap()->worldToMap(x, y, costmap_x, costmap_y)) {
return false;
}
double cell_cost = costmap_ros_->getCostmap()->getCost(costmap_x, costmap_y);
if (cell_cost >= 200) {
return false;
}
}
return true;
}
nav_msgs::msg::Path RRTStar::createPlan(const geometry_msgs::msg::PoseStamped & start, const geometry_msgs::msg::PoseStamped & goal)
{
std::random_device rd;
std::mt19937 gen(rd());
nav_msgs::msg::Path path;
path.header.frame_id = "map";
struct Node {
geometry_msgs::msg::PoseStamped pose;
int parent;
double cost;
std::vector<int> children;
};
std::vector<Node> tree;
PointCloud cloud;
tree.push_back( { start, -1, 0.0, {} } );
cloud.pts.push_back( { start.pose.position.x , start.pose.position.y } );
// 차원수는 2차원. 데이터 소스 , "리프 노드 하나가 최대 10개의 점을 담도록" 트리 분할 기준을 설정하는 파라미터 객체이다.
KDTree kdtree(2, cloud, nanoflann::KDTreeSingleIndexAdaptorParams(10));
kdtree.buildIndex();
std::uniform_real_distribution<> rand_x(costmap_ros_->getCostmap()->getOriginX(), costmap_ros_->getCostmap()->getOriginX() + costmap_ros_->getCostmap()->getSizeInMetersX());
std::uniform_real_distribution<> rand_y(costmap_ros_->getCostmap()->getOriginY(), costmap_ros_->getCostmap()->getOriginY() + costmap_ros_->getCostmap()->getSizeInMetersY());
std::uniform_real_distribution<> rand_goal_sampling(0.0, 1.0);
const int MAX_ITERATIONS = 5000;
const double STEP_SIZE = 0.5;
const double GOAL_THRESHOLD = 0.3;
const double GOAL_BIAS = 0.1;
const double NEIGHBORHOOD_RADIUS = 1.0;
auto start_time = node_->now();
for (int i = 0; i < MAX_ITERATIONS; i++) {
if ((node_->now() - start_time).seconds() > 1.0) break;
// 1. 랜덤 샘플링
geometry_msgs::msg::PoseStamped q_rand;
if (rand_goal_sampling(gen) < GOAL_BIAS)
q_rand = goal;
else {
q_rand.pose.position.x = rand_x(gen);
q_rand.pose.position.y = rand_y(gen);
}
// 2. q_nearest 칮기
double query_pt[2] = { q_rand.pose.position.x , q_rand.pose.position.y }; // kd - tree 쿼리 포인트 배열
size_t nearest_idx; // 최근접 노드 인덱스
double out_dist_sqr; // 제곱 거리 반환 변수
nanoflann::KNNResultSet<double>resultSet(1); // knn의 탐색 결과를 저장할 객체. resultSet안의 숫자만큼의 갯수를 찾음. 여기는 1개
resultSet.init(&nearest_idx, &out_dist_sqr);
kdtree.findNeighbors(resultSet, query_pt, nanoflann::SearchParams(10));
// 3. q_rand방향으로 step size만큼 이동시킨 지점에 q_new를 생성
double angle = atan2(q_rand.pose.position.y - tree[nearest_idx].pose.pose.position.y,
q_rand.pose.position.x - tree[nearest_idx].pose.pose.position.x);
double new_x = tree[nearest_idx].pose.pose.position.x + STEP_SIZE * cos(angle);
double new_y = tree[nearest_idx].pose.pose.position.y + STEP_SIZE * sin(angle);
if (!isCollisionFree(tree[nearest_idx].pose.pose.position.x, tree[nearest_idx].pose.pose.position.y, new_x, new_y))
continue;
// 4. q_new를 기준으로 최적의 부모를 탐색한다.
std::vector<std::pair<size_t, double>> neighbors; // pair을 쓰는 이유는 index,distance쌍을 저장하기 위함.
// RadiusResultSet은 반경r이내에 있는 점들을 검색후, 결과를 저장하는 클래스
// 1번째인자 neighborhood_radius * neighborhood_radius는 검색할 반경의 제곱값을 의미
// 2번째 인자는 검색 결과 들이 저장될 컨테이너
nanoflann::RadiusResultSet<double, size_t> resultSet3(NEIGHBORHOOD_RADIUS * NEIGHBORHOOD_RADIUS, neighbors);
resultSet3.init();
double q_new_pt[2] = { new_x, new_y };
kdtree.findNeighbors(resultSet3, q_new_pt, nanoflann::SearchParams(10));
// 4. 최적의 부모 선택
size_t best_parent_idx = nearest_idx;
double min_cost = tree[nearest_idx].cost + STEP_SIZE;
for (const auto& neighbor : neighbors) {
size_t idx = neighbor.first;
double nx = tree[idx].pose.pose.position.x;
double ny = tree[idx].pose.pose.position.y;
if (!isCollisionFree(nx, ny, new_x, new_y))
continue;
double new_cost = tree[idx].cost + hypot(nx - new_x, ny - new_y);
if (new_cost < min_cost) {
min_cost = new_cost;
best_parent_idx = idx;
}
}
Node q_new;
q_new.pose.pose.position.x = new_x;
q_new.pose.pose.position.y = new_y;
q_new.parent = best_parent_idx;
q_new.cost = tree[best_parent_idx].cost + STEP_SIZE;
tree.push_back(q_new);
cloud.pts.push_back({new_x, new_y});
int q_new_idx = tree.size() - 1;
tree[best_parent_idx].children.push_back(q_new_idx);
kdtree.buildIndex();
if (hypot(goal.pose.position.x - new_x, goal.pose.position.y - new_y) < GOAL_THRESHOLD) {
for (int idx = q_new_idx; idx != -1; idx = tree[idx].parent)
path.poses.insert(path.poses.begin(), tree[idx].pose);
path.header.stamp = node_->now();
auto end_time = node_->now();
auto duration_ms = (end_time - start_time).nanoseconds() / 1e6; // ns → ms 변환
RCLCPP_INFO(logger_, "[RRT Star] Path found in %.3f ms", duration_ms);
return path;
}
// 5. 노드 q_new 추가 후, q_new중심으로 rewire
std::vector<std::pair<size_t, double>> ret_matches;
nanoflann::RadiusResultSet<double, size_t> resultSet2(NEIGHBORHOOD_RADIUS * NEIGHBORHOOD_RADIUS, ret_matches);
resultSet2.init();
double new_point[2] = { new_x, new_y };
kdtree.findNeighbors(resultSet2, new_point, nanoflann::SearchParams(10));
for (const auto& match : ret_matches) {
size_t neighbor_idx = match.first;
double potential_cost = q_new.cost + hypot(tree[neighbor_idx].pose.pose.position.x - new_x,
tree[neighbor_idx].pose.pose.position.y - new_y);
if (potential_cost < tree[neighbor_idx].cost &&
isCollisionFree(new_x, new_y, tree[neighbor_idx].pose.pose.position.x, tree[neighbor_idx].pose.pose.position.y))
{
int old_parent = tree[neighbor_idx].parent;
tree[neighbor_idx].parent = q_new_idx;
tree[neighbor_idx].cost = potential_cost;
// 기존 부모 노드에서 이웃 제거
auto& siblings = tree[old_parent].children;
siblings.erase(std::remove(siblings.begin(), siblings.end(), neighbor_idx), siblings.end());
// 새로운 부모에 추가
tree[q_new_idx].children.push_back(neighbor_idx);
}
}
}
return path;
}
} // namespace rrt_star
#include <pluginlib/class_list_macros.hpp>
PLUGINLIB_EXPORT_CLASS(rrt_star::RRTStar, nav2_core::GlobalPlanner)
이거는 hpp
#ifndef RRT_STAR__RRT_STAR_HPP_
#define RRT_STAR__RRT_STAR_HPP_
#include <nav2_core/global_planner.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <nav_msgs/msg/path.hpp>
#include <rclcpp/rclcpp.hpp>
#include <nanoflann.hpp>
#include <vector>
namespace rrt_star
{
class RRTStar : public nav2_core::GlobalPlanner
{
public:
RRTStar() = default;
virtual ~RRTStar() = default;
void configure(const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
std::string name, std::shared_ptr<tf2_ros::Buffer> tf,
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros) override;
void cleanup() override;
void activate() override;
void deactivate() override;
nav_msgs::msg::Path createPlan(const geometry_msgs::msg::PoseStamped & start,
const geometry_msgs::msg::PoseStamped & goal) override;
private:
rclcpp::Logger logger_{rclcpp::get_logger("RRTStar Algorithm")};
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
bool isCollisionFree(double x0, double y0, double x1, double y1);
rclcpp_lifecycle::LifecycleNode::SharedPtr node_;
struct PointCloud
{
struct Point
{
double x, y;
};
std::vector<Point> pts;
inline size_t kdtree_get_point_count() const { return pts.size(); }
inline double kdtree_get_pt(const size_t idx, int dim) const
{
return dim == 0 ? pts[idx].x : pts[idx].y;
}
template <class BBOX>
bool kdtree_get_bbox(BBOX&) const { return false; }
};
using KDTree = nanoflann::KDTreeSingleIndexAdaptor<
nanoflann::L2_Simple_Adaptor<double, PointCloud>,
PointCloud,
2>;
};
} // namespace rrt_star
#endif // RRT_STAR__RRT_STAR_HPP_