라이다에서 전방 물체를 판단하는 방법

실제로봇의 경우 YDLidar를 사용하는데

이번 프로젝트에서 Lidar의 값을 구독하여 전처리하고, 다시 발행하는 과정에서 문제가 생겼다.

오류의 내용은 QoS (데이터 통신) 문제로 발행하는 부분과 구독하는 부분이 서로 합이 맞지 않는다는 이야기였다.

우리가 짠 코드는

 

라이다의 전방영역의 가장 작은 값이 0.35 이내라면 장애물이 판별되었다는 Topic을 발행하라는 코드이다.

import rclpy as rp
from rclpy.node import Node
from sensor_msgs.msg import LaserScan
from minibot_msgs.msg import IsObstacle

class ObstacleDetect(Node):
    
    # /scan topic을 구독하고 장애물이 가까이 있는지 파악해서 알려줌
    def __init__(self):
        super().__init__("obstacle_detector")
        self.sub_scan = self.create_subscription(LaserScan, "/scan", self.callback_scan, 10)
        self.timer_period = 0.5
        self.publisher = self.create_publisher(IsObstacle, "/obstacle_detect", 10)
        self.timer = self.create_timer(self.timer_period, self.timer_callback)
        self.is_osbstacle = IsObstacle()
        
    def callback_scan(self, msg):
        min_data = min(msg.ranges[170:210])  # ladar의 앞 부분 인식
        self.is_osbstacle.min_distance = min_data
        
        if min_data < 0.35:                    # 0.2m 이하이면 주행 멈추라고 신호 줌
            self.is_osbstacle.check_obstacle = True
        else:
            self.is_osbstacle.check_obstacle = False

    def timer_callback(self):
        self.publisher.publish(self.is_osbstacle)
        
def main(args=None):
    rp.init(args=args)
    
    obstacle_detector = ObstacleDetect()
    rp.spin(obstacle_detector)
    
    obstacle_detector.destroy_node()
    rp.shutdown()
    
if __name__ == "__main__":
    main()

그러나 여기서 앞서 설명한 오류가 발생했다. 이를 해결하기 위해서는 우선 QoS를 고쳐주어야 했다.

2. ROS2의 특징 중 하나인 DDS (feat ChatGPT) (tistory.com)

2. ROS2의 특징 중 하나인 DDS (feat ChatGPT)

여기에 정리가 되어 있지만 간략하게 말하면 QoS는 메시지간의 데이터를 주고받는 데이터통신과정을 어떤식으로 진행할지 정의하는 부분이다.

 

우선 그러면 YDiLiDAR 이 친구는 어떻게 설정이 되어 있는데? 패키지를 뜯어보자.

byeongkyu/ydlidar_ros2_driver: ydlidar driver package under ros2 (github.com)

GitHub - byeongkyu/ydlidar_ros2_driver: ydlidar driver package under ros2

 

여기서 src로 들어가서 Node 파일에 들어간 후에 QoS를 검색한다.

SensorData 타입이다.

이 타입의 경우 센서처럼 데이터가 유실되더라도, 새로운 지금당장의 정보가 중요한 상황에서 사용하는 데이터통신 방식이다.

이를 이제 표윤석 박사님의 책을 참고하여 고쳐보자.

다음의 값으로 코드를 수정하면 무리없이 데이터를 받을 수 있다.

import rclpy as rp
from rclpy.node import Node
from sensor_msgs.msg import LaserScan
from rclpy.qos import QoSDurabilityPolicy
from rclpy.qos import QoSHistoryPolicy
from rclpy.qos import QoSProfile
from rclpy.qos import QoSReliabilityPolicy
from minibot_msgs.msg import IsObstacle
import numpy as np

class ObstacleDetect(Node):
    
    # /scan topic을 구독하고 장애물이 가까이 있는지 파악해서 알려줌
    def __init__(self):
        super().__init__("obstacle_detector")
        qos_profile = QoSProfile(
            history=QoSHistoryPolicy.KEEP_LAST,
            depth=10,
            reliability=QoSReliabilityPolicy.BEST_EFFORT,
        )
        self.sub_scan = self.create_subscription(LaserScan, "/scan", self.callback_scan, qos_profile)
        self.timer_period = 0.5
        self.publisher = self.create_publisher(IsObstacle, "/obstacle_detect", 10)
        self.timer = self.create_timer(self.timer_period, self.timer_callback)
        self.is_osbstacle = IsObstacle()

 

 

또한 후반부에 최소값을 받아오게 하였는데, 실제의 경우 360도의 모든 값을 다 가져오는 것도 아니였다.  누락이 되는 경우 0.0m로 판단을 했는데 이런 경우는 라이다를 손으로 감싸지 않는 이상 발생하지 않으니 현실적으로 불가능하다고 볼 수 있다.

그러니 min값을 탐지하는 알고리즘 대신에 좁은 전방의 영역에서 0.0m값을 제거한 후에 average를 취했다.

def callback_scan(self, msg):
        min_data =  msg.ranges[110:150]  # ladar의 앞 부분 인식
        try:
            min_data = np.average(min_data.remove(0.0))
        except:
            min_data = np.average(min_data)
        self.is_osbstacle.min_distance = float(min_data)
        
        if min_data < 0.3:                    # 0.35m 이하이면 주행 멈추라고 신호 줌
            self.is_osbstacle.check_obstacle = True
        else:
            self.is_osbstacle.check_obstacle = False

    def timer_callback(self):
        self.publisher.publish(self.is_osbstacle)

def main(args=None):
    rp.init(args=args)
    
    obstacle_detector = ObstacleDetect()
    rp.spin(obstacle_detector)
    
    obstacle_detector.destroy_node()
    rp.shutdown()
    
if __name__ == "__main__":
    main()

 

그래서 문제없이 작동을 할 수 있게되었다.