_ROS1_Lidar

이번에는 Lidar센서를 활용하여 rviz에 시각화를 하려고 한다.

이번에는 이전에 사용했던 jackal gazebo 시뮬레이션으로 라이다 데이터를 만들어 사용할 것이다.

사용하는 라이다의 스펙에 맞춰서 설정하여 사용하였다.

gazebo jackal simulation

0.1m ~ 12m, 그리고 360도 전방위 스캔을 할 수 있는 시뮬레이션이다.

/front/scan

/front/scan으로 시뮬레이션 상의 topic을 받고 있으며 해당 데이터를 rviz에 실행을 해보려고 한다.

rviz에 시각하기 위해 다음과 같이 장애물을 놓고 시작하였다.

시각화 프로그램인 rviz를 실행하고 By topic의 laserscan을 추가하면 다음과 같이 붉은 점을 볼 수 있다.

Lidar 데이터 불러오기

결과

하지만, 데이터가 잘 보이지 않으므로 size(m)을 잘 조정한다면 데이터를 크게 볼 수 있다.

size를 0.01에서 0.05로 지정

결과

---

rviz기반 lidar 시각화

이번에는 rviz에 원뿔 형태의 데이터 시각화 프로그램을 작성해보려고 한다.

추후 그동안 실습했던 파일들을 합쳐야 하기에 이번에는 제공하는 rosbag 파일을 활용하려고 한다.

/scan 데이터를 받고 상하좌우 4방향의 /scan1, / scan2, / scan3, / scan4 토픽을 발행하려고 한다.

lidar_range.py를 만들어서 적용하였다.

- lidar_range.py

#!/usr/bin/env python

import serial, time, rospy
from sensor_msgs.msg import Range
from std_msgs.msg import Header

rospy.init_node('lidar_range')

pub1 = rospy.Publisher('scan1', Range, queue_size=1)
pub2 = rospy.Publisher('scan2', Range, queue_size=1)
pub3 = rospy.Publisher('scan3', Range, queue_size=1)
pub4 = rospy.Publisher('scan4', Range, queue_size=1)

msg = Range()
h = Header()
h.frame_id = "sensorXY"
msg.header = h
msg.radiation_type = Range().ULTRASOUND
msg.min_range = 0.02
msg.max_range = 2.0
msg.field_of_view = (30.0/180.0) * 3.14

while not rospy.is_shutdown():
    msg.header.stamp = rospy.Time.now()

    msg.range = 0.4
    pub1.publish(msg)

    msg.range = 0.8
    pub1.publish(msg)

    msg.range = 1.2
    pub1.publish(msg)

    msg.range = 1.6
    pub1.publish(msg)

    time.sleep(0.2)

다음 launch 파일을 실행하면 다음과 같은 결과를 볼 수 있다.

$ roslaunch rviz_lidar lidar_range.launch

---

이제 이렇게 시각화 된 코드를 사방향으로 데이터를 볼 수 있도록 코드를 작성할 것이다.

추가로 작성된 파일은 다음과 같다.

• lidar_urdf.py
• lidar_urdf.launch
• lidar_urdf.urdf

- lidar_urdf.py

#!/usr/bin/env python

import serial, time, rospy
from sensor_msgs.msg import LaserScan
from sensor_msgs.msg import Range
from std_msgs.msg import Header

lidar_points = None

def lidar_callback(data):
    global lidar_points
    lidar_points = data.ranges

rospy.init_node('lidar')
rospy.Subscriber("/scan", LaserScan, lidar_callback, queue_size=1)

pub1 = rospy.Publisher('scan1', Range, queue_size=1)
pub2 = rospy.Publisher('scan2', Range, queue_size=1)
pub3 = rospy.Publisher('scan3', Range, queue_size=1)
pub4 = rospy.Publisher('scan4', Range, queue_size=1)

msg = Range()
h = Header()

msg.radiation_type = Range().ULTRASOUND
msg.min_range = 0.02
msg.max_range = 2.0
msg.field_of_view = (30.0/180.0)*3.14

while not rospy.is_shutdown():
    if lidar_points == None:
        continue

    h.frame_id = "front"
    msg.header = h
    msg.range = lidar_points[90]
    pub1.publish(msg)

    h.frame_id = "right"
    msg.header = h
    msg.range = lidar_points[180]
    pub1.publish(msg)

    h.frame_id = "back"
    msg.header = h
    msg.range = lidar_points[270]
    pub1.publish(msg)

    h.frame_id = "left"
    msg.header = h
    msg.range = lidar_points[0]
    pub1.publish(msg)

    time.sleep(0.5)

 

- lidar_urdf.launch

<launch>
    <param name="robot_description"
            textfile="$(find rviz_lidar)/urdf/lidar_urdf.urdf"/>
    <param name="use_gui" value="true"/>
    <node name="rviz_visualizer" pkg="rviz" type="rviz" required="true"
            args="-d $(find rviz_lidar)/rviz/lidar_urdf.rviz"/>
    <node name="robot_state_publisher" pkg="robot_state_publisher"
            type="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" pkg="rviz_lidar" type="lidar_urdf.py" output="screen"/>
</launch>

 

lidar_urdf.launch를 실행하고 lidar센서로 감지가 되면, 4방향으로 원뿔모양의 라이다 데이터가 나가는 것을 볼 수 있다.