#!/usr/bin/env python
import rospy, time
from sensor_msgs.msg import LaserScan
from xycar_msgs.msg import xycar_motor # LaserScan, xycar_motor 메시지 사용 준비
motor_msg = xycar_motor() # 라이다 거리정보를 담을 저장공간 준비
distance = []
def callback(data): # 라이다 토픽이 들어오면 실행되는 콜백함수 정의
global distance, motor_msg
distance = data.ranges
def drive_go(): # 자동차 전진
global motor_msg
motor_msg.speed = 5
motor_msg.angle = 0
pub.publish(motor_msg)
def drive_stop(): # 자동차 정지
global motor_msg
motor_msg.speed = 0
motor_msg.angle = 0
pub.publish(motor_msg)
rospy.init_node('lidar_driver')
rospy.Subscriber('/scan', LaserScan, callback, queue_size = 1)
pub = rospy.Publisher('xycar_motor', xycar_motor, queue_size = 1) # 노드 선언& 구독과 발행할 토픽 선언
time.sleep(3) # Laser가 가동할 때까지 잠시 기다림
while not rospy.is_shutdown():
ok = 0
for degree in range(60, 120): # 전방이 90도 이므로 60-120도 범위만 스캔
if distance[degree] <= 0.3:
ok += 1
if ok > 3: # 범위 안에 3개가 30cm 이내면 스톱 아니면 전진
drive_stop()
break
if ok <= 3:
drive_go()
<launch>
<include file="$(find xycar_motor)/launch/xycar_motor.launch" />
<include file="$(find xycar_lidar)/launch/xycar_noviewer.launch" />
<node name="lidar_driver" pkg="lidar_drive" type="lidar_gostop.py" output="screen" />
</launch>
