[ROS 튜토리얼] 라이다 센서를 위한 ROS 노드와 토픽

학습 내용

---

라이다 센서를 위한 ROS 노드와 토픽

 

라이다 센서를 위한 ROS 노드와 토픽에 대해 학습해 보자. 우선 자이카 실습에서 사용하게 될 1 채널 2D 라이다의 스펙은 다음과 같다.

• 0.504 º Degree
• 5000 Sampling
• 12cm ~ 12 meter Range

 

라이다를 위한 ROS Package는 xycar_lidar이고 디렉터리 위치는 다음과 같다.

xycar_ws
   ├─ build
   ├─ devel
   └─ src
       └─ xycar_device
           ├─ ...
           ├─ ...
           └─ xycar_lidar

 

라이다의 관련 노드와 토픽

/xycar_lidar 노드에서 발행하는 /scan 토픽을 이용한다. /scan 토픽은 헤더와 각도, 거리 등의 정보가 담겨있고 rosmsg 명령어를 통해 확인해 보면 다음과 같다.

 

/scan 토픽 내용

여기서 angle_min과 angle_max는 각각 최소 각도와 최대 각도를 나타내며 0과 360도를 주면 모든 방향에 대해 동작한다. ranges는 거리정보를 담고 있는 배열로 360도를 측정하였으면 1도씩 360개의 거리 정보가 담겨있다. intensities는 측정된 물체의 강도 혹은 특성에 대한 정보를 담고 있으며 이를 활용하 아스팔트와 차선을 구분할 수도 있다.

 

 

라이다 데이터 출력 프로그램

라이다로부터 주변 물체까지의 거리값을 받아 출력하는 예제를 학습해 보자. 간단히 토픽을 보내고 구독하여 값을 출력해 본다.

 

실습을 위해 my_lidar 패키지를 생성하고 std_msgs와 roscpp를 의존한다. 이후 패키지 내에 lidar_scan.cpp와 lidar_scan.launch 파일을 작성하면 된다.

xycar_ws
   ├─ build
   ├─ devel
   └─ src
       └─ my_lidar (new!)
           ├─ CMakeLists.txt
           ├─ launch
           │     └─ lidar_scan.launch (new!)
           └─ src
               └─ lidar_scan.cpp (new!)

 

lidar_scan.cpp

#include "ros/ros.h"
#include "sensor_msgs/LaserScan.h"
#include <sstream>

std::vector<float> lidar_points;

void lidarCallback(const sensor_msgs::LaserScan::ConstPtr& data)
{
    lidar_points = data->ranges;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "my_lidar");
    ros::NodeHandle n;
    ros::Subscriber sub = n.subscribe("/scan", 1, lidarCallback);

    while (ros::ok())
    {
        if (lidar_points.empty())
        {
            ros::spinOnce();
            continue;
        }

        std::stringstream rtn;
        for (int i = 0; i < 12; ++i)
        {
            rtn << std::fixed << std::setprecision(2) << lidar_points[i * 30] << ", ";
        }

        ROS_INFO_STREAM(rtn.str().substr(0, rtn.str().length() - 2));

        ros::Duration(1.0).sleep();
        ros::spinOnce();
    }

    return 0;
}

 

lidar_scan.launch

<launch>
    <include file="$(find xycar_lidar)/launch/lidar_noviewer.launch" />
    <node name="my_lidar" pkg="my_lidar" type="lidar_scan" output="screen" />
</launch>

 

roslaunch 명령을 통해 확인할 수 있는 결과는 다음과 같다. 현재 자이카에서 동작을 확인할 수 없어 강의의 결과를 참고하였다.

/scan 토픽 실습 결과

 

결과를 확인해 보면 우선 코드에 작성한 대로 360도 주변을 30도씩 나누어 12개 값만 표시하고 있다. 처음의 inf는 라이더가 측정하였을 때 물체와의 거리가 너무 가깝거나 혹은 너무 멀어 돌아오는 신호를 감지하지 못했음을 의미한다. 이후 2.34미터, 1.41미터, 1.19미터,... 등의 거리 정보를 나타낸다. 또한 측정 중 너무 높은 값이나 inf와 같은 예외가 발생할 수 있으므로 이에 대해 필터링 처리를 해야 할 필요가 있다.

 

 

RVIZ에서 라이다 센싱 데이터 시각화

이번에는 RVIZ에서 라이다 센싱 데이터를 시각화하는 예제를 학습해 보자. 이 예제 역시 실제 라이다 장치가 있을 때 결과 확인이 가능하다. 마찬가지로 rviz_lidar 패키지를 생성하고 roscpp와 tf, geometry_msgs, urdf, rivz, xacro를 의존한다. 이후 launch 폴더와 rviz 폴더를 생성하여 각각 필요한 파일을 작성한다. 

 

실제 라이다 장치를 대신하여 /scan 토픽을 발행하는 프로그램을 이용한다. ROS에서 제공하는 "rosbag"은 라이다에서 발행하는 /scan 토픽을 저장해 놓은 파일을 사용하여 그 당시의 시간 간격에 맞추어 /scan 토픽을 발행하는 역할을 한다. 이를 위해 launch 디렉터리 아래에 lidar_3d_rosbag.launch 파일을 만들어야 한다.

 

강의의 실습 결과를 참고하면 다음과 같다. rosbag을 사용하여 src 폴더 안의 bag 파일을 불러와 화면에 출력하고 있는 것을 알 수 있다. 이때 RVIZ 창에서 Add -> LaserScan을 추가하고 토픽을 /scan으로 설정해 주면 된다.

 

 

RVIZ기반 라이다 뷰어 제작

rosbag을 활용하면 토픽을 구독하여 파일로 저장하거나, 파일에서 토픽을 꺼내 발행할 수 있다. 터미널에서 아래의 명령어를 통해 토픽을 저장하고 발행할 수 있다. file_name에는 저장될 파일 이름을 적고 topic_name은 받아오려는 토픽의 이름을 적어주면 된다. 토픽은 다중으로 받아서 저장할 수 있다.

$ rosbag recode -O [file_name] [topic_name1] [topic_name2] [topic_name3]
$ rosbag play [file_name].bag

 

launch 파일에서는 아래와 같이 노드를 선언하면 된다.

<launch>
	<node name="rosbag_play" pkg="rosbag" type="play" output="screen
    	required="true" args="$(find rviz_lidar)/src/lidar_topic.bag"/>
</launch>

 

 

관련된 예제를 통해 rosbag 활용 방법에 대해 자세히 익혀보자. 실제 라이다 장치가 없이 동작할 수 있도록 이번에는 라이다 데이터를 Range 데이터로 바꿔서 RVIZ로 동작을 확인해본다. 먼저 rosbag으로 lidar_topic.bag 파일에 저장된 라이다 토픽을 하나씩 발행한다. 발행된 scan 토픽에서 장애물까지의 거리 정보를 꺼내어 scan1, scan2, scan3, scan4 4개의 토픽에 각각 담아서 발행한다. RVIZ에서는 Range 형식으로 거리 정보를 시각화하여 보여준다. 과제의 전체 흐름도는 아래와 같다.

 

rosbag 토픽을 Range 형식으로 변환하는 RVIZ 실습 예제의 전체 흐름도

 

실습을 위해 rviz_lidar 패키지에 토픽의 거리 정보를 이용하여 새로운 토픽을 발행하는 lidar_urdf.cpp 소스 파일과 lidar_urdf.launch 파일, lidar_urdf.urdf, lidar_urdf.rviz 파일을 새로 작성한다. 

 

lidar_urdf.urdf

<?xml version="1.0" ?>
<robot name="xycar" xmlns:xacro="http://www.ros.org/wiki/xacro">

 <link name="base_link"/>
  <link name="baseplate">
    <visual>
      <material name="red"/>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.2 0.2 0.07"/>
      </geometry>
    </visual>
  </link>

  <joint name="base_link_to_baseplate" type="fixed">
    <parent link="base_link"/>
    <child link="baseplate"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
  </joint>

  <link name="front"/>

  <joint name="baseplate_to_front" type="fixed">
    <parent link="baseplate"/>
    <child link="front"/>
    <origin rpy="0 0 0" xyz="0.1 0 0"/>
  </joint>

  <link name="back"/>

  <joint name="baseplate_to_back" type="fixed">
    <parent link="baseplate"/>
    <child link="back"/>
    <origin rpy="0 0 3.14" xyz="-0.1 0 0"/>
  </joint>

  <link name="left"/>

  <joint name="baseplate_to_left" type="fixed">
    <parent link="baseplate"/>
    <child link="left"/>
    <origin rpy="0 0 1.57" xyz="0 0.1 0"/>
  </joint>

  <link name="right"/>

  <joint name="baseplate_to_right" type="fixed">
    <parent link="baseplate"/>
    <child link="right"/>
    <origin rpy="0 0 -1.57" xyz="0 -0.1 0"/>
  </joint>
</robot>

 

lidar_urdf.launch

<launch>
    <param name="robot_description" textfile="$(find rviz_lidar)/urdf/lidar_urdf.urdf"/>
    <param name="use_gui" value="true"/>

    <!-- rviz display -->
    <node name="rviz_visualizer" pkg="rviz" type="rviz" required="true" 
          args="-d $(find rviz_lidar)/rviz/lidar_range.rviz"/>
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>

    <node name="rosbag_play" pkg="rosbag" type="play" output="screen"
          required="true" args="$(find rviz_lidar)/src/lidar_topic.bag"/>

    <node name="lidar_urdf" pkg="rviz_lidar" type="lidar_urdf"/>
</launch>

 

lidar_urdf.cpp

#include <ros/ros.h>
#include <sensor_msgs/Range.h>
#include <sensor_msgs/LaserScan.h>

sensor_msgs::LaserScan::ConstPtr lidar_points;

void lidarCallback(const sensor_msgs::LaserScan::ConstPtr& data) {
    lidar_points = data;
}

int main(int argc, char **argv) {
    ros::init(argc, argv, "lidar_urdf");
    ros::NodeHandle nh;

    ros::Subscriber lidarSub = nh.subscribe("/scan", 1, lidarCallback);

    ros::Publisher pub1 = nh.advertise<sensor_msgs::Range>("scan1", 1);
    ros::Publisher pub2 = nh.advertise<sensor_msgs::Range>("scan2", 1);
    ros::Publisher pub3 = nh.advertise<sensor_msgs::Range>("scan3", 1);
    ros::Publisher pub4 = nh.advertise<sensor_msgs::Range>("scan4", 1);

    sensor_msgs::Range msg;
    msg.radiation_type = sensor_msgs::Range::ULTRASOUND;
    msg.min_range = 0.02;                   // 2cm
    msg.max_range = 2.0;                    // 2m
    msg.field_of_view = (30.0 / 180.0) * 3.14;  // radian expression 1/6 pi

    ros::Rate rate(2.0);  // 0.5Hz

    while (ros::ok()) {
        ros::spinOnce();

        if (lidar_points == nullptr) {
            continue;
        }

        msg.header = lidar_points->header;

        msg.header.frame_id = "front";
        msg.range = lidar_points->ranges[90];
        pub1.publish(msg);

        msg.header.frame_id = "right";
        msg.range = lidar_points->ranges[180];
        pub2.publish(msg);

        msg.header.frame_id = "back";
        msg.range = lidar_points->ranges[270];
        pub3.publish(msg);

        msg.header.frame_id = "left";
        msg.range = lidar_points->ranges[0];
        pub4.publish(msg);

        rate.sleep();
    }

    return 0;
}

 

rviz 파일을 만들기 위해서 RVIZ를 실행시키고 Fixed Frame의 값을 base_link로 설정한 뒤 Add -> RobotModel 추가, Add -> By topic -> scan1, 2, 3, 4를 추가한다. Range에 생긴 /scan1, /scan2 등의 색상을 변경해 주고 save 하면 rviz 파일이 생성된다.

 

실습 결과는 다음과 같다. 라이다 정보가 들어있는 bag 파일에서 Range로 변환하여 이를 화면에 출력하고 있는 것을 확인할 수 있다. 또한 rqt_graph를 확인하여 노드와 토픽의 구조를 확인할 수 있다.

 

 

rqt_graph 결과

 

 

 

결론

이번에는 라이다 센서를 위한 ROS 노드와 토픽에 대해 학습하였다. 라이다 센서 토픽이 포함하는 정보와 이를 발행하여 값을 출력하고 시각화하는 방법에 대해 학습하였다. 또한 예제를 통해 라이다 센서 토픽을 저장하고 발행하는 rosbag의 활용 방법에 대해 배우고 실제 라이더 없이 라이다 센서 토픽이 저장된 bag 파일에서 토픽을 발행하여 Range으로 변환 후 RVIZ에서 시각화하여 확인해 볼 수 있었다. 이를 통해 ROS에서 라이다 센서 데이터를 이해하고 이를 받아와 활용하는 방법에 대해 알 수 있었다.