costmap_2d에서 로컬 장애물 미제거 현상 진단
ROS 내비게이션 스택을 활용한 로봇 개발 중, 로컬 코스트맵(costmap_2d)에서 장애물이 제대로 제거되지 않아 로봇이 움직이지 못하는 현상이 발생할 수 있습니다. 특히 동적인 환경에서 이러한 문제가 두드러지게 나타나며, 사람이 지나가거나 물체가 이동한 후에도 해당 위치에 장애물 정보가 계속 남아있는 경우가 있습니다. 이는 레이저 스캐너가 특정 영역에 장애물을 감지하지 못했을 때, 이전에 기록된 장애물 정보가 지워지지 않고 누적되기 때문입니다.
일반적으로 Lidar 센서는 장애물을 감지하지 못할 경우 해당 거리 값을 최대 거리로 설정합니다. 이론적으로는 레이저 빔이 특정 경로에서 장애물을 감지하지 못하면, 그 경로상의 이전 장애물 정보는 자동으로 클리어되어야 합니다. 이러한 맥락에서 costmap_2d의 obstacle layer에 있는 장애물 제거 로직을 먼저 살펴보는 것이 자연스러운 디버깅 과정입니다. 특히 raytrace_range 파라미터와 관련된 코드 부분을 검토했지만, 초기에는 문제가 발견되지 않았습니다. obstacle layer의 기본 동작은 레이저 스캔 데이터가 유효한 범위 내에 있을 경우 해당 경로의 장애물을 지우는 방식으로, 계산 효율성 측면에서도 합리적으로 보였습니다.
그러나 심층 분석 결과, 문제의 원인은 ObstacleLayer::laserScanCallback 함수 내의 특정 코드 라인에 있었습니다. 해당 함수는 수신된 레이저 스캔 메시지를 처리하는데, 이때 laser_geometry 패키지의 기능을 활용하여 스캔 데이터를 PointCloud 형식으로 변환합니다.
void ObstacleLayer::laserScanCallback(const sensor_msgs::LaserScan::ConstPtr& current_scan_msg) {
// ... 기타 초기화 및 변환 로직
sensor_msgs::PointCloud2 converted_cloud;
scan_projector_.transformLaserScanToPointCloud(current_scan_msg->header.frame_id,
*current_scan_msg,
converted_cloud,
*tf_buffer_);
// ... 변환된 PointCloud를 사용하여 코스트맵 업데이트
}
겉보기에는 단순한 형식 변환 과정처럼 보이지만, laser_geometry 내부 구현을 확인한 결과 여기에 중요한 로직이 숨어 있었습니다. transformLaserScanToPointCloud 함수는 기본적으로 스캔 데이터의 최대 거리를 초과하는 포인트를 필터링하는 내부 로직을 포함하고 있었습니다. 즉, scan_in.range_max보다 큰 거리 값을 가진 스캔 포인트는 클라우드 생성 시점부터 제외됩니다. 이로 인해 costmap_2d의 장애물 제거 로직은 해당 경로에 대한 유효한 스캔 데이터를 받지 못하게 되고, 결과적으로 이전에 기록된 장애물 정보를 지울 수 없게 되는 것입니다.
// laser_geometry 패키지 내부의 예시적인 스캔 처리 로직
void LaserProjector::processScanData(const sensor_msgs::LaserScan& input_scan,
sensor_msgs::PointCloud2& output_cloud) {
// ... 초기 설정 및 메모리 할당
const float min_valid_range = input_scan.range_min;
const float max_effective_range = input_scan.range_max; // 스캔 데이터의 최대 범위 사용
for (size_t i = 0; i < input_scan.ranges.size(); ++i) {
float distance_reading = input_scan.ranges[i];
// 유효 범위를 벗어나는 (너무 가깝거나, 너무 멀거나, 유효하지 않은) 포인트는 무시
if (std::isnan(distance_reading) || distance_reading < min_valid_range || distance_reading > max_effective_range) {
continue;
}
// 유효한 포인트만 PointCloud에 추가하는 로직
// ... (예: 각도와 거리를 이용해 3D 포인트 계산 및 추가)
}
// ... PointCloud 완성
}
해결 방안:
이 문제에 대한 해결책은 비교적 간단합니다. 다음 두 가지 방법 중 하나를 고려할 수 있습니다:
laser_geometry패키지 코드를 직접 수정하여range_max필터링 로직을 제거하거나, 필요에 따라 더 큰 값으로 조정합니다.- 레이저 스캐너의
range_max파라미터 값을 기본값보다 훨씬 크게 설정합니다. 이는laser_geometry가 대부분의 유효 스캔 데이터를 변환하도록 하여, 장애물 제거 로직이 올바르게 작동할 수 있도록 합니다. 이 방법을 사용할 경우 다음 사항을 주의해야 합니다:obstacle_range파라미터는 새로 설정된range_max값보다 작아야 합니다. 그렇지 않으면 로봇이 항상 장애물에 둘러싸여 있다고 인식할 수 있습니다.- AMCL이나 GMapping과 같은 다른 모듈의 최대 거리 제한(
max_range또는 유사한 파라미터)보다 새로 설정된range_max가 커야 합니다. 이는 잘못된 가상의 장애물 인식을 방지하기 위함입니다.
목표 지점 도달 시 로봇의 흔들림 문제
DWA(Dynamic Window Approach) 플래너를 사용하는 로봇이 목표 지점에 도달했을 때, 특히 최종 방향 정렬 단계에서 로봇이 좌우로 흔들리거나 불안정하게 움직이는 현상이 관찰될 수 있습니다. 이는 로봇이 먼저 목표 위치에 도달한 다음, 제자리에서 최종 방향으로 회전하는 과정에서 발생합니다.
이러한 흔들림은 로봇이 목표 위치에 거의 도달했지만, 미세한 위치 오차로 인해 설정된 허용 오차(tolerance) 범위를 일시적으로 벗어나게 될 때 발생합니다. 이때 로컬 플래너는 목표에 다시 도달하기 위해 새로운 경로를 계산하려고 시도하고, 이 과정이 반복되면서 로봇이 마치 목표 지점 사이에서 '고민'하는 것처럼 흔들리는 움직임을 보이게 됩니다.
ROS 내비게이션 스택은 이러한 문제를 해결하기 위해 latch_xy_goal_tolerance라는 파라미터를 제공합니다. 이 파라미터는 로봇이 한 번이라도 목표 위치의 XY 평면 오차 범위 내에 도달하면, 이후에는 XY 위치 목표에 대한 검사를 중단하고 오직 회전 목표 달성에만 집중하도록 로컬 플래너를 '고정(latch)'시키는 역할을 합니다. 이론적으로는 이 설정이 로봇의 흔들림을 방지해야 합니다.
하지만 실제 적용 시 이 파라미터가 예상대로 작동하지 않는 경우가 있습니다. 문제의 핵심은 DWAPlannerROS::setPlan 함수 내의 로직에 있습니다. 이 함수는 새로운 전역 계획(global plan)이 수신될 때마다 latchedStopRotateController_.resetLatching() 메서드를 호출하여 래치(latch) 상태를 재설정합니다.
bool DWAPlannerROS::setPlan(const std::vector& new_global_path) {
if (!isInitialized()) {
ROS_ERROR("Local planner not initialized. Call initialize() first.");
return false;
}
// 새로운 전역 경로가 설정될 때마다 래치 상태가 초기화됩니다.
// 주기적으로 동일한 경로가 전달될 경우, 이로 인해 latchedStopRotateController의 래치 기능이 무력화될 수 있습니다.
latchedStopRotateController_.resetLatching();
ROS_INFO("Successfully received a new global path.");
return internal_dwa_planner_->setPlan(new_global_path);
}
일반적으로 전역 계획은 1초마다와 같은 주기적인 빈도로 업데이트됩니다. 로봇이 목표 위치에 도달하여 최종 회전 단계에 있을 때, 아직 최종 방향 목표에는 도달하지 않은 상태이므로 전역 플래너는 계속해서 동일한(또는 매우 유사한) 전역 계획을 생성하여 setPlan 함수를 호출하게 됩니다. 이로 인해 latch_xy_goal_tolerance가 설정되었음에도 불구하고, 로봇이 목표 위치에 도달할 때마다 래치 상태가 계속 재설정되어 로컬 플래너가 비활성화되지 못하고 흔들림이 반복되는 것입니다.
해결 방안:
이 문제를 해결하기 위한 몇 가지 접근 방식이 있습니다:
resetLatching()호출 조건 수정:setPlan함수 내에서resetLatching()을 호출하기 전에 현재의 전역 목표 포즈가 이전 목표 포즈와 실제로 변경되었는지 확인하는 로직을 추가합니다. 목표가 동일하다면 래치를 재설정하지 않도록 하여, 로봇이 최종 회전 단계에 있을 때 래치 상태가 유지되도록 합니다.- 전역 계획 빈도 조정: 전역 플래너의 업데이트 빈도(
planner_frequency)를 0으로 설정하여, 전역 계획이 새 목표가 주어졌을 때나 로컬 플래너가 실패했을 때만 생성되도록 합니다. 이렇게 하면 최종 회전 단계에서 불필요하게setPlan이 호출되는 것을 방지할 수 있습니다. 그러나 이 방법은 동적인 환경에서 전역 경로 업데이트가 필요할 때 로봇의 반응성을 저하시킬 수 있으므로 신중하게 고려해야 합니다.