LiDAR(YDLIDAR G2)를 이용한 실습(turtlesim_node + LaserScan)

YDLIDAR G2를 사용하여 라이다의 전방 60도 범위, 20cm 이하 거리안에 장애물이 검출되면 무한회전하는 turtlesim의 거북이를 정지시키는 실습을 해보았습니다.

 

1. LaserScan 형식의 /ydlidar_node에서 발행하는 /scan 토픽을 받습니다.

2. 전방 60도 범위라는 조건을 수행하기위해 라이다의 resolution에 따른 ranges의 범위 계산하였습니다.

3. 거리 20cm 이하에서 장애물이 검출되도록 거리 정보를 담은 ranges의 요소들에 조건을 달았습니다.

4. 위 두 조건에 부합하면, 거북이를 정지하도록 하였습니다.

 

결과

Node Graph

 

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

#! /usr/bin/env python

import rospy
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan

class LidarDetect:
    def __init__(self):
        self.detectstop = Twist()
        self.pub_turtlestop = rospy.Publisher("/turtle1/cmd_vel", Twist, queue_size = 5)
        self.sub_laser = rospy.Subscriber("/scan", LaserScan, self.callback)
        
    def callback(self, data):
        for i in data.ranges[214:300]:
            if i <= 0.2:
                self.detectstop.angular.z = 0.0
                self.detectstop.linear.x = 0.0
                self.pub_turtlestop.publish(self.detectstop)
                print("Turtle stop!!")
            
            else:
                self.detectstop.linear.x = 3.0
                self.detectstop.angular.z = 1.57
                self.pub_turtlestop.publish(self.detectstop)
                        
def main():
    rospy.init_node("lidartest")
    sub = LidarDetect()
    rospy.spin()
    
if __name__ == "__main__":
    main()