LiDAR(YDLIDAR G2)를 이용한 실습(LaserScan -> PointCloud)

YDLIDAR G2를 사용하여 사전에 rosbag -record로 bag 파일을 저장하였습니다.

이번에 해본 실습은 bag 파일에 저장된 LaserScan 형식의 토픽을 받아, 그 정보들을 활용하여 PointCloud 형식으로 변환하는 연산을 거친 후 퍼블리시되는 토픽을 RViz에서 확인해보는 것입니다. 

PointCloud의 x, y의 좌표는 거리와 각도를 알면 구할 수 때문에, theta, distance값을 받아 numpy의 cos, sin함수로 구하였습니다.

 

결과

 

아래는 작성된 코드입니다. 

#! /usr/bin/env python

import rospy
from sensor_msgs.msg import LaserScan, PointCloud
from geometry_msgs.msg import Point32 
from std_msgs.msg import Float64
import numpy as np

class SubLaserScan:
    def __init__(self):
        self.pub_pointcloud = rospy.Publisher("/pointcloud", PointCloud, queue_size = 5)
        self.sub_laser = rospy.Subscriber("/scan", LaserScan, self.callback)

    def calc_axis_xy(self, _theta, _distance, _min_range, _max_range):
        if _min_range <= _distance <= _max_range:
            x = np.cos(_theta) * _distance
            y = np.sin(_theta) * _distance
            return (x, y)
        else:
            return (0, 0)

    def callback(self, data):
        print(data.ranges)
        
        min_angle = data.angle_min
        min_range = data.range_min
        max_range = data.range_max
        angle_increment = data.angle_increment
        PC_data = PointCloud()
        PC_data.header = data.header
        
        for i in range(len(data.ranges)):
            x, y = self.calc_axis_xy(min_angle + angle_increment * i, data.ranges[i], min_range, max_range)
            if [x, y] != [0,0]:
                PC_data.points.append(Point32(x,y,1))
        self.pub_pointcloud.publish(PC_data)
        
        
def main():
    rospy.init_node("lidartest")
    lidar_test = SubLaserScan()
    rospy.spin()
    
if __name__ == "__main__":
    main()