#!/usr/bin/env python
import rospy, time
from sensor_msgs.msg import Int32MultiArray
from xycar_msgs.msg import xycar_motor # Int32MultiArray, xycar_motor 메시지 사용 준비
ultra_msg = None
motor_msg = xycar_motor() # 초음파센서 거리 정보를 담을 저장공간 준비
def callback(data): # 초음파센서 토픽이 들어오면 실행되는 콜백함수 정의
global ultra_msg
ultra_msg = data.data
def drive_go(): # 자동차 전진
global motor_msg, pub
motor_msg.speed = 5
motor_msg.angle = 0
pub.publish(motor_msg)
def drive_stop(): # 자동차 정지
global motor_msg, pub
motor_msg.speed = 0
motor_msg.angle = 0
pub.publish(motor_msg)
rospy.init_node('ultra_driver')
rospy.Subscriber("xycar_ultrasonic", Int32MultiArray, callback, queue_size = 1)
pub = rospy.Publisher('xycar_motor', xycar_motor, queue_size = 1) # 노드 선언 & 구독과 발행할 토픽 선언
time.sleep(2) # 초음파 센서가 가동할 때까지 잠시 기다림
while not rospy.is_shutdown():
if ultra_msg[2] > 0 and ultra_msg[2] < 10:
drive_stop()
else:
drive_go() # 전방 초음파 센서가 감지한 거리 정보가 0 < 거리 < 10cm 범위에 있으면 정차, 그보다 크면 전진 (거리값 0 : 무한대, 장애물이 없음을 의미)
<launch>
<include file="$(find xycar_motor)/launch/xycar_motor.launch" />
<node name="xycar_ultra" pkg="xycar_ultrasonic" type="xycar_ultrasonic.py" output="screen" />
<node name="ultra_driver" pkg="ultra_drive" type="ultra_gostop.py" output="screen" />
</launch>
