소개
AutoDrive 시뮬레이터에서는 기본적으로 Odom 토픽을 제공하지 않습니다. Technical Guide에서 좌표와 관련된 /autodrive/roboracer\_1/ips 토픽이 제공되는 것을 알 수 있었지만, 실제 대회에서는 사용할 수 없다고 표시되어있습니다.
여러분들도 아시다시피 현재 위치를 알아야 SLAM을 하거나 주행 알고리즘을 사용할테니 Odom 토픽을 만드는건 필수입니다. 그리고 AutoDrive에서는 /autodrive/roboracer_1/left_encoder /autodrive/roboracer_1/right_encoder /autodrive/roboracer_1/ips /autodrive/roboracer_1/imu /autodrive/roboracer_1/lidar 같은 토픽을 사용할 수 있습니다.
지금 제 생각으로는 4가지 정도 방법으로 Odom을 만들 수 있을 것 같습니다. 1. 대회에서는 사용할 수 없지만 ips와 imu로 odom을 만든다. 2. 바퀴 회전량과 imu를 이용해서 만든다. 3. 맵이 있어야하지만 lidar를 이용하는 파티클필터로 만든다. 그리고 이번 글의 주제인 rf2o laser odometry로 만든다 정도입니다.
rf2o는 라이다 센서 값으로 로봇의 이동 경로를 추종하는 방법이며 깃허브에 ros2 패키지로 제공되고 있습니다. 다른 사람이 만들어 놓은 패키지를 사용하기만 하면 되기 때문에 가장 간단하게 Odom을 만들 수 있습니다. 그래서 일단 이번 글에서는 rf2o를 사용해보고 다음 글에서는 1번 2번 방법을 사용해보겠습니다.
그리고 글을 시작하기전에 저는 기계, 전자, 자동차, 로봇 등의 공학을 전공하지 않았습니다. 그래서 로봇 제어, 물리 같은 부분에 잘못된 정보가 있을 수 있습니다. 그래서 잘못된 정보는 댓글로 알려주시면 감사하겠습니다.
1. rf2o 패키지 가져오기
먼저 지난번에 만든 api 컨테이너에 접속합니다.
docker exec -it autodrive_roboracer_api bash
또는
docker run -it --name autodrive_roboracer_api -p 4567:4567 --ipc=host --entrypoint /bin/bash -v /tmp/.X11-unix:/tmp/.X11-unix:rw --env DISPLAY=host.docker.internal:0.0 --privileged --gpus all autodriveecosystem/autodrive_roboracer_api:2025-icra-compete
현재 /home/autodrive_devkit 경로에 위치하고 있으며 cd src 명령어로 src 폴더로 이동합니다.
그리고 rf2o 패키지를 git clone 명령어로 가져옵니다.
git clone -b ros2 https://github.com/MAPIRlab/rf2o_laser_odometry.git
아래 사진처럼 ls 명령어를 사용했을 때 `
rf2o_laser_odometry`
폴더가 생성되어 있어야 합니다.
그리고 tmux로 다시 접속하고 브릿지를 실행합니다.
ros2 launch autodrive_roboracer bringup_graphics.launch.py
rf2o 패키지를 사용하기 위해서는 빌드 -> 소스 로드 과정을 진행해야합니다.
colcon build && source install/setup.bash
그리고 rf2o 런치 실행 명령어를 실행해 봅시다.
ros2 launch rf2o_laser_odometry rf2o_laser_odometry.launch.py
그러면 아래 사진처럼 `Waiting for laser_scans` 출력이 나타납니다.
사실 AutoDrive에서 제공하는 토픽 이름이 맞지 않아서 발생하는 당연한 결과입니다. 이제부터 rf2o를 사용할 수 있도록 토픽과 프레임을 맞춰주는 작업을 진행하겠습니다.
2. AutoDrive 좌표계 구조 살펴보기
Odom을 만들기 위해서는 일단 자동차의 좌표계 같은 정보를 일단 알아야합니다. 그래서 터미널에 아래 명령어로 rqt를 설치해줍니다.
apt install ros-humble-rqt*
그리고 `rqt` 명령어를 터미널에 입력하여 실행합니다. 현재 rf2o는 종료된 상태이고 브릿지만 실행한 상태에서 현재 tf 및 토픽을 시각화할 수 있습니다.
토픽과 tf tree를 실행하면 아래 처럼 나타납니다.
그리고 브릿지 코드로 가서 보면 아래 코드가 있는 것을 볼 수 있는데, 이 부분을 보면 tf transform을 수행하는 코드가 있습니다.
broadcast_transform(msg_transform, transform_broadcaster, "roboracer_1", "world", autodrive.position, autodrive.orientation_quaternion) # Vehicle frame defined at center of rear axle
broadcast_transform(msg_transform, transform_broadcaster, "left_encoder", "roboracer_1", np.asarray([0.0, 0.12, 0.0]), quaternion_from_euler(0.0, 120*autodrive.encoder_angles[0]%6.283, 0.0))
broadcast_transform(msg_transform, transform_broadcaster, "right_encoder", "roboracer_1", np.asarray([0.0, -0.12, 0.0]), quaternion_from_euler(0.0, 120*autodrive.encoder_angles[1]%6.283, 0.0))
broadcast_transform(msg_transform, transform_broadcaster, "ips", "roboracer_1", np.asarray([0.08, 0.0, 0.055]), np.asarray([0.0, 0.0, 0.0, 1.0]))
broadcast_transform(msg_transform, transform_broadcaster, "imu", "roboracer_1", np.asarray([0.08, 0.0, 0.055]), np.asarray([0.0, 0.0, 0.0, 1.0]))
broadcast_transform(msg_transform, transform_broadcaster, "lidar", "roboracer_1", np.asarray([0.2733, 0.0, 0.096]), np.asarray([0.0, 0.0, 0.0, 1.0]))
broadcast_transform(msg_transform, transform_broadcaster, "front_camera", "roboracer_1", np.asarray([-0.015, 0.0, 0.15]), np.asarray([0, 0.0871557, 0, 0.9961947]))
broadcast_transform(msg_transform, transform_broadcaster, "front_left_wheel", "roboracer_1", np.asarray([0.33, 0.118, 0.0]), quaternion_from_euler(0.0, 0.0, np.arctan((2*0.141537*np.tan(autodrive.steering))/(2*0.141537-2*0.0765*np.tan(autodrive.steering)))))
broadcast_transform(msg_transform, transform_broadcaster, "front_right_wheel", "roboracer_1", np.asarray([0.33, -0.118, 0.0]), quaternion_from_euler(0.0, 0.0, np.arctan((2*0.141537*np.tan(autodrive.steering))/(2*0.141537+2*0.0765*np.tan(autodrive.steering)))))
broadcast_transform(msg_transform, transform_broadcaster, "rear_left_wheel", "roboracer_1", np.asarray([0.0, 0.118, 0.0]), quaternion_from_euler(0.0, autodrive.encoder_angles[0]%6.283, 0.0))
broadcast_transform(msg_transform, transform_broadcaster, "rear_right_wheel", "roboracer_1", np.asarray([0.0, -0.118, 0.0]), quaternion_from_euler(0.0, autodrive.encoder_angles[1]%6.283, 0.0))
해당 코드는 bridge 함수로 시뮬레이터에서 소켓 통신으로 데이터가 들어오면 리스너처럼 계속 수행되는 코드라고 보시면 됩니다. 여기서 world -> roboracer_1 변환 코드를 보겠습니다.
broadcast_transform(msg_transform, transform_broadcaster, "roboracer_1", "world", autodrive.position, autodrive.orientation_quaternion)
위 코드의 `tf transform`을 제가 아는 선에서 설명하면 `roboracer_1` 프레임이 `world` 기준으로 `autodrive.position` 위치에 있고 `autodrive.orientation_quaternion`만큼 회전되어 있음으로 설정하는 코드입니다.
즉, `roboracer_1`의 위치와 자세를 `world` 좌표계 기준으로 설명하게 되는거라고 이해하였습니다.
world -> roboracer_1 말고 나머지 부분을 보시면 left_encoder, righgt_encoder, ips, imu 등의 프레임은 roboracer_1 기준으로 각각 좌표와 회전으로 설정되는 것으로 보시면 되겠습니다. 그래서 시뮬레이터에서 실제 자동차의 위치가 전송되면 roboracer의 위치는 변하고 나머지 센서의 좌표계 프레임도 자동차 기준으로 동일한 위치에 계속 부착되는? 것으로 보았습니다. 그리고 각각의 상수 값들은 AutoDrive 페이지에 정리되어 있었고 실제 코드로 작성된 것과 동일한 것을 확인할 수 있었습니다.
AutoDrive의 좌표계가 어떻게 설정되어있는지 알아보았습니다. 이제는 토픽을 살펴보도록 하겠습니다.
3. AutoDrive 토픽 구조 살펴보기
AutoDrive의 토픽은 아까 실행했던 `rqt`로 보는 방법과 그냥 명령어로 `ros2 topic list` 명령어로 살펴볼 수 있습니다. 지금은 rqt를 실행해놓았으니 rqt로 살펴보겠습니다.
rqt의 `/tf` 토픽 왼쪽에 체크박스를 체크하고 하단의 화살표를 열어보면 계속해서 프레임 id가 변경되는 것을 볼 수 있습니다. 이는 시뮬레이터에서 데이터를 전송하고 있는데 이걸 브릿지 코드가 받아서 계속해서 tf transform을 수행하고 있는것입니다.
현재 브릿지만 실행하였을 때 나타나는 토픽은 위 사진과 같으며 `imu`만 체크해서 다시 보겠습니다.
그러면 `header`를 클릭할 수 있고 보면 frame_id는 `imu`로 나타나고있습니다. 여기서는 토픽을 이런 방법으로 볼 수 있다는 것으로 마치고 본격적으로 Odom을 만드는 작업을 수행해보겠습니다.
4. AutoDrive 프레임 수정 및 rf2o odom 생성
먼저 현재 가장 위에 존재하는 프레임 이름은 `world`입니다. 실제 차량에서 사용할때는 map -> odom -> base_link 형태의 프레임 구조를 갖고 있습니다. 하지만 world라는 실제 시뮬레이터 좌표계에 차량 위치가 주어지므로 나중에 SLAM하고 파티클필터로 위치를 찾을 때를 대비하여 world -> map -> odom 형태로 좌표계를 만들어야합니다.
일단 `/home/autodrive_devkit/src/rf2o_laser_odometry/launch/rf2o_laser_odometry.launch.py` 코드의 `laser_scan_topic`을 autodrive에서 제공하는 lidar topic으로 수정하면 아래 코드처럼 수정할 수 있습니다.
그리고 odom topic도 미리 `/odom_rf2o`에서 `/odom`으로 바꿔줍시다.
Node(
package='rf2o_laser_odometry',
executable='rf2o_laser_odometry_node',
name='rf2o_laser_odometry',
output='screen',
parameters=[{
'laser_scan_topic' : '/autodrive/roboracer_1/lidar',
'odom_topic' : '/odom',
'publish_tf' : True,
'base_frame_id' : 'base_link',
'odom_frame_id' : 'odom',
'init_pose_from_topic' : '',
'freq' : 20.0}],
),
이 상태에서 빌드하고 브릿지와 rf2o를 실행해줍시다.
그러면 `TF` 토픽에 Status 경고가 표시되는것을 볼 수 있습니다.
그리고 rqt를 확인해보면 world와 odom 토픽이 분리되어 나타나는것을 볼 수 있습니다. rviz에서 `global options fixed frame`을 `odom`으로 설정하고 `odom`토픽을 시각화하면 아래 사진처럼 odom을 볼 수 있습니다. 여기서 시뮬레이터의 차량을 움직이면 초기 위치에서 벗어나 odom이 움직이는 것을 볼 수 있습니다.
하지만 lidar나 다른 토픽들이 나타나지 않습니다. 이것은 당연한것으로 기준 좌표계가 달라서 표시할 수 없는것입니다. 저는 여기서 map 밑에 odom을 연결하고 base_link에 scan과 imu를 연결해주려고합니다.
왜 기존에 있는 센서를 사용하지 않느냐라고 할 수 있는데 world 좌표계에 roboracer_1이 이미 연결되어있기 때문에 똑같은 센서 데이터를 기준 좌표계와 토픽 이름만 변경하여 base_link 밑에 프레임으로 만들어서 퍼블리시 해줄겁니다. 이렇게 하면 실제 데이터는 world 기준으로 보면 되고 센서 데이터로 나타나는 상태 차량 정보는 base_link 밑에 프레임으로 보게 할 수 있습니다.
ros2 pkg create tf_transform_f1tenth --build-type ament_cmake --dependencies rclcpp tf2_ros geometry_msgs
명령어로 패키지를 만들어줍니다. 해당 패키지는 `src` 폴더 안에서 명령어를 실행하여 만들면 됩니다. 하지만 해당 패키지를 만드는 과정을 전부 설명할 수 없으니 깃허브로 패키지를 공유하겠습니다.
아래 과정을 따라하시면 제가 만든 tf transform 패키지를 다운받고 실행할 수 있습니다. 먼저 `/home/autodrive_devkit` 경로에서 시작하는지 확인해주세요. 그리고 아래 과정을 따라하시면 됩니다.
1. src 이동 및 tf_transform_f1tenth 패키지 git clone
cd src && git clone https://github.com/wndudwkd003/tf_transform_f1tenth.git
cd ..
다시 `/home/autodrive_devkit` 경로로 이동합니다.
2. 패키지 빌드 및 source
colcon build
source install/setup.bash
여기서 source는 각 터미널마다 전부 해주셔야합니다.
3. 브릿지, rf2o, tf_transform_f1tenth 패키지 각각 실행
또한 시뮬레이터와 연결되어있어야합니다.
# 터미널 1
ros2 launch autodrive_roboracer bringup_graphics.launch.py
# 터미널2
ros2 launch rf2o_laser_odometry rf2o_laser_odometry.launch.py
# 터미널3
ros2 run tf_transform_f1tenth tf_publish_for_rf2o_node
그러면 터미널 3번째에서는 `[static_tf_publisher_node]: Published static transforms` 메시지가 출력되고 rviz에서는 아래 사진처럼 fixed frame이 `world`인 상태에서도 `odom` 토픽을 정상적으로 볼 수 있습니다.
여기서 odom 위치와 차량의 위치와 다르게 나오는데 이게 사실 원래 정상입니다. 초기 위치를 0, 0, 0으로 주었기 때문에 맞지 않는것입니다. 그리고 여기서 rqt를 실행해서 tf tree를 확인해보면 아래 사진처럼 프레임이 구성됩니다.
그리고 차량을 움직여보면 회전하는만큼, 이동하는 만큼 odom이 움직이는 것을 볼 수 있습니다. 여기서 어느정도 오차가 발생할 수 있는데 이는 차량의 상태를 추종하는데 오직 lidar 데이터만 사용하기 때문입니다. 여기서 차량의 바퀴 회전량, IMU 센서 등을 사용하고 보정을 하면 더 정확한 데이터를 얻을 수 있을겁니다. 해당 과정은 다음에 진행해보려고 합니다.
이제 odom 토픽이 만들어졌고 slam을 진행할 준비가 되었습니다. 다음에서 slam tool box를 활용해서 slam을 진행하겠습니다.
4. SLAM
SLAM에 대해서 정확한 소개는 다음에 적어보겠습니다. 여기서 SLAM은 SLAM Tool Box를 이용한다 정도로만 알고계시면 될것같습니다.
터미널에서 아래 명령어를 입력하여 SLAM Tool Box를 설치합니다.
apt update && apt install ros-humble-slam-toolbox -y
만약 설치 과정에서 에러가 발생하면 ` apt-get update --fix-missing` 명령어를 수행하고 다시 진행해보세요.
그리고 `/home/autodrive_devkit/src/config` 해당 경로의 폴더를 만들고 `slam_params.yaml` 파일을 만들어줍니다. 내용은 아래 yaml 코드를 복사하여 붙여넣습니다.
아래 더보기 버튼을 누르면 yaml 파일을 볼 수 있습니다.
slam_toolbox:
ros__parameters:
# Plugin params
solver_plugin: solver_plugins::CeresSolver
ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
ceres_preconditioner: SCHUR_JACOBI
ceres_trust_strategy: LEVENBERG_MARQUARDT
ceres_dogleg_type: TRADITIONAL_DOGLEG
ceres_loss_function: None
# ROS Parameters
odom_frame: odom
map_frame: map
base_frame: base_link
scan_topic: /scan
use_map_saver: true
mode: mapping #localization
# if you'd like to immediately start continuing a map at a given pose
# or at the dock, but they are mutually exclusive, if pose is given
# will use pose
#map_file_name: test_steve
#map_start_pose: [0.0, 0.0, 0.0]
#map_start_at_dock: true
debug_logging: false
throttle_scans: 1
transform_publish_period: 0.02 #if 0 never publishes odometry
map_update_interval: 5.0
resolution: 0.05
min_laser_range: 0.0 #for rastering images
max_laser_range: 20.0 #for rastering images
minimum_time_interval: 0.5
transform_timeout: 0.2
tf_buffer_duration: 30.
stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps
enable_interactive_mode: true
# General Parameters
use_scan_matching: true
use_scan_barycenter: true
minimum_travel_distance: 0.5
minimum_travel_heading: 0.5
scan_buffer_size: 10
scan_buffer_maximum_scan_distance: 10.0
link_match_minimum_response_fine: 0.1
link_scan_maximum_distance: 1.5
loop_search_maximum_distance: 3.0
do_loop_closing: true
loop_match_minimum_chain_size: 10
loop_match_maximum_variance_coarse: 3.0
loop_match_minimum_response_coarse: 0.35
loop_match_minimum_response_fine: 0.45
# Correlation Parameters - Correlation Parameters
correlation_search_space_dimension: 0.5
correlation_search_space_resolution: 0.01
correlation_search_space_smear_deviation: 0.1
# Correlation Parameters - Loop Closure Parameters
loop_search_space_dimension: 8.0
loop_search_space_resolution: 0.05
loop_search_space_smear_deviation: 0.03
# Scan Matcher Parameters
distance_variance_penalty: 0.5
angle_variance_penalty: 1.0
fine_search_angle_offset: 0.00349
coarse_search_angle_offset: 0.349
coarse_angle_resolution: 0.0349
minimum_angle_penalty: 0.9
minimum_distance_penalty: 0.5
use_response_expansion: true
여기서 대부분 똑같고 `base_frame`이 변경되었습니다. 그리고 아래 명령어를 새로운 터미널에서 실행하여봅시다. 현재 브릿지, rf2o, tf_transform_f1tenth 패키지 3개가 전부 실행되어있어야합니다. 그리고 올바른 맵을 그리기 위해서 시뮬레이터를 한 번 리셋하여 초기 위치로 이동시키는 것을 추천드리겠습니다.
만약 slam tool box 실행에 에러가 발생하면 `source /opt/ros/humble/setup.bash` 명령어를 입력하고 다시 진행해주세요.
ros2 launch slam_toolbox online_sync_launch.py slam_params_file:=/home/autodrive_devkit/src/config/slam_params.yaml
slam tool box 실행 명령어를 실행하고 rviz에 Map 토픽을 추가하면 아래 사진처럼 시뮬레이터 맵에 대해 occupancy grid map이 생성됩니다.
여기서 `/scan` 토픽을 시각화하면 아래처럼 현재 맵에 대해서 딱 맞게 lidar가 시각화됩니다. 아래 사진에서 흰색이 재퍼블리시하고있는 `/scan` 토픽입니다.
현재 상황에서 차를 조심스럽게 움직여서 전체 맵을 그려주시면 시뮬레이터 맵을 그릴 수 있습니다.
전체 맵을 돌았을 때 위 사진 처럼 그릴 수 있습니다. 차를 조금 천천히 움직이거나 맵이 그리고 움직이거나 하게 되면 조금 더 깨끗한 맵을 그릴 수 있을겁니다. 어느정도 맵을 다 그렸으면 상단에 `Panels` 탭을 클릭합니다.
그리고 `Add New Panel` 버튼을 클릭 -> `slam_toolbox` -> `SlamToolboxPugin` 클릭 후 `OK` 버튼을 눌러줍니다. 그러면 왼쪽 하단에 새롭게 패널이 추가되고 `Save Map` 버튼 오른쪽 칸에 저장할 맵 이름을 입력하고 `Save Map` 버튼을 눌러줍니다.
그러면 아래 사진처럼 맵이 `.pgm` 확장자로 저장됩니다. `gimp`로 확인하셔도 좋습니다. (gimp /home/autodrive_devkit/save_map_name.pgm) 하지만 저는 왜인지 모르겠지만 gimp 실행이 안되어서 photoshop으로 확인하였습니다.
위 사진처럼 나타납니다. 차량을 좀 천천히 움직이면 더 깨끗한 맵이 나오겠지만 여기서는 단순히 확인하기 위함이기 때문에 이정도만 하겠습니다.
결론
이번 과정을 통해서 AutoDrive에서 제공하는 토픽과 프레임에 추가로 `map`, `odom`, `base_link`, `scan`, `imu_link` 프레임을 추가하고 `/odom` 토픽을 `rf2o` 패키지로 만들 수 있었으며 `/scan`, `/imu` 토픽을 새로 재퍼블리시 하는 과정에서 새로 만든 프레임에 tf transform을 수행하였습니다.
새롭게 만든 프레임과 토픽을 통해 `slam tool box`로 시뮬레이터 맵을 `occupancy grid map`으로 만들 수 있었습니다. 이번 과정은 다음에 진행할 파티클필터 localization을 위한 과정이었습니다. 다음 게시글은 파티클필터를 설치하고 실행해보도록 하겠습니다.
그리고 그 다음 과정은 rf2o odom을 사용하는 것이 아니라 다른 방법으로 odom을 만들어보겠습니다.
잘못된 정보나 질문이 있으시면 언제든 댓글로 문의주시길 바랍니다. 감사합니다.
'f1tenth' 카테고리의 다른 글
| [AutoDrive] Window 11 로컬 f1tenth autodrive 시뮬레이터 및 docker api 환경 구축 (2) | 2025.05.11 |
|---|