시나리오
-
퇴근 후, 텅빈 실내를 배경으로 로봇이 침입자를 탐지하면, 경고음과 함께 자신의 위치정보와 침입자 수와 같은 정보들을 관리페이지에 보내준다.
-
로봇은 정해진 루트를 따라 반복순찰한다.
터틀봇, 개발환경 구축
turtlebot3는 라즈베이파이와 아두이노로 동작하는 깡통 로봇이였다. 하루동안 조립하였다. 그리고 라즈베리파이에 리눅스를 설치하고, ROS(로봇 개발플랫폼)를 설치하고,아두이노와 네트워크를 세팅하였다. 시작부터 난관이였다. 라즈베리파이에 알맞은 리눅스이미지 파일을 찾아야했고 버전도 다양했다. 또한, 리눅스 버전에 따른 ROS버전도 달라졌다. 당시, 구선생이라는 유튜버님의 영상으로 개발환경구축에 많은 도움 받았다. 로봇을 움직이기 위해, remotePC에도 리눅스를 설치하여야 했고, 개인 컴퓨터가 없었기에 외장ssd를 구매하여 그곳에 리눅스를 설치하였다. 물론 쉽지않았다. 설치 직후 계속 컴퓨터가 멈췄고, 해상도는 맞지 않았다. 와이파이부터 모든걸 다 세팅해 주어야했다. 멈춤현상은 알맞은 그래픽드라이버를 따로 설치하여 해결하였다. 개발환경을 구축하면서 리눅스 명령어들의 익숙해지고 꽤 재밌었다.
구현
환경구축 이후, 터틀봇에서 제공해주는 패키지 코드를 분석해 가며 공부하고, 많은 오픈소스들과 기존 코드들을 이용하여 위의 시나리오를 구현하고자 하였다.
먼저, SLAM으로 탐색하여 맵을 만들었다.
SLAM
slam기법 중 gmapping 기법을 사용하였다.
이는 openslam에 공개된 slam의 한 종류로 ros에서 패키지로 제공 되어 이를 사용하였다.
표윤석 교수님의 강의를 보며 slam_gmapping이 어떻게 맵파일을 만들어주는지 정리하였다.
실직적으로 원하는 값은 로봇의 좌표,바퀴의 위치가 아닌 센서의 위치이다. 위치값과 라이다센서값들을 gampping에 보내주면 map파일이 생성된다.
센서의 위치를 구하는법이 궁금하다면 tf를 따로 공부해보면 보면 알수있다.
Navigation
네이게이션 알고리즘으로는 DWA기법응 사용했고 터틀봇패키지에서 제공해준다.
DWA알고리즘은 속도관점으로 최적의길을 찾아가는 알고리즘이다.
(x,y,z)값으로 이동해라! 가 아닌, 현재 최적루트는 병진속도: x, 회전속도: y 입니다. 라고 생각하면 감이 잡히기 시작한다.
표윤석 교수님이 말씀하신 것처럼 신기하고 재밌는 관점의 알고리즘이다.
시간이나 공간에 대한 함수를 시간 또는 공간 주파수 성분으로 분해하는 변환인 푸리에 변환과 느낌이 비슷하다.
amcl
이 로봇은 자신의 위치를 바로 파악하지 못한다. 이 로봇에는 GPS도, 카메라도 없다.
조금씩 움직이면서, 라이다 센서의 값과 맵파일을 매칭시켜 자신의 위치를 유추한다.
처음에는 수 많은 초록색 화살표들이 퍼져있다. 이 화살표는 로봇이 존재할 수 있는 확률과 방향을 의미한다. remotePC에서 로봇의 초기위치를 잡아주고, 움직여주는 방식으로 퍼져있는 화살표들을 실제위치에 근접하는 곳으로 수렴시킬 수 있다. 이 화살표들이 한곳으로 모이는 것으로 로봇의 자신의 위치를 확신할 수 있다.
follow_waypoints
이후, 오픈소스 패키지인 follow_waypoints 와 속도 명령을 주는 Twist 메세지를 이용하여 반복순찰코드를 작성하였다.
위의 오픈패키지는 순찰경로를 csv파일로 저장하고 불러오게 도와주며, 밑에 코드를 통해 로봇이 경로를 순회할 수 있도록 해준다. schedule을 통해 원하는 시간에 맞춰 출발하고, 반복시킬 수도 있다.
밑에 코드를 실행하면, 제자리를 3바퀴정도 돈다.
제자리회전을 하는 이유는 amcl알고리즘의 particle filter를 모아주기 위함이다.
쉽게말해, 순찰 시작 후 자신의 위치를 인지하는 과정이라고 생각할 수 있다.
import rospy
from geometry_msgs.msg import Twist
from std_msgs.msg import Empty
import sys, select, os
import time
import schedule
if os.name == 'nt':
import msvcrt
else:
import tty, termios
e = """
Communications Failed
"""
def start_journey():
pub2 = rospy.Publisher('start_journey', Empty, queue_size=10)
empty = Empty()
pub2.publish(empty)
if __name__=="__main__":
if os.name != 'nt':
settings = termios.tcgetattr(sys.stdin)
rospy.init_node('turtlebot3_patrol')
pub = rospy.Publisher('cmd_vel', Twist, queue_size=10)
pub2 = rospy.Publisher('start_journey', Empty, queue_size=10)
turtlebot3_model = rospy.get_param("model", "burger")
max_time = time.time() + (20)
try:
while(1):
twist = Twist()
twist.linear.x = 0.0; twist.linear.y = 0.0; twist.linear.z = 0.0
twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = 1.0
pub.publish(twist)
if time.time() > max_time :
break
time.sleep(1)
except:
print(e)
finally:
twist = Twist()
twist.linear.x = 0.0; twist.linear.y = 0.0; twist.linear.z = 0.0
twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = 0.0
pub.publish(twist)
start_journey()
#schedule.every().hour.at(":16").do(start_journey)
#while(1):
#schedule.run_pending()
#time.sleep(1)
if os.name != 'nt':
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)하지만 모든일이 쉽게 풀리지 않듯, 로봇은 버벅이고, 멈추며, 시간은 부족하였다.
침입자 탐지까지 구현해야했고, 처음 접하는 플랫폼과 소프트웨어뿐만이 아닌 하드웨어에서의 문제점등 골치덩어리였다.
우선 큰 문제는 두가지였다.
- 뭐가 문제 일까? 왜? 로봇이 목표에 도달했는데도 머물러있지 답답하게??
- 도착지점을 인지하지 못하는 것인가?
- 왜 정해진 경로대로 가지않고, 돌아가거나 비효율적인 움직임을 보이는거지??
parameter
계속 주행하는 로봇을 관찰하며 생각했고, 코드를 들여다보고, 알고리즘에 사용되는 변수들의 출처를 찾아다녔고, parameter를 발견한다.
이값들을 수정하고, 검색해가며 공부하였다.
-
뭐가 문제 일까? 왜? 로봇이 목표에 도달했는데도 머물러있지 답답하게??
-
도착지점을 인지하지 못하는 것인가?
도착인식 범위를 늘려주었다. -
왜 정해진 경로대로 가지않고, 돌아가거나 비효율적인 움직임을 보이는거지??
경로의존도를 늘려주었다.
또한, parameter로 로봇의 초기위치를 고정시켰다. 이는 자신의 위치를 보다 훨씬 빠르게 인지할 수 있도록 했다.
지금 생각해보면 당연하고, 단순하다. 이렇게 주행기능을 만들고, 침입자 탐지기능을 구현하기 시작했다.
객체탐지
시간이 없었다. 침입자 탐지를 위해 카메라가 내장된 로봇에서 개발을 이어나갔고,
무겁지 않으면서, 학습된 dataset이 필요했다. 그리하여 yolov3-tiny를 이용해 구현하기로 하였다. 라벨링으로 조건을 걸어 사람 이외는 탐지하지 않게하였고, box의 갯수정보(침입자 수), 시간, 배터리, tf를 통해 얻은 맵위에서 로봇의 상대적위치 값들을 정제하여, 팀원이 만든 데이터베이스로 넘겨주었다.
또한, 경적은 탐지시, pygame를 통해 경고음.mp3 파일을 실행시켜주는 것으로 마쳤다.
밑에 코드는 결과물 코드를 백업하고, 내가 막 수정하고, 실험해봤던 코드이다. 경고음기능이 들어있지않고, callback()의 내용을 간단히 수정하고 공개한다.
활용하고싶은 msg파일들을 import하고, 콜백함수를 수정하는 것으로 기능은 달라진다.
밑에 코드의 callback 함수는 간단한 임시 메세지를 구독하고 프린트한다.
#!/usr/bin/env python3
import cv2
import numpy as np
import os
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import PoseWithCovarianceStamped
import subprocess
class MinimalSubscriber(Node):
def __init__(self):
super().__init__('minimal_sub')
self.subscriber= self.create_subscription( String,'hi', self.callback, 10)
#self.subscriber # prevent unused variable warning
def callback(self,msg):
#self.get_logger().info(msg.pose.pose)
#ret, frame = VideoSignal.read()
#frame = cv2.transpose(frame)
print(msg.data)
def main(args=None):
rclpy.init(args=args)
node = MinimalSubscriber()
global VideoSignal
VideoSignal = cv2.VideoCapture(8)
weights_path = '/home/desk/coco/yolov3-tiny.weights'
config_path = '/home/desk/coco/yolov3-tiny.cfg'
net = cv2.dnn.readNetFromDarknet(config_path, weights_path)
classes = []
with open("/home/desk/coco/coco.names", "r") as f:
classes = [line.strip() for line in f.readlines()]
layer_names = net.getLayerNames()
output_layers = [layer_names[i - 1] for i in net.getUnconnectedOutLayers()]
colors = np.random.uniform(0, 255, size=(len(classes), 3))
global ret
global frame
#print('hi')
while rclpy.ok():# True
ret, frame = VideoSignal.read()
frame = cv2.transpose(frame)
#print(ret)
if(ret==False):
print('camera error')
break
h, w, c = frame.shape
blob = cv2.dnn.blobFromImage(frame, 0.00392, (416, 416), (0, 0, 0), True, crop=False)
net.setInput(blob)
outs = net.forward(output_layers)
# 정보를 화면에 표시
class_ids = []
confidences = []
boxes = []
#print('hi')
for out in outs:
#print('hi')
for detection in out:
scores = detection[5:]
class_id = np.argmax(scores)
confidence = scores[class_id]
if confidence > 0.5:
# Object detected
center_x = int(detection[0] * w)
center_y = int(detection[1] * h)
dw = int(detection[2] * w)
dh = int(detection[3] * h)
# Rectangle coordinate
x = int(center_x - dw / 2)
y = int(center_y - dh / 2)
boxes.append([x, y, dw, dh])
confidences.append(float(confidence))
class_ids.append(class_id)
if cv2.waitKey(1) == ord('q'):
break
video.release()#오픈한 캡쳐 객체 해제
cv2.destroyAllWindows()
indexes = cv2.dnn.NMSBoxes(boxes, confidences, 0.45, 0.4)
#print('endfor')
cnt = 0
for i in range(len(boxes)):
#print('s')
if i in indexes:
x, y, w, h = boxes[i]
label = str(classes[class_ids[i]])
score = confidences[i]
if label == 'person':
cnt = cnt + 1
# 경계상자와 클래스 정보 이미지에 입력
cv2.rectangle(frame, (x, y), (x + w, y + h), (0, 0, 255), 5)
cv2.putText(frame, label, (x, y - 20), cv2.FONT_ITALIC, 0.5, (255, 255, 255), 1)
rclpy.spin_once(node,timeout_sec=0.001)
cv2.imshow("YOLOv3", frame)
if(cnt>0):
print('person count : ' + str(cnt))
if cv2.waitKey(1) == ord('q'):#q or esc to get out of process
subprocess.run(["/home/desk/delete_log_file.sh","arguments"],shell=True)#delete log file
node.destroy_node()
rclpy.shutdown()
VideoSignal.release()#오픈한 캡쳐 객체 해제 if not camera cannot be connected
cv2.destroyAllWindows()
break
if cv2.waitKey(100) > 0:
VideoSignal.release()
break
if __name__ == '__main__':
main()
turtlebot3에서 새로운 로봇으로 넘어가 침입자 탐지를 구현하였고 역시 쉽지 않았다. 카메라 넘버가 계속 변경됐고
VideoSignal = cv2.VideoCapture(???)
ROS에서 ROS2로 넘어가서 개발하면서, 변경된 문법들과 방식에 적응해야했다.
코드가 멈추기도 하였다.
튜토리얼을 찾아서 공부하고 적용하면서, 가장 기억에 남는것은
rclpy.spin_once(node,timeout_sec=0.001) 이 메소드이다.
rclpy.spin_once
ROS2 튜토리얼에서는 rclpy.spin(node)사용법을 알려준다.
이 기능은 매개변수에 있는 node를 계속 반복실행 시켜주는 것이다.
위의 메소드를 사용하게되면 카메라가 멈춘다, 즉 진행하던 코드가 멈추고
node를 반복실행하는 무한루프에 빠진다. 그래서 rclpy.spin_once(node)를 사용하였다.
그러나, 문제는 계속 발생했다. 해결하였지만 문제를 발견하는데 오래걸렸다. 당시 4학년인데, 개발을 잠시 멈추고 디버깅 공부를 그제서야 하였다.
내 생각대로라면 node를 한번 반복하고 코드를 이어서 실행해야하는데 계속 카메라가 멈췄다. 코드가 진행되지 않았다. 이유는 개발초에는 위치값을 받을 때, tf를 사용하지않고 PoseWithCovarianceStamped을 사용하였는데 이것은 로봇이 움직이지 않으면, 메세지를 publish하지 않는다. callback()에서 원하던 메세지가 오지않으면 올 때까지 무한대기를 하고있던게 원인이였다. 나는 문제해결을 위해, 다시 메소드의 매개변수들을 알아보았고 timeout이라는 대기시간을 설정하는 매개변수를 찾아 수정하여 해결했다.
결과물
- remotePC 화면
-
주행하는 turtlebot3_burger
-
yolov3-tiny 침입자 탐지
