이번 포스팅은 이전 포스팅에서 구축한 환경을 기반으로 Gazebo를 이용해서 2D LiDAR를 부착하고 동작을 확인해 보는 과정입니다.
이 포스트는 다음 영상의 내용을 기반으로 개발하고자 하는 로봇에 적합하게 수정했습니다.
How do we add LIDAR to a ROS robot?
이 포스트는 다음 과정을 완료한 후에 참고하시길 바랍니다.
1. LiDAR 란
- 한국어 위키에는 LiDAR에 대해서 아래와 같이 설명하고 있습니다.
◦ 라이다(LIDAR/LiDAR, light detection and ranging” 또는 “laser imaging, detection, and ranging”의 약자))는 레이저 펄스를 쏘고 반사되어 돌아오는 시간을 측정하여 반사체의 위치좌표를 측정하는 레이다 시스템이다. 항공 또는 위성탑재되어 지형측량에 사용되며 스피드 건, 자율이동로봇, 자율주행 자동차 등에도 활용되는데 이 시스템이 각광받고 있다. 미국의 상장사인 벨로다인과 루미나 테크놀로지의 차량용 라이다 개발이 가장 앞선 것으로 알려져 있다. - LiDAR는 크게 2D LiDAR와 3D LiDAR로 나눌 수 있습니다.
- 2D LiDAR는 관찰 지점으로부터 2차원 상의 평면 정보를 제공합니다. 즉 센서와 평행한 평면을 기준으로 물체와의 거리가 얼마나 떨어져 있는지 확인할 수 있습니다.
- 3D LiDAR는 관찰 지점으로부터 3차원 상의 공간 정보를 제공합니다. 즉 센서의 위치를 기준으로 공간에 물체와의 거리가 얼마나 떨어져 있는지 확인할 수 있습니다.
- 또한 2D와 3D 정보를 동시에 제공하는 LiDAR가 있습니다.
- 이번 포스트에서는 2D LiDAR를 시뮬레이션해 보도록 하겠습니다.
2. 실행파일 생성
- 아래 명령을 실행해서 'src/urdf_tutorial/urdf/robot_3.xacro' 파일을 'src/urdf_tutorial/urdf/robot_4.xacro' 파일로 복사합니다.
$ cd ~/Workspace/ros_ws
$ cp src/urdf_tutorial/urdf/robot_3.xacro src/urdf_tutorial/urdf/robot_4.xacro- 다음은 ‘src/urdf_tutorial/launch/robot_4.launch.py’ 파일을 만들고 아래와 같이 편집합니다. 파일의 내용은 ‘src/urdf_tutorial/launch/robot_3.launch.py’에서 'robot_3.xacro'을 'robot_4.xacro'으로 변경했습니다.
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.substitutions import LaunchConfiguration
from launch.actions import DeclareLaunchArgument
from launch_ros.actions import Node
import xacro
def generate_launch_description():
use_sim_time = LaunchConfiguration("use_sim_time")
pkg_path = os.path.join(get_package_share_directory("urdf_tutorial"))
xacro_file = os.path.join(pkg_path, "urdf", "robot_4.xacro")
robot_description = xacro.process_file(xacro_file)
params = {"robot_description": robot_description.toxml(), "use_sim_time": use_sim_time}
return LaunchDescription(
[
DeclareLaunchArgument(
"use_sim_time", default_value="false", description="use sim time"
),
Node(
package="robot_state_publisher",
executable="robot_state_publisher",
output="screen",
parameters=[params],
),
]
)- 다음은 ‘src/urdf_tutorial/launch/lidar.launch.py’ 파일을 만들고 아래와 같이 편집합니다. 파일의 내용은 ‘src/urdf_tutorial/launch/sim.launch.py’에서 'robot_3.launch.py'을 'robot_4.launch.py'으로 변경했습니다.
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node
def generate_launch_description():
package_name = "urdf_tutorial"
rsp = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
[os.path.join(get_package_share_directory(package_name), "launch", "robot_4.launch.py")]
),
launch_arguments={"use_sim_time": "true"}.items(),
)
# Include the Gazebo launch file, provided by the gazebo_ros package
gazebo = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
[os.path.join(get_package_share_directory("gazebo_ros"), "launch", "gazebo.launch.py")]
),
)
# Run the spawner node from the gazebo_ros package. The entity name doesn't really matter if you only have a single robot.
spawn_entity = Node(
package="gazebo_ros",
executable="spawn_entity.py",
arguments=["-topic", "robot_description", "-entity", "with_robot"],
output="screen",
)
# Launch them all!
return LaunchDescription(
[
rsp,
gazebo,
spawn_entity,
]
)3. URDF에 LiDAR 추가하기
- URDF를 수정해서 자동차에 2D LiDAR 구조물을 부착하는 과정입니다.
- 'src/urdf_tutorial/urdf/robot_4.xacro' 파일을 상단에 아래 내용을 추가해서 red material을 추가합니다. 새로 추가되는 LiDAR를 빨간색으로 설정하기 위함입니다.
<material name="red">
<color rgba="1 0 0 1"/>
</material>- 'src/urdf_tutorial/urdf/robot_4.xacro' 파일의 ‘caster_wheel’ link 밑에 아래 내용을 추가해서 2D LiDAR 형상을 만듭니다.
◦ laser_joint: fixed joint로 2D LiDAR가 자동차의 중앙 위쪽에 자리 잡을 수 있도록 위치를 조정했습니다.
◦ laser_frame: 동그란 모양의 반지름 3cm, 높이 3cm 인 cylinder입니다. 색깔은 빨간색으로 설정했습니다.
<!-- LiDAR -->
<joint name="laser_joint" type="fixed">
<parent link="body"/>
<child link="laser_frame"/>
<origin xyz="0.1 0 0.075" rpy="0 0 0"/>
</joint>
<link name="laser_frame">
<visual>
<geometry>
<cylinder radius="0.03" length="0.03"/>
</geometry>
<material name="red"/>
</visual>
<collision>
<geometry>
<cylinder radius="0.03" length="0.03"/>
</geometry>
</collision>
</link>- 첫 번째 터미널에서 아래 명령을 실행합니다.
$ cd ~/Workspace/ros_ws/
$ colcon build --symlink-install
$ source install/setup.bash
$ ros2 launch urdf_tutorial lidar.launch.py- Gazebo에서 자동차에 LiDAR 형상이 추가된 것을 확인할 수 있습니다.
4. 2D LiDAR 기능 추가하기
- 이전 단계에서 추가된 2D LiDAR 구조물에 2D LiDAR 기능을 추가하는 과정입니다.
- 이전에 첫 번째 터미널에서 실행했던 기능은 ‘CTRL+C’를 눌러서 프로그램을 모두 종료합니다.
- ‘src/urdf_tutorial/urdf/lidar.xacro’ 파일을 만들고 아래와 같이 수정합니다.
◦ update_rate: 초당 10회 정보를 제공합니다.
◦ samples: 검색 구간 내에서 4개의 광선을 발사합니다. (이 부분을 조절하면서 간단한 테스트를 진행하겠습니다.)
◦ min_angle, max_angle: 2D LiDAR가 스캔할 구간입니다. $-\pi$부터 $+\pi$까지 $2\pi$ 전체를 스캔합니다.
◦ range min/max: 2D LiDAR가 스캔 가능한 거리의 최소, 최대 값입니다.
◦ argument: ros에 전달할 topic 이름입니다.
◦ output_type: 출력 형식입니다.
◦ frame_name: 2D LiDAR가 부착된 센서 이름입니다. - 자세한 내용은 ROS 2 Migration: Ray sensors를 참고하세요.
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<gazebo reference="laser_frame">
<material>Gazebo/Red</material>
<sensor name="laser" type="ray">
<pose>0 0 0 0 0 0</pose>
<visualize>true</visualize>
<update_rate>10</update_rate>
<ray>
<scan>
<horizontal>
<samples>4</samples>
<min_angle>-3.14</min_angle>
<max_angle>3.14</max_angle>
</horizontal>
</scan>
<range>
<min>0.3</min>
<max>12</max>
</range>
</ray>
<plugin name="laser_controller" filename="libgazebo_ros_ray_sensor.so">
<ros>
<argument>~/out:=scan</argument>
</ros>
<output_type>sensor_msgs/LaserScan</output_type>
<frame_name>laser_frame</frame_name>
</plugin>
</sensor>
</gazebo>
</robot>- 'src/urdf_tutorial/urdf/robot_4.xacro' 파일에 아래 내용을 추가해서 'lidar.xacro'를 포함해 줍니다.
<xacro:include filename="lidar.xacro"/>- 'src/urdf_tutorial/urdf/robot_4.xacro' 파일의 최종 내용은 아래와 같습니다.
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="urdf_tutorial">
<!-- MACROS -->
<xacro:include filename="macros.xacro"/>
<!-- COLOR -->
<material name="white">
<color rgba="1 1 1 1" />
</material>
<material name="blue">
<color rgba="0 0 1 1"/>
</material>
<material name="black">
<color rgba="0 0 0 1"/>
</material>
<material name="red">
<color rgba="1 0 0 1"/>
</material>
<!-- BASE LINK -->
<link name="base_link">
</link>
<!-- BODY LINK -->
<joint name="body_joint" type="fixed">
<parent link="base_link"/>
<child link="body"/>
<origin xyz="-0.12 0 0"/>
</joint>
<link name="body">
<visual>
<origin xyz="0.1 0 0.03"/>
<geometry>
<box size="0.2 0.1 0.06"/>
</geometry>
<material name="white"/>
</visual>
<collision>
<origin xyz="0.1 0 0.03"/>
<geometry>
<box size="0.2 0.1 0.06"/>
</geometry>
</collision>
<xacro:inertial_box mass="0.5" x="0.2" y="0.1" z="0.06">
<origin xyz="0.1 0 0.03" rpy="0 0 0"/>
</xacro:inertial_box>
</link>
<gazebo reference="body">
<material>Gazebo/White</material>
</gazebo>
<!-- LEFT WHEEL LINK -->
<joint name="left_wheel_joint" type="continuous">
<parent link="base_link"/>
<child link="left_wheel"/>
<origin xyz="0 0.065 0" rpy="-${pi/2} 0 0" />
<axis xyz="0 0 1"/>
</joint>
<link name="left_wheel">
<visual>
<geometry>
<cylinder radius="0.03" length="0.03"/>
</geometry>
<material name="blue"/>
</visual>
<collision>
<geometry>
<cylinder radius="0.03" length="0.03"/>
</geometry>
</collision>
<xacro:inertial_cylinder mass="0.1" length="0.03" radius="0.03">
<origin xyz="0 0 0" rpy="0 0 0"/>
</xacro:inertial_cylinder>
</link>
<gazebo reference="left_wheel">
<material>Gazebo/Black</material>
</gazebo>
<!-- RIGHT WHEEL LINK -->
<joint name="right_wheel_joint" type="continuous">
<parent link="base_link"/>
<child link="right_wheel"/>
<origin xyz="0 -0.065 0" rpy="${pi/2} 0 0" />
<axis xyz="0 0 -1"/>
</joint>
<link name="right_wheel">
<visual>
<geometry>
<cylinder radius="0.03" length="0.03"/>
</geometry>
<material name="blue"/>
</visual>
<collision>
<geometry>
<cylinder radius="0.03" length="0.03"/>
</geometry>
</collision>
<xacro:inertial_cylinder mass="0.1" length="0.03" radius="0.03">
<origin xyz="0 0 0" rpy="0 0 0"/>
</xacro:inertial_cylinder>
</link>
<gazebo reference="right_wheel">
<material>Gazebo/Black</material>
</gazebo>
<!-- CASTER WHEEL LINK -->
<joint name="caster_wheel_joint" type="fixed">
<parent link="body"/>
<child link="caster_wheel"/>
<origin xyz="0.03 0 0"/>
</joint>
<link name="caster_wheel">
<visual>
<geometry>
<sphere radius="0.03"/>
</geometry>
<material name="black"/>
</visual>
<collision>
<geometry>
<sphere radius="0.03"/>
</geometry>
</collision>
<xacro:inertial_sphere mass="0.1" radius="0.03">
<origin xyz="0 0 0" rpy="0 0 0"/>
</xacro:inertial_sphere>
</link>
<gazebo reference="caster_wheel">
<material>Gazebo/Blue</material>
<mu1 value="0.001"/>
<mu2 value="0.001"/>
</gazebo>
<!-- LiDAR -->
<joint name="laser_joint" type="fixed">
<parent link="body"/>
<child link="laser_frame"/>
<origin xyz="0.1 0 0.075" rpy="0 0 0"/>
</joint>
<link name="laser_frame">
<visual>
<geometry>
<cylinder radius="0.03" length="0.03"/>
</geometry>
<material name="red"/>
</visual>
<collision>
<geometry>
<cylinder radius="0.03" length="0.03"/>
</geometry>
</collision>
</link>
<!-- GAZEBO -->
<xacro:include filename="gazebo.xacro"/>
<xacro:include filename="lidar.xacro"/>
</robot>- 첫 번째 터미널에서 아래 명령을 실행합니다. Gazebo에서 바닥에 3개의 파란색 선이 2D LiDAR가 보낸 laser 신호입니다. 4를 입력했는데 선이 3개만 보이는 이유는 시작 지점과 끝 지점이 겹쳐진 상태이기 때문입니다.
- Gazebo에서 ‘Insert’ 탭을 누르고 ‘Insert model …’을 누른 상태로 5분 정도 기다리면 설치 가능한 모델 목록이 나타납니다.
- model 목록에서 ‘Construction Barrel’, ‘Construction Cone’을 적당하게 배치합니다.
- 이후에 목록에서 ‘with-bot’을 선택 후 마우스 메뉴 버튼을 누른 후 팝업메뉴에서 ‘Delete’를 눌러서 with-bot을 제거합니다.
- 메뉴에서 ‘File’ >> ‘Save world As’를 선택합니다.
- 'src/urdf_tutorial/config' 폴더에 ‘with_robot.world’라는 파일 명으로 저장합니다.
- 첫 번째 터미널에서 ‘CTRL+C’를 눌러서 프로그램을 종료 후 아래 명령을 실행해서 다시 실행합니다.
$ ros2 launch urdf_tutorial lidar.launch.py world:=src/urdf_tutorial/config/with_robot.world- 아래 그림과 같이 저장한 Gazebo가 ‘with_robot.word’를 함께 불러온 것을 확인할 수 있습니다.
- 두 번째 터미널에서 아래 명령을 아래 명령을 실행해서 Rviz를 실행하고 저장된 'src/urdf_tutorial/config/robot.rviz'를 읽어 옵니다.
$ cd ~/Workspace/ros_ws/
$ rviz2- Rviz 창에서 아래 순서대로 실행하면 아래와 같이 2D LiDAR가 추가된 자동차를 확인할 수 있습니다.
- ‘Add’ 버튼을 누르고 ‘LaserScan’을 추가합니다.
- ‘LaserScan’의 ‘Topic’을 ‘/scan’으로 변경하면 LiDAR에서 검출된 정보가 빨간색 점으로 표시됩니다. 매우 작은 점으로 표현되므로 꼼꼼히 잘 확인하면 찾을 수 있을 겁니다.
- 메뉴에서 ‘File’ >> ‘Save Config As’를 선택 후 'src/urdf_tutorial/config/lidar.rviz'에 현재 설정을 저장합니다.
- 세 번째 터미널에서 아래 명령을 실행합니다.
$ cd ~/Workspace/ros_ws/
$ ros2 topic echo /scan- 2D LiDAR에서 보내주는 정보를 확인할 수 있습니다. samples를 4로 설정했기 때문에 ‘ranges’ 정보가 4개가 수신되는 것을 확인할 수 있습니다. 특히 아래 정보 중 ‘angle_increment’ 값은 $2\pi/3$값을 가지는 것을 확인할 수 있습니다.
5. 2D LiDAR samples 늘리고 주행해 보기
- 2D LiDAR의 samples 수를 4에서 360으로 늘려보고 수신되는 정보를 기반으로 키보드로 주행을 해 보는 과정입니다.
- 우선 모든 터미널에서 ‘CTRL+C’를 눌러서 실행 중인 모든 프로그램을 종료합니다.
- ‘src/urdf_tutorial/urdf/lidar.xacro’ 파일에서 ‘laser_frame’의 samples를 360으로 변경합니다.
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<gazebo reference="laser_frame">
<material>Gazebo/Red</material>
<sensor name="laser" type="ray">
<pose>0 0 0 0 0 0</pose>
<visualize>true</visualize>
<update_rate>10</update_rate>
<ray>
<scan>
<horizontal>
<samples>360</samples>
<min_angle>-3.14</min_angle>
<max_angle>3.14</max_angle>
</horizontal>
</scan>
<range>
<min>0.3</min>
<max>12</max>
</range>
</ray>
<plugin name="laser_controller" filename="libgazebo_ros_ray_sensor.so">
<ros>
<argument>~/out:=scan</argument>
</ros>
<output_type>sensor_msgs/LaserScan</output_type>
<frame_name>laser_frame</frame_name>
</plugin>
</sensor>
</gazebo>
</robot>- 첫 번째 터미널에서 아래 명령을 실행합니다.
$ colcon build --symlink-install
$ ros2 launch urdf_tutorial lidar.launch.py world:=src/urdf_tutorial/config/with_robot.world- 아래 그림과 같이 360개의 laser 빔을 이용해서 주변을 탐지하는 것을 확인할 수 있습니다.
- 두 번째 터미널에서 아래 명령을 아래 명령을 실행해서 Rviz를 실행하고 저장된 'src/urdf_tutorial/config/lidar.rviz'를 읽어 옵니다.
$ rviz2- 2D LiDAR가 검출한 정보가 화면에 출력된 것을 확인할 수 있습니다. 이 정보가 자동차가 수신할 수 있는 정보입니다.
- 세 번째 터미널에서 아래 명령을 입력합니다.
$ ros2 run teleop_twist_keyboard teleop_twist_keyboard- 적당한 키를 입력해서 자동차를 이동시켜 봅니다.
- Rviz에서 자동차가 이동하는 것을 확인할 수 있습니다.
- Gezebo에서도 자동차가 이동한 것을 확인할 수 있습니다.
- 네 번째 터미널에서 아래 명령을 실행합니다.
$ cd ~/Workspace/ros_ws/
$ ros2 topic echo /scan- 2D LiDAR에서 보내주는 정보를 확인할 수 있습니다. samples를 360으로 설정했기 때문에 ‘ranges’ 정보가 매우 많아진 것을 확인할 수 있습니다. 특히 아래 정보 중 ‘angle_increment’ 값은 $2\pi/360$값을 가지는 것을 확인할 수 있습니다.
