gazebo
[turtlebot4][path] rrt planner__2 ......... tf관련 에러
정지홍
2025. 6. 10. 13:29
https://jihong.tistory.com/651
[turtlebot4][path] rrt planner__1
1. 패키지 설치mkdir rrt_testcd rrt_testros2 pkg create --build-type ament_cmake rrt_planner --dependencies rclcpp nav2_core pluginlib nav2_costmap_2d geometry_msgs nav2_msgs nav_msgs 2. rrt_planner / include / rrt_planner.hpp#ifndef RRT_PLANNER__RRT_P
jihong.tistory.com
위의 planner는 현재 controller_server에서 에러가 발생한다...
__ID__CONTROLLER_SERVER__
[INFO] [1749528739.922357963] [controller_server]:
controller_server lifecycle node launched.
Waiting on external lifecycle transitions to activate
See https://design.ros2.org/articles/node_lifecycle.html for more information.
[INFO] [1749528740.287452338] [controller_server]: Creating controller server
[INFO] [1749528740.331993132] [local_costmap.local_costmap]:
local_costmap lifecycle node launched.
Waiting on external lifecycle transitions to activate
See https://design.ros2.org/articles/node_lifecycle.html for more information.
[INFO] [1749528740.417867734] [local_costmap.local_costmap]: Creating Costmap
[INFO] [1749529224.489922937] [controller_server]: Configuring controller interface
[INFO] [1749529224.490366100] [controller_server]: getting goal checker plugins..
[INFO] [1749529224.490574081] [controller_server]: Controller frequency set to 20.0000Hz
[INFO] [1749529224.490614421] [local_costmap.local_costmap]: Configuring
[INFO] [1749529224.502574746] [local_costmap.local_costmap]: Using plugin "static_layer"
[INFO] [1749529226.535232607] [local_costmap.local_costmap]: Subscribing to the map topic (/map) with transient local durability
[INFO] [1749529226.538675546] [local_costmap.local_costmap]: Initialized plugin "static_layer"
[INFO] [1749529226.538732756] [local_costmap.local_costmap]: Using plugin "voxel_layer"
[INFO] [1749529226.540243344] [local_costmap.local_costmap]: Subscribed to Topics: scan
[INFO] [1749529226.601249286] [local_costmap.local_costmap]: Initialized plugin "voxel_layer"
[INFO] [1749529226.601288036] [local_costmap.local_costmap]: Using plugin "inflation_layer"
[INFO] [1749529226.602569503] [local_costmap.local_costmap]: Initialized plugin "inflation_layer"
[INFO] [1749529226.641373444] [local_costmap.local_costmap]: StaticLayer: Resizing static layer to 620 X 1138 at 0.050000 m/pix
[INFO] [1749529226.670882785] [controller_server]: Created progress_checker : progress_checker of type nav2_controller::SimpleProgressChecker
[INFO] [1749529226.712877283] [controller_server]: Created goal checker : general_goal_checker of type nav2_controller::SimpleGoalChecker
[INFO] [1749529226.713972559] [controller_server]: Controller Server has general_goal_checker goal checkers available.
[INFO] [1749529227.072415038] [controller_server]: Created controller : FollowPath of type dwb_core::DWBLocalPlanner
[INFO] [1749529227.076313039] [controller_server]: Setting transform_tolerance to 0.200000
[INFO] [1749529228.189372880] [controller_server]: Using critic "RotateToGoal" (dwb_critics::RotateToGoalCritic)
[INFO] [1749529228.190383776] [controller_server]: Critic plugin initialized
[INFO] [1749529228.190613707] [controller_server]: Using critic "Oscillation" (dwb_critics::OscillationCritic)
[INFO] [1749529228.191492632] [controller_server]: Critic plugin initialized
[INFO] [1749529228.192114195] [controller_server]: Using critic "BaseObstacle" (dwb_critics::BaseObstacleCritic)
[INFO] [1749529228.192532327] [controller_server]: Critic plugin initialized
[INFO] [1749529228.192736308] [controller_server]: Using critic "GoalAlign" (dwb_critics::GoalAlignCritic)
[INFO] [1749529228.193412322] [controller_server]: Critic plugin initialized
[INFO] [1749529228.193614073] [controller_server]: Using critic "PathAlign" (dwb_critics::PathAlignCritic)
[INFO] [1749529228.194767710] [controller_server]: Critic plugin initialized
[INFO] [1749529228.195079441] [controller_server]: Using critic "PathDist" (dwb_critics::PathDistCritic)
[INFO] [1749529228.196175957] [controller_server]: Critic plugin initialized
[INFO] [1749529228.196950501] [controller_server]: Using critic "GoalDist" (dwb_critics::GoalDistCritic)
[INFO] [1749529228.197410054] [controller_server]: Critic plugin initialized
[INFO] [1749529228.197442284] [controller_server]: Controller Server has FollowPath controllers available.
[INFO] [1749529239.515215876] [controller_server]: Activating
[INFO] [1749529239.515270366] [local_costmap.local_costmap]: Activating
[INFO] [1749529239.515285667] [local_costmap.local_costmap]: Checking transform
[INFO] [1749529239.515388227] [local_costmap.local_costmap]: start
[INFO] [1749529239.716995233] [controller_server]: Creating bond (controller_server) to lifecycle manager.
[INFO] [1749529372.906331112] [controller_server]: Received a goal, begin computing control effort.
[WARN] [1749529372.906420013] [controller_server]: No goal checker was specified in parameter 'current_goal_checker'. Server will use only plugin loaded general_goal_checker . This warning will appear once.
[ERROR] [1749529372.910746426] [tf_help]: Transform data too old when converting from map to odom
[ERROR] [1749529372.910805096] [tf_help]: Data time: 1749529372s 884181932ns, Transform time: 537s 946000000ns
[INFO] [1749529372.910822617] [controller_server]: Reached the goal!
[INFO] [1749529388.532860450] [controller_server]: Received a goal, begin computing control effort.
[ERROR] [1749529388.535635895] [tf_help]: Transform data too old when converting from map to odom
[ERROR] [1749529388.535651455] [tf_help]: Data time: 1749529388s 511332613ns, Transform time: 552s 745000000ns
[INFO] [1749529388.535660385] [controller_server]: Reached the goal!
[INFO] [1749529415.058869655] [controller_server]: Received a goal, begin computing control effort.
[ERROR] [1749529415.061857191] [tf_help]: Transform data too old when converting from map to odom
[ERROR] [1749529415.061905982] [tf_help]: Data time: 1749529415s 37542769ns, Transform time: 578s 137000000ns
[ERROR] [1749529415.112193855] [tf_help]: Transform data too old when converting from map to odom
[ERROR] [1749529415.112220995] [tf_help]: Data time: 1749529415s 37542769ns, Transform time: 578s 185000000ns
[ERROR] [1749529415.161587444] [tf_help]: Transform data too old when converting from map to odom
[ERROR] [1749529415.161613964] [tf_help]: Data time: 1749529415s 37542769ns, Transform time: 578s 233000000ns
[ERROR] [1749529415.211943187] [tf_help]: Transform data too old when converting from map to odom
예상 원인 1 : isCollisionFree함수
- 매번 경로 후보를 생성하기 위해서, 수십~수백번 costmap에 접근함...
위의 부분이 루프 안에서 지나치게 많이 호출이 된다면 병목이 발생할 수 있음. - 그래서 아래 파일처럼 우선 수정함.
#include "rrt_planner/rrt_planner.hpp"
#include <random>
namespace rrt_planner
{
void RRTPlanner::configure(const rclcpp_lifecycle::LifecycleNode::WeakPtr &,
std::string, std::shared_ptr<tf2_ros::Buffer>,
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
{
costmap_ros_ = costmap_ros;
RCLCPP_INFO(logger_, "RRTPlanner configured");
}
void RRTPlanner::cleanup()
{
RCLCPP_INFO(logger_, "RRTPlanner cleanup");
}
void RRTPlanner::activate()
{
RCLCPP_INFO(logger_, "RRTPlanner activated");
}
void RRTPlanner::deactivate()
{
RCLCPP_INFO(logger_, "RRTPlanner deactivated");
}
// 두 점 (x0 , y0)에서 (x1 , y1)까지 직선 경로가 장애물 없이 통과 가능한지 체크
bool RRTPlanner::isCollisionFree(double x0, double y0, double x1, double y1)
{
unsigned int mx, my; // 코스트맵의 좌표계 변수
double distance = hypot(x1 - x0, y1 - y0); // 거리 구하기
int steps = static_cast<int>(distance / 0.5); // 0.5 = 50cm
// 직선 경로를 5cm 단위로 샘플링하면서 각각의 점이 장애물과 겹치는지 확인
for (int i = 0; i <= steps; i++) {
double x = x0 + (x1 - x0) * i / steps;
double y = y0 + (y1 - y0) * i / steps;
// 지도 좌표계에서 costmap 좌표계로 변환 불가한다면( 이 경우는 맵 밖인 경우 ).... 충돌로 간주
if (!costmap_ros_->getCostmap()->worldToMap(x, y, mx, my)) return false;
// 해당 셀의 코스트가 254이상인 경우에..... 충돌로 간주
if (costmap_ros_->getCostmap()->getCost(mx, my) >= 254) return false;
}
return true;
}
// 실제 RRT 알고리즘을 통해서 start --> goal 경로를 생성하는 함수
nav_msgs::msg::Path RRTPlanner::createPlan(
const geometry_msgs::msg::PoseStamped & start,
const geometry_msgs::msg::PoseStamped & goal)
{
nav_msgs::msg::Path path; // 이는 ros2에서 path정보를 담아서 전송할때 사용하는 메시지 타입이다.
// path메시지에의 헤더에는 stamp와 frame_id가 있다.
path.header.stamp = rclcpp::Clock().now(); // 경로 메시지의 타임 스탬프
path.header.frame_id = "map"; // 전역 좌표계....( frame_id는 map )
// node 구조체이다. 트리의 한 노드이며 위치와 부모 인덱스를 저장.
// node.pose.pose.position.x에서 첫번째 pose는 PostStamped메시지(타임스탬프 +프레임 + pose)를 의미
// 두번째 pose는 Pose 메시지 (Point position + Quaternion orientation)
struct Node {
geometry_msgs::msg::PoseStamped pose;
int parent;
double cost;
};
/*
// 내부 람다: 두 점간의 충돌 검사를 위한 함수.
auto isCollisionFree = [this](double x0, double y0, double x1, double y1) {
unsigned int mx, my;
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, mx, my)) return false;
if (costmap_ros_->getCostmap()->getCost(mx, my) >= 254) return false;
}
return true;
};*/
// 노드(트리) 저장 벡터이다. 첫 노드는 시작위치를 의미한다.
std::vector<Node> nodes;
nodes.push_back({start, -1 , 0}); // 시작 노드는 부모가 없다.
// 랜덤 샘플링을 위한 난수 생성기. 분포를 설정
std::random_device rd;
std::mt19937 gen(rd());
// 지도 범위 내에서 x,y를 무작위로 샘플링한다.
// uniform_real_distribution<> name(a,b)은 c++ 표준 라이브러리에 포함된 난수 분포 클래스다.
// 이는 실수형 난수를 균등하게 생성해주는 역할을 한다. 즉, 지정한 범위 내에서 "모든 값이 동일한 확률"로 나올수 있게 실수형 난수를 뽑아쥼
// getOriginX()는 costmap의 x최솟값
// getSizeInMetersX()는 costmap의 x축 길이
std::uniform_real_distribution<> dis_x(
costmap_ros_->getCostmap()->getOriginX(),
costmap_ros_->getCostmap()->getOriginX() + costmap_ros_->getCostmap()->getSizeInMetersX());
std::uniform_real_distribution<> dis_y(
costmap_ros_->getCostmap()->getOriginY(),
costmap_ros_->getCostmap()->getOriginY() + costmap_ros_->getCostmap()->getSizeInMetersY());
// 목표점으로의 bias ( 확률적 목표점 샘플링 )
std::uniform_real_distribution<> dis_goal(0.0, 1.0);
// RRT의 파라미터
const int MAX_ITERATIONS = 300;
const double STEP_SIZE = 0.2; // 한번에 뻗어 나가는 거리
const double GOAL_THRESHOLD = 0.3; // 목표 지점에 도달했다고 보는 거리
const double GOAL_BIAS = 0.1; // goal을 샘플링 할 확률... 10 %
auto start_time = rclcpp::Clock().now(); // 경로 생성 시작 시간
RCLCPP_INFO(logger_, "RRT start time is %.6f" , start_time.seconds());
// RRT 트리 생성 반복 루프..... ( max_iterations 혹은 timeLimits까지 반복 )
for (int i = 0; i < MAX_ITERATIONS; ++i) {
// 경로 탐색이 0.5초 초과 시 타임아웃 ( 실시간성을 보장 , 무한루프 방지 )
if ((rclcpp::Clock().now() - start_time).seconds() > 0.5) {
RCLCPP_INFO(logger_, "RRT timeout %.6f " , start_time.seconds());
RCLCPP_WARN(logger_, "RRT planner timeout");
break;
}
// 1. 랜덤 샘플링
geometry_msgs::msg::PoseStamped random_pose;
// 20%확률로 goal 위치를 직접 샘플링
if (dis_goal(gen) < GOAL_BIAS)
random_pose = goal;
else {
random_pose.pose.position.x = dis_x(gen);
random_pose.pose.position.y = dis_y(gen);
}
// 2. q_rand에서 가장 가까운 노드인 q_near을 찾는다.
int nearest_idx = -1;
double min_dist = std::numeric_limits<double>::max();
for (size_t j = 0; j < nodes.size(); ++j) {
double dx = random_pose.pose.position.x - nodes[j].pose.pose.position.x;
double dy = random_pose.pose.position.y - nodes[j].pose.pose.position.y;
double dist = sqrt(dx*dx + dy*dy);
if (dist < min_dist) {
min_dist = dist;
nearest_idx = j;
}
}
// 3. q_near에서 q_rand로 뻗어날 방향을 구한다. 그리고 step_size만큼 이동한 위치의 지점을 찾는다.
double angle = atan2(random_pose.pose.position.y - nodes[nearest_idx].pose.pose.position.y,
random_pose.pose.position.x - nodes[nearest_idx].pose.pose.position.x);
double new_x = nodes[nearest_idx].pose.pose.position.x + STEP_SIZE * cos(angle);
double new_y = nodes[nearest_idx].pose.pose.position.y + STEP_SIZE * sin(angle);
if (!isCollisionFree(nodes[nearest_idx].pose.pose.position.x,
nodes[nearest_idx].pose.pose.position.y,
new_x, new_y)) {
//RCLCPP_INFO(logger_ , "노드 생성중 장애물 충돌...");
continue;
}
Node new_node;
new_node.pose.pose.position.x = new_x;
new_node.pose.pose.position.y = new_y;
new_node.parent = nearest_idx;
new_node.cost = nodes[new_node.parent].cost + hypot(
new_x-nodes[new_node.parent].pose.pose.position.x , new_y-nodes[new_node.parent].pose.pose.position.y);
nodes.push_back(new_node);
if (hypot(goal.pose.position.x - new_x, goal.pose.position.y - new_y) < GOAL_THRESHOLD) {
Node goal_node;
goal_node.pose = goal;
goal_node.parent = nodes.size() - 1;
goal_node.cost = new_node.cost + hypot(
goal.pose.position.x - new_x,
goal.pose.position.y - new_y
);
nodes.push_back(goal_node);
int current_idx = nodes.size()-1;
RCLCPP_INFO( logger_ , "최종적인 %.6f 입니다." , nodes.back().cost );
while (current_idx != -1) {
path.poses.insert(path.poses.begin(), nodes[current_idx].pose);
current_idx = nodes[current_idx].parent;
}
RCLCPP_INFO(logger_, "Path size: %ld", path.poses.size());
return path;
}
}
RCLCPP_WARN(logger_, "Failed to find path!");
return path;
}
} // namespace rrt_planner
#include <pluginlib/class_list_macros.hpp>
PLUGINLIB_EXPORT_CLASS(rrt_planner::RRTPlanner, nav2_core::GlobalPlanner)
예상 원인 2 : 지속적인 path return으로 인한 계산량 증가
- createPlan에서 path가 반환되어도 계속해서 path를 계산하고 있다.
- navigate_to_pose_w_replanning_and_recovery.xml을 수정해야 한다.
- 그중 RateController hz="1.0"이 핵심이다. 이것의 의미는 1초에 1번 computePathToPose 노드가 실행된다.
이로 인해서 경로가 지속적으로 재계획된다. - RateController hz="0.1"로 수정하면 10초에 한번씩 계산하게 할수있다.
- 그중 RateController hz="1.0"이 핵심이다. 이것의 의미는 1초에 1번 computePathToPose 노드가 실행된다.
예상 원인 3 : planner에서 유효한 경로를 못만들거나, timestamp가 잘못된 경우
- 만약에 empty한 path를 받으면, 당연히 controller는 움질일수 없으니 실패한다.
- 동시에, 오래된 TF 데이터를 참조하는 오류가 발생한다.
- 우선 코드상에 path.header.stamp = rclcpp::Clock().now()라고 된 부분은 시스템의 시간을 받아온다. 그래서 이를 노드의 시간으로 가져오게 auto start_time = node_->now();같이 변경하자.
예상 원인 4 : path.pose의 header.stamp와 header.frame_id가 일치않아서 발생하는 TF오류
- 즉, path.poses의 header.stamp/frame_id가 각자 따로였다.
- 그래서 “controller가 참조하는 모든 pose의 시간과 프레임이, path의 header와 같게 설정한다.”
#include "rrt_planner/rrt_planner.hpp"
#include <random>
namespace rrt_planner
{
void RRTPlanner::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_, "RRTPlanner configured");
}
void RRTPlanner::cleanup()
{
RCLCPP_INFO(logger_, "RRTPlanner cleanup");
}
void RRTPlanner::activate()
{
RCLCPP_INFO(logger_, "RRTPlanner activated");
}
void RRTPlanner::deactivate()
{
RCLCPP_INFO(logger_, "RRTPlanner deactivated");
}
// 두 점 (x0 , y0)에서 (x1 , y1)까지 직선 경로가 장애물 없이 통과 가능한지 체크
bool RRTPlanner::isCollisionFree(double x0, double y0, double x1, double y1)
{
unsigned int mx, my; // 코스트맵의 좌표계 변수
double distance = hypot(x1 - x0, y1 - y0); // 거리 구하기
int steps = static_cast<int>(distance / 0.05); // 0.5 = 50cm
// 직선 경로를 5cm 단위로 샘플링하면서 각각의 점이 장애물과 겹치는지 확인
for (int i = 0; i <= steps; i++) {
double x = x0 + (x1 - x0) * i / steps;
double y = y0 + (y1 - y0) * i / steps;
// 지도 좌표계에서 costmap 좌표계로 변환 불가한다면( 이 경우는 맵 밖인 경우 ).... 충돌로 간주
if (!costmap_ros_->getCostmap()->worldToMap(x, y, mx, my)) {
RCLCPP_WARN(logger_, "좌표가 코스트맵 바깥임: (%f,%f)", x, y);
return false;
}
// 해당 셀의 코스트가 254이상인 경우에..... 충돌로 간주
double cell_cost = costmap_ros_ -> getCostmap() -> getCost(mx,my);
if (cell_cost >= 254){
RCLCPP_WARN(logger_, "충돌 감지됨! 코스트(%f) 좌표:(%u,%u)", cell_cost , mx, my);
return false;
}
else if( cell_cost >= 200){
RCLCPP_WARN(logger_ , "미지의 영역! 코스트 (%f) 좌표:(%u,%u)", cell_cost , mx, my);
}
}
return true;
}
// 실제 RRT 알고리즘을 통해서 start --> goal 경로를 생성하는 함수
nav_msgs::msg::Path RRTPlanner::createPlan(
const geometry_msgs::msg::PoseStamped & start,
const geometry_msgs::msg::PoseStamped & goal)
{
nav_msgs::msg::Path path; // 이는 ros2에서 path정보를 담아서 전송할때 사용하는 메시지 타입이다.
// path메시지에의 헤더에는 stamp와 frame_id가 있다.
// path.header.stamp = rclcpp::Clock().now(); // 경로 메시지의 타임 스탬프
//path.header.stamp = this->now();
// path.header.stamp = node_->now();
path.header.frame_id = "map"; // 전역 좌표계....( frame_id는 map )
// node 구조체이다. 트리의 한 노드이며 위치와 부모 인덱스를 저장.
// node.pose.pose.position.x에서 첫번째 pose는 PostStamped메시지(타임스탬프 +프레임 + pose)를 의미
// 두번째 pose는 Pose 메시지 (Point position + Quaternion orientation)
struct Node {
geometry_msgs::msg::PoseStamped pose;
int parent;
double cost;
};
// 노드(트리) 저장 벡터이다. 첫 노드는 시작위치를 의미한다.
std::vector<Node> nodes;
nodes.push_back({start, -1 , 0}); // 시작 노드는 부모가 없다.
// 랜덤 샘플링을 위한 난수 생성기. 분포를 설정
std::random_device rd;
std::mt19937 gen(rd());
// 지도 범위 내에서 x,y를 무작위로 샘플링한다.
// uniform_real_distribution<> name(a,b)은 c++ 표준 라이브러리에 포함된 난수 분포 클래스다.
// 이는 실수형 난수를 균등하게 생성해주는 역할을 한다. 즉, 지정한 범위 내에서 "모든 값이 동일한 확률"로 나올수 있게 실수형 난수를 뽑아쥼
// getOriginX()는 costmap의 x최솟값
// getSizeInMetersX()는 costmap의 x축 길이
std::uniform_real_distribution<> dis_x(
costmap_ros_->getCostmap()->getOriginX(),
costmap_ros_->getCostmap()->getOriginX() + costmap_ros_->getCostmap()->getSizeInMetersX());
std::uniform_real_distribution<> dis_y(
costmap_ros_->getCostmap()->getOriginY(),
costmap_ros_->getCostmap()->getOriginY() + costmap_ros_->getCostmap()->getSizeInMetersY());
// 목표점으로의 bias ( 확률적 목표점 샘플링 )
std::uniform_real_distribution<> dis_goal(0.0, 1.0);
// RRT의 파라미터
const int MAX_ITERATIONS = 300;
const double STEP_SIZE = 0.2; // 한번에 뻗어 나가는 거리
const double GOAL_THRESHOLD = 0.3; // 목표 지점에 도달했다고 보는 거리
const double GOAL_BIAS = 0.1; // goal을 샘플링 할 확률... 10 %
auto start_time = node_->now();
RCLCPP_INFO(logger_, "RRT start time is %.6f" , start_time.seconds());
// RRT 트리 생성 반복 루프..... ( max_iterations 혹은 timeLimits까지 반복 )
for (int i = 0; i < MAX_ITERATIONS; ++i) {
// 경로 탐색이 0.5초 초과 시 타임아웃 ( 실시간성을 보장 , 무한루프 방지 )
if ((node_->now() - start_time).seconds() > 0.5) {
RCLCPP_INFO(logger_, "RRT timeout %.6f " , start_time.seconds());
RCLCPP_WARN(logger_, "RRT planner timeout");
break;
}
// 1. 랜덤 샘플링
geometry_msgs::msg::PoseStamped random_pose;
// 20%확률로 goal 위치를 직접 샘플링
if (dis_goal(gen) < GOAL_BIAS)
random_pose = goal;
else {
random_pose.pose.position.x = dis_x(gen);
random_pose.pose.position.y = dis_y(gen);
}
// 2. q_rand에서 가장 가까운 노드인 q_near을 찾는다.
int nearest_idx = -1;
double min_dist = std::numeric_limits<double>::max();
for (size_t j = 0; j < nodes.size(); ++j) {
double dx = random_pose.pose.position.x - nodes[j].pose.pose.position.x;
double dy = random_pose.pose.position.y - nodes[j].pose.pose.position.y;
double dist = sqrt(dx*dx + dy*dy);
if (dist < min_dist) {
min_dist = dist;
nearest_idx = j;
}
}
// 3. q_near에서 q_rand로 뻗어날 방향을 구한다. 그리고 step_size만큼 이동한 위치의 지점을 찾는다.
double angle = atan2(random_pose.pose.position.y - nodes[nearest_idx].pose.pose.position.y,
random_pose.pose.position.x - nodes[nearest_idx].pose.pose.position.x);
double new_x = nodes[nearest_idx].pose.pose.position.x + STEP_SIZE * cos(angle);
double new_y = nodes[nearest_idx].pose.pose.position.y + STEP_SIZE * sin(angle);
if (!isCollisionFree(nodes[nearest_idx].pose.pose.position.x,
nodes[nearest_idx].pose.pose.position.y,
new_x, new_y)) {
//RCLCPP_INFO(logger_ , "노드 생성중 장애물 충돌...");
continue;
}
Node new_node;
new_node.pose.pose.position.x = new_x;
new_node.pose.pose.position.y = new_y;
new_node.parent = nearest_idx;
new_node.cost = nodes[new_node.parent].cost + hypot(
new_x-nodes[new_node.parent].pose.pose.position.x , new_y-nodes[new_node.parent].pose.pose.position.y);
nodes.push_back(new_node);
// new_node.pose.header.stamp = node_->now();
if (hypot(goal.pose.position.x - new_x, goal.pose.position.y - new_y) < GOAL_THRESHOLD) {
Node goal_node;
goal_node.pose = goal;
goal_node.parent = nodes.size() - 1;
goal_node.cost = new_node.cost + hypot(
goal.pose.position.x - new_x,
goal.pose.position.y - new_y
);
nodes.push_back(goal_node);
int current_idx = nodes.size()-1;
RCLCPP_INFO( logger_ , "최종적인 %.6f 입니다." , nodes.back().cost );
while (current_idx != -1) {
path.poses.insert( path.poses.begin() , nodes[current_idx].pose);
current_idx = nodes[current_idx].parent;
}
RCLCPP_INFO(logger_, "Path size: %ld", path.poses.size());
path.header.stamp = node_->now();
path.header.frame_id = "map";
for (auto &pose : path.poses) {
pose.header.stamp = path.header.stamp;
pose.header.frame_id = path.header.frame_id;
}
return path;
}
}
RCLCPP_WARN(logger_, "Failed to find path!");
return path;
}
} // namespace rrt_planner
#include <pluginlib/class_list_macros.hpp>
PLUGINLIB_EXPORT_CLASS(rrt_planner::RRTPlanner, nav2_core::GlobalPlanner)