Сегодня мы рассмотрим, как обнаружить с помощью камеры Raspberry PI красный шарик, и как начать наводить на него наш дрон.
В предыдущих статьях мы подробно рассматривали запуск автономного виртуального и реального дрона. Настало время нашему дрону обрести реальную цель.
Целью будет, как мы упоминали, красный шарик.
Подготовка Raspberry
Программу для фильтра мы будем писать на Desktop – версии Raspbian. Поскольку для настройки фильтра мы заходим одновременно крутить настройки и видеть результат наших накруток.
На отдельную флешку поставим Desktop-ный Raspbian.
Для работы с десктопным Raspbian нужны отдельные монитор+клавиатура+мышь, подключенные к Raspberry, что не очень удобно. Но можно работать с десктопом headless-Raspberry и по сети, через VNC.
Для этого нужно:
- Включить VNC в raspi-config
- Настроить точку доступа Wifi на raspberry. Фокус с wpa_supplicant на десктопной версии не проходит, инструкция по настройке wifi точки на Raspberry лежит здесь.
- В качестве клиента можно использовать realvnc.
Для разработки фильтра на Питоне — поставим OpenCV:
sudo apt-get install python-opencv
HSV фильтр для видеопотока
Простой способ и быстрый способ выделить цветной объект на картинке – перевести картинку в цветовое пространство HSV, а затем – отфильтровать по нужному диапазону Тона (Hue), Насыщенности (Saturation) и Яркости (value), с помощью cv2.inRange. За основу я брал вот этот пример.
Особенность нашей задачи заключается в том, что шарик у нас – красный, а если мы внимательно посмотрим на шкалу Тона (HUE), то увидим, что красный цвет находится по её краям:
Поэтому если фильтровать только один кусок диапазона – при изменении освещения шарик может «исчезать» из фильтра, что скажется отрицательно на целеустремлённости нашего дрона. Поэтому кроме H, S и V параметров добавим в наш фильтр флаг инверсии invert – чтобы иметь возможность отфильтровать всё кроме шарика, а затем просто инвертировать фильтр.
#coding=utf-8 import cv2 import numpy as np def nothing(*arg): pass cv2.namedWindow( "result" ) # создаем главное окно cv2.namedWindow( "img" ) # создаем окно cv2.namedWindow( "settings" ) # создаем окно настроек cap = cv2.VideoCapture(0)#video.create_capture(0) # создаем 6 бегунков для настройки начального и конечного цвета фильтра cv2.createTrackbar('invert', 'settings', 0, 1, nothing) cv2.createTrackbar('h1', 'settings', 0, 255, nothing) cv2.createTrackbar('s1', 'settings', 0, 255, nothing) cv2.createTrackbar('v1', 'settings', 0, 255, nothing) cv2.createTrackbar('h2', 'settings', 255, 255, nothing) cv2.createTrackbar('s2', 'settings', 255, 255, nothing) cv2.createTrackbar('v2', 'settings', 255, 255, nothing) while True: flag, img = cap.read() hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV ) # считываем значения бегунков invert = cv2.getTrackbarPos('invert', 'settings') h1 = cv2.getTrackbarPos('h1', 'settings') s1 = cv2.getTrackbarPos('s1', 'settings') v1 = cv2.getTrackbarPos('v1', 'settings') h2 = cv2.getTrackbarPos('h2', 'settings') s2 = cv2.getTrackbarPos('s2', 'settings') v2 = cv2.getTrackbarPos('v2', 'settings') # формируем начальный и конечный цвет фильтра h_min = np.array((h1, s1, v1), np.uint8) h_max = np.array((h2, s2, v2), np.uint8) # накладываем фильтр на кадр в модели HSV thresh = cv2.inRange(hsv, h_min, h_max) if invert>0: cv2.bitwise_not(thresh,thresh) cv2.imshow('result', thresh) cv2.imshow('img', img) ch = cv2.waitKey(5) if ch == 27: break cap.release() cv2.destroyAllWindows()
Настройка фильтра на шарик выглядит примерно так:
Расчёт положения шарика в пространстве
Получив фильтр, мы можем найти шарик на картинке фильтра с помощью функции выделения контура cv2.findContours.
В нашей программе мы переберём все найденные контуры, выделим те, которые больше 10х10 точек, и выберем из них самый большой – это и будет искомый шарик. Если в камеру попало несколько объектов – для наведения мы используем ближайший, т.е. самый большой.
Полезную информацию фильтра выведем на полученную картинку: нарисуем найденные контуры и обведём самый большой из них, выделим линиями его центр (куда нам лететь), выведем диаметр шарика в точках и посчитаем расстояние до него, угол отклонения по рысканью. Кроме того, в центре экрана я добавил «прицел» + посчитал и вывел частоту кадров – чтобы контролировать скорость работы нашего фильтра.
#coding=utf-8 import cv2 import numpy as np import math if __name__ == '__main__': def nothing(*arg): pass cv2.namedWindow( "result" ) # создаем главное окно cv2.namedWindow( "source" ) # source video cv2.namedWindow( "settings" ) # создаем окно настроек cap = cv2.VideoCapture(0) cap.set(cv2.CAP_PROP_FRAME_WIDTH, 320) cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 240) cap.set(cv2.CAP_PROP_FPS,40) # создаем 6 бегунков для настройки начального и конечного цвета фильтра cv2.createTrackbar('invert', 'settings', 0, 1, nothing) cv2.createTrackbar('h1', 'settings', 0, 255, nothing) cv2.createTrackbar('s1', 'settings', 0, 255, nothing) cv2.createTrackbar('v1', 'settings', 0, 255, nothing) cv2.createTrackbar('h2', 'settings', 255, 255, nothing) cv2.createTrackbar('s2', 'settings', 255, 255, nothing) cv2.createTrackbar('v2', 'settings', 255, 255, nothing) crange = [0,0,0, 0,0,0] #set up initial values cv2.setTrackbarPos('invert','settings',1) cv2.setTrackbarPos('h1','settings',7) cv2.setTrackbarPos('h2','settings',158) cv2.setTrackbarPos('s1','settings',0) cv2.setTrackbarPos('s2','settings',255) cv2.setTrackbarPos('v1','settings',0) cv2.setTrackbarPos('v2','settings',255) color_blue = (255,255,0) color_red = (0,0,255) color_green = (0,255,0) color_white = (255,255,255) color_yellow = (0,255,255) color_black = (0,0,0) real_ballon_r = 0.145 #baloon real radius in meters while True: flag, img = cap.read() hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV ) # считываем значения бегунков invert = cv2.getTrackbarPos('invert', 'settings') h1 = cv2.getTrackbarPos('h1', 'settings') s1 = cv2.getTrackbarPos('s1', 'settings') v1 = cv2.getTrackbarPos('v1', 'settings') h2 = cv2.getTrackbarPos('h2', 'settings') s2 = cv2.getTrackbarPos('s2', 'settings') v2 = cv2.getTrackbarPos('v2', 'settings') # формируем начальный и конечный цвет фильтра h_min = np.array((h1, s1, v1), np.uint8) h_max = np.array((h2, s2, v2), np.uint8) # накладываем фильтр на кадр в модели HSV thresh = cv2.inRange(hsv, h_min, h_max) if invert>0: cv2.bitwise_not(thresh,thresh) #find contours _, contours0, hierarchy = cv2.findContours( thresh.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE) max_area = 0 max_center_x=None max_center_y=None max_baloon_r=None max_yaw_shift=None for cnt in contours0: rect = cv2.minAreaRect(cnt) box = cv2.boxPoints(rect) box = np.int0(box) center = (int(rect[0][0]),int(rect[0][1])) area = int(rect[1][0]*rect[1][1]) if area > 100: baloon_r = int(math.sqrt(area)/2) yaw_shift = int( (center[0] - 320/2) * 130 / 320) # 130 degree camera for 320 pics image cv2.circle(img, center, baloon_r , color_red, 1) if area>max_area: max_area=area max_center_x = center[0] max_center_y = center[1] max_baloon_r = baloon_r max_yaw_shift = yaw_shift #draw main baloon if max_area>0: cv2.circle(img, (max_center_x,max_center_y), max_baloon_r+2 , color_blue, 2) cv2.putText(img, "D=%d" % int(max_baloon_r*2), (10, 20), cv2.FONT_HERSHEY_SIMPLEX, 0.5, color_red, 1) cv2.putText(img, "angle=%d '" % max_yaw_shift, (10, 40), cv2.FONT_HERSHEY_SIMPLEX, 0.5, color_green, 1) max_distance = real_ballon_r/math.tan(max_baloon_r/320.0*(math.pi*160.0/360.0)) #distance to baloon #print max_distance cv2.putText(img, "%f m" % max_distance, (10, 60), cv2.FONT_HERSHEY_SIMPLEX, 0.5, color_blue, 1) cv2.line(img, (max_center_x, 1), (max_center_x, 239) , color_white, 1) cv2.line(img, (1,max_center_y), (319, max_center_y) , color_white, 1) #draw target cv2.line(img, (320/2-10, 240/2-10), (320/2+10, 240/2+10) , color_yellow, 1) cv2.line(img, (320/2+10, 240/2-10), (320/2-10, 240/2+10) , color_yellow, 1) # show results cv2.imshow('source', img) cv2.imshow('result', thresh) ch = cv2.waitKey(5) if ch == 27: break cap.release() cv2.destroyAllWindows()
Программа по картинке с камеры считает расстояние до шарика и углы по рысканью и тангажу, на которые шарик отклоняется от центра картинки камеры. Этой информации нам достаточно для наведения дрона на шарик. Дальше мы займёмся программой наведения.
Пакет ROS, обнаруживающий шарик
Разработанный функционал можно переносить на систему headless-Raspberry с установленным ROS. Мы создадим ROS-пакет baloon, который будет выполнять следующие функции:
- Запускать mavros, обеспечивающий нам связь с полётным контроллером
- Запускать нашу программу обнаружения шарика. В программу мы добавим публикацию координат шарика и отладочных изображений в соответствующие ROS-топики
- Запускать web_video_server – компонент ROS, который позволит нам наблюдать за нашим фильтром извне с помощью браузера
Перед созданием ROS-пакета – добавим в нашу программу-фильтр следующий функционал для работы в среде ROS:
- Чтение параметров камеры. Параметры в ROS задаются в виде yaml-файла, который обычно получают в процессе калибровки камеры. Нас пока интересуют только размеры картинки (ширина и высота), но чтобы следовать концепции ROS мы получим их из yaml-файла.
- Получение заданных параметров фильтра. Подобранные ранее параметры фильтра мы пропишем в файле запуска нашего ROS-пакета.
- Публикацию полученных координат шарика в ROS-топик
/baloon_detector/twist - Публикацию отладочных изображений в ROS-топики
/baloon_detector/image_filterи/baloon_detector/image_result
Можно запускать программу на выполнение с помощью обычного python baloon_pose_cv.py – но существует и более удобный способ – упаковать нужный нам функционал в пакет ROS, и настроить его автозапуск при старте системы.
В сети довольно много уроков по созданию ROS-пакетов, поэтому я ограничусь перечнем нужных шагов.
Создадим каталог рабочего пространства ROS для нашего пакета, и сам пакет ROS:
mkdir -p ~/baloon_ws/src cd ~/baloon_ws/ catkin_make source devel/setup.bash cd src catkin_create_pkg baloon std_msgs rospy
Поместим в каталог ~/baloon_ws/src/baloon/src файлы, необходимые для запуска нашего пакета:
- baloon_pose_cv.py – программа обнаружения шарика
- baloon_cv.launch – launch-файл для запуска нашего ROS пакета
- fe130_320_01.yaml – файл, полученный в результате калибровки камеры. Нас в нём интересуют только размеры картинки, но в дальнейшем данный файл можно использовать для более глубокого анализа изображений, восстановления 3D-сцен по картинке и т.д.
- baloon.service, roscore.env – эти файлы используются при автозапуске ROS пакета
Ниже приводим тексты каждого из файлов:
#!/usr/bin/env python # coding=UTF-8 # zuza baloon seeker import rospy from sensor_msgs.msg import CameraInfo, Image import time import cv2 from cv_bridge import CvBridge, CvBridgeError import numpy as np #import tf.transformations as t from geometry_msgs.msg import TwistStamped, Quaternion, Point, PoseStamped, TransformStamped import copy from std_msgs.msg import ColorRGBA import tf2_ros import math import yaml my_cam_info=None my_fps=0 my_last_fps=0 my_time_fps=time.time() bridge = CvBridge() mtx=None dist=None def raspicam_loop_cv(): global my_fps, my_last_fps, my_time_fps, mtx, dist # initialize the camera and grab a reference to the raw camera capture camera = cv2.VideoCapture(0) camera.set(cv2.CAP_PROP_FRAME_WIDTH, my_cam_info.width) camera.set(cv2.CAP_PROP_FRAME_HEIGHT, my_cam_info.height) camera.set(cv2.CAP_PROP_FPS, rospy.get_param('/baloon_detector/framerate')) # allow the camera to warmup time.sleep(0.1) color_blue = (255,255,0) color_red = (0,0,255) color_green = (0,255,0) color_white = (255,255,255) color_yellow = (0,255,255) color_black = (0,0,0) # считываем значения бегунков invert = rospy.get_param('/baloon_detector/invert') h1 = rospy.get_param('/baloon_detector/h1') s1 = rospy.get_param('/baloon_detector/s1') v1 = rospy.get_param('/baloon_detector/v1') h2 = rospy.get_param('/baloon_detector/h2') s2 = rospy.get_param('/baloon_detector/s2') v2 = rospy.get_param('/baloon_detector/v2') real_ballon_r = rospy.get_param('/baloon_detector/real_ballon_r') # формируем начальный и конечный цвет фильтра h_min = np.array((h1, s1, v1), np.uint8) h_max = np.array((h2, s2, v2), np.uint8) while not rospy.is_shutdown(): ret, image_raw = camera.read() # calculate FPS cur_time2 = time.time() if cur_time2-my_time_fps>5: my_last_fps=my_fps my_fps=1 my_time_fps=cur_time2 else: my_fps+=1 image_hsv = cv2.cvtColor(image_raw, cv2.COLOR_BGR2HSV ) # накладываем фильтр на кадр в модели HSV thresh = cv2.inRange(image_hsv, h_min, h_max) if invert>0: cv2.bitwise_not(thresh,thresh) #find contours _, contours0, hierarchy = cv2.findContours( thresh.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE) max_area = 0 max_center_x=None max_center_y=None max_baloon_r=None max_yaw_shift=None for cnt in contours0: rect = cv2.minAreaRect(cnt) box = cv2.boxPoints(rect) box = np.int0(box) center = (int(rect[0][0]),int(rect[0][1])) area = int(rect[1][0]*rect[1][1]) if area > 100: baloon_r = int(math.sqrt(area)/2) yaw_shift = int( (center[0] - my_cam_info.width/2) * 130 / my_cam_info.width) # 130 degree camera for my_cam_info.width pics image cv2.circle(image_raw, center, baloon_r , color_red, 1) if area>max_area: max_area=area max_center_x = center[0] max_center_y = center[1] max_baloon_r = baloon_r max_yaw_shift = yaw_shift #draw main baloon if max_area>0: cv2.circle(image_raw, (max_center_x,max_center_y), max_baloon_r+2 , color_blue, 2) cv2.putText(image_raw, "D=%d" % int(max_baloon_r*2), (10, 20), cv2.FONT_HERSHEY_SIMPLEX, 0.5, color_red, 1) cv2.putText(image_raw, "angle=%d '" % max_yaw_shift, (10, 40), cv2.FONT_HERSHEY_SIMPLEX, 0.5, color_green, 1) max_distance = real_ballon_r/math.tan(max_baloon_r/float(my_cam_info.width)*(math.pi*160.0/360.0)) #distance to baloon #print max_distance cv2.putText(image_raw, "%f m" % max_distance, (10, 60), cv2.FONT_HERSHEY_SIMPLEX, 0.5, color_blue, 1) cv2.line(image_raw, (max_center_x, 1), (max_center_x, my_cam_info.height-1) , color_white, 1) cv2.line(image_raw, (1,max_center_y), (my_cam_info.width-1, max_center_y) , color_white, 1) # post baloon Twist tws = TwistStamped() tws.header.stamp = rospy.Time.now() tws.header.frame_id = 'fcu' tws.twist.linear.x = max_distance tws.twist.angular.z = float(-max_yaw_shift)/360.0*math.pi#yaw tws.twist.angular.y = float( (max_center_y - my_cam_info.height/2) * 130.0 / my_cam_info.height) /360.0*math.pi #pitch publisher_baloontwist.publish(tws) #draw target cv2.line(image_raw, (my_cam_info.width/2-10, my_cam_info.height/2-10), (my_cam_info.width/2+10, my_cam_info.height/2+10) , color_yellow, 1) cv2.line(image_raw, (my_cam_info.width/2+10, my_cam_info.height/2-10), (my_cam_info.width/2-10, my_cam_info.height/2+10) , color_yellow, 1) #Draw FPS on image cv2.putText(image_raw,"fps: "+str(my_last_fps/5),(120,20), cv2.FONT_HERSHEY_SIMPLEX, 0.5,(0,255,0),1,cv2.LINE_AA) # публикуем 1 кадр из 5, дабы уменьшить тормоза по вайфаю if my_fps%5==0: try: publisher_image_raw_result.publish(bridge.cv2_to_imgmsg(image_raw,"bgr8")) publisher_image_raw_filter.publish(bridge.cv2_to_imgmsg(thresh, "mono8")) except CvBridgeError as e: print(e) def publish_dummy_vp(event): ps = PoseStamped() ps.header.stamp = rospy.Time.now() ps.header.frame_id = 'local_origin' ps.pose.orientation.w = 1; publisher_vp.publish(ps); def callback_handle_pose(mavros_pose): rospy.loginfo("Got mavros pose, stop publishing zeroes.") dummy_timer.shutdown() handle_pose_sub.unregister() def process_camera_info_yaml(): global mtx, dist, my_cam_info my_cam_info = CameraInfo() filename = rospy.get_param('/baloon_detector/camera_info_url') with open(filename, 'r') as ymlfile: cfg = yaml.load(ymlfile) my_cam_info.width = cfg['image_width'] my_cam_info.height= cfg['image_height'] my_cam_info.K = cfg['camera_matrix']['data'] my_cam_info.D = cfg['distortion_coefficients']['data'] mtx=np.zeros((3,3)) dist=np.zeros((1,5)) for i in range(3): for j in range(3): mtx[i,j]=my_cam_info.K[i*3+j] for i in range(5): dist[0,i]=my_cam_info.D[i] print mtx, dist if __name__ == '__main__': rospy.init_node('baloon_detector') ## init pose publishing from FCU publisher_vp = rospy.Publisher('/mavros/vision_pose/pose', PoseStamped, queue_size=1) dummy_timer=rospy.Timer(rospy.Duration(0.5), publish_dummy_vp) handle_pose_sub = rospy.Subscriber('mavros/local_position/pose', PoseStamped, callback_handle_pose) ## init camera process_camera_info_yaml() print my_cam_info rospy.loginfo("image translation starting.........") publisher_image_raw_filter = rospy.Publisher('baloon_detector/image_filter', Image, queue_size=1) publisher_image_raw_result = rospy.Publisher('baloon_detector/image_result', Image, queue_size=1) publisher_baloontwist= rospy.Publisher('baloon_detector/twist', TwistStamped ,queue_size=1) raspicam_loop_cv() try: rospy.spin() except KeyboardInterrupt: print("Shutting down")
<launch> <!-- mavros --> <include file="$(find mavros)/launch/px4.launch"> <arg name="fcu_url" value="/dev/ttyAMA0:921600"/> <arg name="gcs_url" value="tcp-l://0.0.0.0:5760"/> </include> <!-- baloon pose estimator --> <node name="baloon_detector" pkg="baloon" type="baloon_pose_cv.py" output="screen"> <param name="invert" value="1"/> <param name="h1" value="7"/> <param name="h2" value="158"/> <param name="s1" value="0"/> <param name="s2" value="255"/> <param name="v1" value="0"/> <param name="v2" value="255"/> <param name="real_ballon_r" value="0.145"/> <param name="camera_info_url" value="$(find baloon)/src/fe130_320_01.yaml"/> <param name="framerate" value="40"/> </node> <!-- web video server --> <node name="web_video_server" pkg="web_video_server" type="web_video_server" required="false" respawn="true" respawn_delay="5"/> </launch>
image_width: 320 image_height: 240 camera_name: main_camera_optical camera_matrix: rows: 3 cols: 3 data: [251.8636348237197, 0, 161.853506252244, 0, 252.36606604425, 102.0038140308112, 0, 0, 1] distortion_model: plumb_bob distortion_coefficients: rows: 1 cols: 5 data: [-0.4424451138703088, 0.1594038086314775, 0.006694781700363117, 0.00174908936506397, 0] rectification_matrix: rows: 3 cols: 3 data: [1, 0, 0, 0, 1, 0, 0, 0, 1] projection_matrix: rows: 3 cols: 4 data: [174.0442047119141, 0, 163.822732720786, 0, 0, 175.2916412353516, 105.5565883832869, 0, 0, 0, 1, 0]
После помещения файлов в каталог baloon_ws/src/baloon/src, нужно добавить аттрибут «исполняемый файл» (cmod +x) файлам baloon_pose_cv.py и baloon.service. Затем, в качестве первого теста, запускаем нашу ROS-ноду вручную:
roslaunch baloon baloon_cv.launch
После запуска ноды мы можем наблюдать картинку нашего фильтра и шарика через веб-браузер:
А также получить координаты обнаруженного шарика в топике /baloon_detector/twist:
pi@raspberry:~ $ rostopic list |grep baloon /baloon_detector/image_filter /baloon_detector/image_result /baloon_detector/twist pi@raspberry:~ $ rostopic echo /baloon_detector/twist header: seq: 1 stamp: secs: 1554730508 nsecs: 603406906 frame_id: "fcu" twist: linear: x: 6.6452559203 y: 0.0 z: 0.0 angular: x: 0.0 y: 0.137081068334 z: -0.165806278939 ---
Остаётся добавить автозапуск нашей ноды при старте Raspberry.
Для этого создадим файл описания сервиса systemd baloon.service, следующего содержания:
[Unit] Description=BALOON ROS package Requires=roscore.service After=roscore.service [Service] EnvironmentFile=/home/pi/baloon_ws/src/baloon/src/roscore.env ExecStart=/opt/ros/kinetic/bin/roslaunch baloon baloon_cv.launch --wait Restart=on-abort [Install] WantedBy=multi-user.target
Файл ссылается на описание переменных среды roscore.env:
ROS_ROOT=/opt/ros/kinetic/share/ros ROS_DISTRO=kinetic ROS_PACKAGE_PATH=/home/pi/baloon_ws/src:/opt/ros/kinetic/share ROS_PORT=11311 ROS_MASTER_URI=http://localhost:11311 CMAKE_PREFIX_PATH=/home/pi/baloon_ws/devel:/opt/ros/kinetic PATH=/opt/ros/kinetic/bin:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin:/usr/games:/usr/local/games:/snap/bin LD_LIBRARY_PATH=/opt/ros/kinetic/lib PYTHONPATH=/home/pi/catkin_ws/devel/lib/python2.7/dist-packages:/opt/ros/kinetic/lib/python2.7/dist-packages ROS_IP=192.168.11.1
Подключение и запуск сервиса осуществляется с помощью команд:
sudo systemctl enable ~/baloon_ws/src/baloon/src/baloon.service sudo systemctl start baloon
Теперь нода должна запускаться автоматически при старте Raspberry.
Остановить ноду можно командой sudo systemctl stop baloon, отменить автозапуск – sudo systemctl disable balloon.
В результате наших действий мы получили «автоматический обнаруживатель» шариков, запускаемый при старте системы на бортовом компьютере.
В следующей части перейдём к финальному запуску самонаводящегося дрона.
Исходный код программ выложен на Гитхабе.
ссылка на оригинал статьи https://habr.com/ru/post/462923/
Добавить комментарий