Robotics

Elevation Mapping Manual

연 수 2025. 3. 6. 09:05
반응형

Elevation Mapping Manual 작성

  • Elevation Mapping을 진행하기 위한, 기본 세팅 진행
# docker images
docker pull osrf/ros:humble-desktop

# X서버에 도커 컨테이너 접근 허용
xhost +local:docker

# docker 실행
docker run -it --name ys_test --network host \
  --gpus all \
  -e DISPLAY=$DISPLAY \
  -v /tmp/.X11-unix:/tmp/.X11-unix \
  osrf/ros:humble-desktop

# docker 실행
docker exec -it ys_test /bin/bash

# 패키지 목록 업데이트
apt update

# 저장소 활성화
apt install software-properties-common
add-apt-repository universe
apt update

# gedit 설치
apt install gedit

# ifconfig 설치
apt update && apt install -y net-tools

# ros2 설정 
source /opt/ros/humble/setup.bash
export ROS_DOMAIN_ID=42

Elevation Mapping 설치

# 워크스페이스 생성
mkdir -p workspace/src

apt-get install libeigen3-dev
source /opt/ros/humble/setup.bash

# gridmap 설치
cd workspace/src
mkdir -p gridmap_dep/src
cd gridmap_dep
apt update && apt install -y wget
apt update && apt install -y curl
cd src
git clone https://github.com/ANYbotics/grid_map.git --branch humble
cd ..
rosdep update
rosdep install -y --ignore-src --from-paths src

# kindr 설치
cd /workspace/src
git clone https://github.com/ANYbotics/kindr.git
cd kindr
mkdir build
cd build
cmake .. -DUSE_CMAKE=true
make install

# kinr_ros 설치
cd /workspace.src
git clone https://github.com/SivertHavso/kindr_ros.git --branch galactic

## colcon build -> error 발생할꺼임 / 밑에 ### kind_ros 패키지 빌드 관련 에러 해결 참고

# Point Cloud Library 다운
apt install libpcl-dev

# grid map 테스트
source /workspace/install/setup.bash 
ros2 launch grid_map_demos simple_demo_launch.py

# elevation_mapping git
cd /workspace/src
git clone https://github.com/Muhammad540/elevation_mapping.git
cd /worksapce
source /workspace/install/setup.bash 

# elevation mapping 실행
ros2 launch elevation_mapping elevationMapping_launch.py

# Traversablility Estimation 설치
cd /workspace/src
git clone https://github.com/Muhammad540/traversability_estimation.git

## colcon build error 발생할꺼임 / 밑에 ### traversability_estimation 패키지 빌드 관련 에러 해결 참고
cd /workspace
colcon build

# Traversablility Estimation 실행
source /workspace/install/setup.bash 
ros2 launch traversability_estimation traversability_estimation.launch.py

kind_ros 패키지 빌드 관련 에러 해결

# kind_msgs의 CmakeLists.txt 수정
cmake_minimum_required(VERSION 3.5.1)
project(kindr_msgs)

find_package(ament_cmake_auto)
ament_auto_find_build_dependencies()

rosidl_generate_interfaces(${PROJECT_NAME}
  "msg/VectorAtPosition.msg"
  DEPENDENCIES std_msgs geometry_msgs
  ADD_LINTER_TESTS
)

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  ament_lint_auto_find_test_dependencies()
endif()

ament_auto_package()

kind_msgs의 package.xml 수정

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>kindr_msgs</name>
  <version>0.3.4</version>
  <description>Kindr common msgs</description>
  <maintainer email="rdiethelm@anybotics.com">Remo Diethelm</maintainer>
  <maintainer email="pleemann@anybotics.com">Philipp Leemann</maintainer>

  <license>BSD</license>
  <url type="website">https://github.com/anybotics/kindr_ros</url>

  <author email="pfankhauser@anybotics.com">Peter Fankhauser</author>

  <buildtool_depend>ament_cmake_auto</buildtool_depend>

  <build_depend>rosidl_default_generators</build_depend>
  <build_depend>geometry_msgs</build_depend>
  <build_depend>std_msgs</build_depend>

  <exec_depend>rosidl_default_runtime</exec_depend>
  <exec_depend>geometry_msgs</exec_depend>
  <exec_depend>std_msgs</exec_depend>

  <test_depend>ament_lint_auto</test_depend>

  <member_of_group>rosidl_interface_packages</member_of_group>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

kind_ros의 CmakeLists.txt 수정

cmake_minimum_required(VERSION 3.5)
project(kindr_ros)

# Use C++14 standard
if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 14)
endif()
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

find_package(ament_cmake REQUIRED)
find_package(tf2 REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(kindr REQUIRED)

# 추가: kindr의 include 경로를 지정합니다.
if(NOT kindr_INCLUDE_DIRS)
  set(kindr_INCLUDE_DIRS "/usr/local/include/kindr")
endif()
include_directories(${kindr_INCLUDE_DIRS})

find_package(eigen3_cmake_module REQUIRED)
find_package(Eigen3 REQUIRED)

set(dependencies
  eigen3_cmake_module
  Eigen3
  tf2
  geometry_msgs
)

install(DIRECTORY include/${PROJECT_NAME}/
        DESTINATION include/${PROJECT_NAME})

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  ament_lint_auto_find_test_dependencies()

  find_package(ament_cmake_gtest REQUIRED)
  ament_add_gtest(${PROJECT_NAME}-test
    test/test_main.cpp
    test/RosGeometryMsgPhysicalQuantitiesTest.cpp
    test/RosGeometryMsgRotationTest.cpp
    test/RosGeometryMsgPoseTest.cpp
    test/RosTfPoseTest.cpp
    test/TfConventionTest.cpp
    WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}/test
  )

  if(TARGET ${PROJECT_NAME}-test)
    target_include_directories(${PROJECT_NAME}-test PUBLIC
      include
      ${EIGEN3_INCLUDE_DIRS}
      ${kindr_INCLUDE_DIRS}  # 테스트 타겟에도 추가
    )
    ament_target_dependencies(${PROJECT_NAME}-test
      ${dependencies}
    )
  endif()
endif()

ament_export_include_directories(include)
ament_export_dependencies(
  ${dependencies}
)

ament_package()

kinr_ros의 package.xml 수정

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>kindr_ros</name>
  <version>0.3.4</version>
  <description>The kindr_ros package</description>
  <maintainer email="rdiethelm@anybotics.com">Remo Diethelm</maintainer>
  <maintainer email="pleemann@anybotics.com">Philipp Leemann</maintainer>

  <license>BSD</license>
  <url type="website">https://github.com/anybotics/kindr_ros</url>

  <author email="cgehring@anybotics.com">Christian Gehring</author>

  <buildtool_depend>ament_cmake</buildtool_depend>
  <buildtool_depend>eigen3_cmake_module</buildtool_depend>

  <buildtool_export_depend>eigen3_cmake_module</buildtool_export_depend>

  <depend>kindr</depend>
  <depend>geometry_msgs</depend>
  <depend>tf2</depend>
  <build_depend>eigen</build_depend>

  <test_depend>ament_cmake_gtest</test_depend>
  <test_depend>ament_lint_auto</test_depend>

  <build_export_depend>eigen</build_export_depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

traversability 패키지 빌드 관련 에러 해결

gedit /worksapce/src/traversability_estimation/traversability_estimation/src/TraversabilityEstimation.cpp 

# 239번째 ~ 240번째 주석처리
  // response->header.frame_id = traversabilityMap_.getMapFrameId();
  // response->header.stamp = nodeHandle_->now();

Go2- Elevation mapping 연동 진행

go2 - elevation mapping 연동 (진행중)

realsense 설치

  • depth camera의 pointcloud를 받기 위해 설치
  • sudo apt install ros-humble-librealsense2*
  • sudo apt install ros-humble-realsense2-*

ROS 2 humble에서, pointcloud msg /camera/camera/depth/color/points 안나옴

  • 버전, gpu, jetpack 등 다양하게 survey & local pc에서는 잘 나옴

  • depth image를 pointcloud로 변환해서, 진행

    # 노드 작성
ros2 run realsense_pointcloud_converter depth_to_pc_node

  • elevation mapping 연동 진행 (미완성 : depth point가 너무 sparse함)

보안사항

  • realsense node 최적화
  • pointcloud 범위 한정 / voxelization
  • elevation mapping test
  • jeston orin에서 테스트 진행

파라미터 정리

  1. Postprocessor Pipeline 파라미터
  2. postprocessor_pipeline: filter1: name: inpaint type: gridMapCv/InpaintFilter params: input_layer: elevation output_layer: elevation_inpainted radius: 0.05 filter2: name: surface_normals type: gridMapFilters/NormalVectorsFilter params: input_layer: elevation_inpainted output_layers_prefix: normal_vectors_ radius: 0.1 normal_vector_positive_axis: z
filter1 (inpaint)

type: gridMapCv/InpaintFilter
역할: Elevation Map에 “구멍(hole)”이 생긴 셀이나 NaN 영역을 보간(Interpolating)하여 매꿉니다.
매개변수
input_layer: elevation → 원본 레이어
output_layer: elevation_inpainted → 결과 레이어
radius: 0.05 → 인페인트(보간) 알고리즘이 주변 픽셀을 탐색할 때 사용하는 거리(단위 m).
filter2 (surface_normals)

type: gridMapFilters/NormalVectorsFilter
역할: 지도(elevation_inpainted)에서 각 셀의 표면 법선 벡터를 계산합니다.
매개변수
input_layer: elevation_inpainted → 인페인트된 고도 레이어 사용
output_layers_prefix: normal_vectors_ → 결과는 normal_vectors_x, normal_vectors_y, normal_vectors_z 등의 새 레이어들로 저장
radius: 0.1 → 주변 높이 정보를 사용해 표면 기울기/법선을 추정할 때 사용할 이웃 범위
normal_vector_positive_axis: z → 법선 벡터가 가능한 한 +Z 방향을 기준으로 계산되도록 설정

2. Sensor Processor 파라미터

sensor_processor:
ros__parameters:
type: structured_light
cutoff_min_depth: 0.2
cutoff_max_depth: 3.25
normal_factor_a: 0.000611
normal_factor_b: 0.003587
normal_factor_c: 0.3515
normal_factor_d: 0
normal_factor_e: 1
lateral_factor: 0.01576

type: structured_light

RealSense D435 같은 “Structured Light” 센서 특성(깊이 오차 모델)을 사용하는 프로세서로 설정.
cutoff_min_depth, cutoff_max_depth

센서에서 유효 범위를 벗어나는 깊이(너무 가깝거나 먼 포인트)를 무시합니다.
여기서는 0.2m보다 가까운 점, 3.25m보다 먼 점을 제거.
normal_factor_a, b, c, d, e, lateral_factor

Structured Light 센서의 노이즈(깊이 오차, 측방향 오차 등)를 모델링할 때 사용하는 파라미터들입니다.
대체로 A·B·C·D·E는 깊이에 따른 오차 함수를 구성하고, lateral_factor는 가로 방향(픽셀 x축)으로 증가하는 오차를 의미합니다.

3. Elevation Mapping 노드 파라미터

elevation_mapping:
ros__parameters:
min_update_rate: 1.0
time_tolerance: 0.5
sensor_processor/ignore_points_above: 0.4

robot_motion_map_update/covariance_scale_translation_x: 0.0
...
robot_motion_map_update/covariance_scale_rotation_z: 0.0

length_in_x: 10.0
length_in_y: 10.0
position_x: 0.0
position_y: 0.0
resolution: 0.1

min_variance: 0.0001
max_variance: 0.05
mahalanobis_distance_threshold: 2.5
multi_height_noise: 0.01
fused_map_publishing_rate: 5.0
enable_skip_lower_points: true
scanning_duration: 0.01
remove_penetrated_points_rate: 5.0
visibility_cleanup_rate: 10.0
enable_continuous_cleanup: false
min_update_rate (1.0)

Elevation Mapping 노드가 최소 몇 Hz로 업데이트되길 기대하는지 나타냅니다.
센서 입력이 너무 늦으면 경고를 내거나, 내부적으로 timeout 처리를 할 수 있습니다.
time_tolerance (0.5)

TF 변환을 요청할 때 허용되는 타임스탬프 오차(초).
센서 메시지와 TF가 완벽히 같은 시간을 갖지 않을 수 있으므로, 이 오차 내에서 보정해줍니다.
sensor_processor/ignore_points_above (0.4)

포인트 클라우드에서 특정 기준 이상(높이 차가 0.4m 이상)인 점들을 무시할 수 있습니다.
보통 센서 기준으로 너무 높게 뜬 노이즈 제거 목적으로 사용됩니다(세부 해석은 설정에 따라 다를 수 있음).
robot_motion_map_update/covariance_scale_* (모두 0.0)

로봇이 움직일 때, 지도 셀의 분산(variance)을 추가로 증가시키는 스케일 계수.
여기서 전부 0.0으로 설정되어 있어, “로봇 이동에 따른 추가 불확실도”를 고려하지 않는 상태입니다.
length_in_x, length_in_y (10.0)

Elevation Map의 X·Y 방향 크기(미터). 여기서는 10m x 10m 지도.
맵의 이동 윈도우(로봇 주변을 유지) 형태나 고정 지도 형태로 운용될 때 사용.
position_x, position_y (0.0)

맵의 중심 좌표를 world에서 어디로 잡을지 설정.
일반적으로 초기값 0.0, 0.0으로 두고, 로봇이 움직이면서 갱신할 수도 있음.
resolution (0.1)

지도 해상도(셀 크기). 0.1m → 1칸 = 10cm.
크기가 10m이고 해상도가 0.1m이면, 대략 100 x 100 셀 정도가 됩니다.
min_variance, max_variance

지도 셀 높이 추정치의 분산(variance) 최소·최대값 제한.
너무 확신이 높아져 분산이 0으로 수렴하지 않도록 하거나(최소값), 너무 커지지 않도록 최대값을 둠.
mahalanobis_distance_threshold (2.5)

새로 들어온 측정(포인트)이 현재 셀의 높이 통계와 비교했을 때, 어느 정도 차이가 나면 outlier로 처리할지 결정하는 임계값.
분산을 고려한 Mahalanobis distance를 사용.
multi_height_noise (0.01)

하나의 셀이 여러 다른 높이 레벨을 동시에 관측할 때(예: 책상 위+바닥), 추가로 노이즈(분산)를 늘릴지 결정하는 계수.
fused_map_publishing_rate (5.0)

최종 맵(융합된 Elevation Map)을 몇 Hz로 퍼블리시할지 설정. 5면 5Hz.
enable_skip_lower_points (true)

이미 지도에 기록된 높이보다 훨씬 낮은(즉 지면 아래) 포인트가 들어올 경우, 노이즈로 보고 무시할 수 있는 옵션.
scanning_duration (0.01)

센서가 들어오는 한 번의 콜백 내에서 포인트들을 처리하는 데 할당할 최대 시간.
실시간성 보장을 위해 짧게 잡으면, 한 주기에 다 못 처리한 점이 있으면 다음 cycle로 넘길 수 있습니다.
remove_penetrated_points_rate (5.0)

맵 상 표면보다 아래(관통)된 것으로 보이는 점들을 제거하는 주기(Hz).
5.0이면 1초에 5번 관통점 제거 작업을 수행.
visibility_cleanup_rate (10.0)

센서에서 보이지 않는 부분(장기간 미관측)이나 사라진 물체 등을 맵에서 정리(클린업)하는 주기(Hz).
enable_continuous_cleanup: false이므로, 여기서 설정된 Hz마다 한 번씩만 실행.
enable_continuous_cleanup (false)

true로 설정 시, 맵 정리(클린업)를 수시로(연속적으로) 진행. 여기서는 주기적(cleanup_rate)으로만 수행.


4. 최상위 Elevation Mapping 설정 및 Inputs

/**:
ros__parameters:
inputs: ['ground_truth_cloud']
ground_truth_cloud:
type: pointcloud
topic: /nvblox_node/back_projected_depth/front_stereo_camera_left_optical
queue_size: 10
publish_on_update: true
sensor_processor:
type: laser
frame_id: front_stereo_camera_left_optical

map_frame_id: "map"
robot_base_frame_id: "base_link"
robot_pose_with_covariance_topic: "/chassis/odom"
track_point_frame_id: "base_link"
track_point_x: 0.0
track_point_y: 0.0
track_point_z: 0.0
inputs: ['ground_truth_cloud']

Elevation Mapping 노드가 입력으로 사용할 센서(포인트클라우드) “이름” 리스트입니다.
ground_truth_cloud 세부

type: pointcloud → 이 입력은 PointCloud를 의미.
topic: /nvblox_node/back_projected_depth/front_stereo_camera_left_optical → 실제로 구독할 ROS 토픽 이름.
queue_size: 10 → 내부 구독 버퍼 크기.
publish_on_update: true → 이 센서로 업데이트가 발생할 때마다 맵을 갱신할지 여부.
sensor_processor.type: laser → 이 포인트클라우드에 대해 Laser 센서 모델을 적용(간단하게 처리)할 수 있음.
frame_id: front_stereo_camera_left_optical → 이 입력 데이터를 “front_stereo_camera_left_optical” 프레임 기준으로 해석.
map_frame_id: "map"

Elevation Mapping이 최종 지도를 표현할 좌표계(최상위 프레임) = "map".
robot_base_frame_id: "base_link"

로봇의 기준 프레임. Elevation Mapping은 TF를 통해 “map -> base_link” 변환을 lookup.
robot_pose_with_covariance_topic: "/chassis/odom"

로봇의 위치/자세 추정을 포함한 PoseWithCovarianceStamped(혹은 Odometry)를 구독하는 토픽.
내부적으로 TF lookup 시 참조할 수도 있고, 또는 속도·가속도 추정에 활용.
track_point_frame_id: "base_link" + track_point_x/y/z

맵을 로봇 중심 기준으로 이동 윈도우 형태로 운영할 때, “어느 점”을 기준으로 맵을 업데이트할지 결정.
여기서는 “base_link” 프레임의 (0,0,0) 지점을 트래킹.
반응형