From 993cc50276e85c73ac321a2d960501220e400f59 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Sun, 7 Apr 2019 16:12:02 +0300 Subject: [PATCH 1/7] docs: edit nti.md --- docs/ru/nti2019.md | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/docs/ru/nti2019.md b/docs/ru/nti2019.md index af6332f9..15027d38 100644 --- a/docs/ru/nti2019.md +++ b/docs/ru/nti2019.md @@ -44,8 +44,8 @@ navigate(x=0, y=0, z=1, speed=0.5, frame_id='body', auto_arm=True) # Ждем 5 секунд rospy.sleep(5) -# Полет на координаты x=3, y=2, z=1 площадки со скоростью 0.5 метров в секунду -navigate(x=3, y=2, z=1, speed=0.5, frame_id='aruco_map') +# Полет на координаты x=3, y=2, z=1 площадки с углом по рысканью 3.14 радиан со скоростью 0.5 метров в секунду +navigate(x=3, y=2, z=1, yaw=3.14, speed=0.5, frame_id='aruco_map') # Ждем 5 секунд rospy.sleep(5) @@ -54,4 +54,10 @@ rospy.sleep(5) land() ``` +Пример взлета на высоту 1 метр из командной строки: + +```bash +rosservice call /navigate "{x: 0.0, y: 0.0, z: 2, yaw: 0.0, yaw_rate: 0.0, speed: 0.5, frame_id: 'body', auto_arm: true}" +``` + Для более подробной информации и описания других команд смотрите [API simple_offboard](simple_offboad.md) и [примеры кода](snippets.md). From ae9302bfc219338f0e3727034f9bb7a19736a4b5 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Sun, 7 Apr 2019 16:35:29 +0300 Subject: [PATCH 2/7] Remove +x flag from main_camera.launch --- clever/launch/main_camera.launch | 0 1 file changed, 0 insertions(+), 0 deletions(-) mode change 100755 => 100644 clever/launch/main_camera.launch diff --git a/clever/launch/main_camera.launch b/clever/launch/main_camera.launch old mode 100755 new mode 100644 From 0df66a8df7d84b38359b1879f49e245d88b08cb3 Mon Sep 17 00:00:00 2001 From: sfalexrog Date: Sat, 6 Apr 2019 23:20:35 +0300 Subject: [PATCH 3/7] Add annotated MQTT sample --- docs/ru/nti2019.md | 41 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 41 insertions(+) diff --git a/docs/ru/nti2019.md b/docs/ru/nti2019.md index 15027d38..8b496bfa 100644 --- a/docs/ru/nti2019.md +++ b/docs/ru/nti2019.md @@ -12,6 +12,47 @@ hbmqtt_pub --url mqtt://192.168.0.1:1883 -t /copters/copter1 -m 'сообщен Где `192.168.0.1` – IP-адрес MQTT-брокера, `сообщение` – сообщение для публикации, `/copters/copter1` – необходимый топик для публикации. +### Работа с MQTT + +В образе Клевера предустановлена библиотека `paho-mqtt` для Python. Пример работы с этой библиотекой описан ниже: + +```python +import paho.mqtt.client as mqtt # Импортирование библиотеки mqtt + +# Callback, вызываемый при получении от сервера подтверждения о подключении +def on_connect(client, userdata, flags, rc): + print ("Connected with result code "+str(rc)) + + # Если подписываться на топик в on_connect, то при обрыве соединения + # и повторном подключении произойдёт автоматическое переподписание + client.subscribe("/copters/copter1") + +# Callback, вызываемый при появлении сообщения в одном из топиков, на который +# подписан клиент +def on_message(client, userdata, msg): + # В объекте msg хранится топик, в который пришло сообщение (в поле topic) + # и само сообщение (в поле payload) + print (msg.topic+" "+str(msg.payload)) + +# Инициализация клиента MQTT +client = mqtt.Client() +# Здесь указываются callback'и, вызываемые при подключении и получении сообщения +client.on_connect = on_connect +client.on_message = on_message + +# Подключение к MQTT-брокеру. Первый параметр - имя или адрес брокера, второй - порт +# (по умолчанию 1883), третий - максимальное время между сообщениями в секундах +# (по умолчанию 60). +client.connect("192.168.11.162", 1883, 60) + +# Метод loop_start создаёт поток, в котором будет производиться опрос сервера и +# вызов callback'ов. +client.loop_start() +# Далее продолжается ваша программа +``` + +Более подробная документация доступна на [странице библиотеки в PyPI](https://pypi.org/project/paho-mqtt/). + ## Работа с Клевером Для выполнения команд на Клевере: From 375b19146c53f6a6de00e388ae0d2867896543d3 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Sun, 7 Apr 2019 19:14:34 +0300 Subject: [PATCH 4/7] docs: changes --- docs/ru/nti2019.md | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/docs/ru/nti2019.md b/docs/ru/nti2019.md index 8b496bfa..727deeeb 100644 --- a/docs/ru/nti2019.md +++ b/docs/ru/nti2019.md @@ -2,16 +2,6 @@ ## Работа с MQTT -### Проверка - -Публикация сообщений в топик для проверки может быть осуществлена с помощью команды `hbmqtt_pub`: - -```bash -hbmqtt_pub --url mqtt://192.168.0.1:1883 -t /copters/copter1 -m 'сообщение' -``` - -Где `192.168.0.1` – IP-адрес MQTT-брокера, `сообщение` – сообщение для публикации, `/copters/copter1` – необходимый топик для публикации. - ### Работа с MQTT В образе Клевера предустановлена библиотека `paho-mqtt` для Python. Пример работы с этой библиотекой описан ниже: @@ -53,6 +43,16 @@ client.loop_start() Более подробная документация доступна на [странице библиотеки в PyPI](https://pypi.org/project/paho-mqtt/). +### Проверка + +Публикация сообщений в топик для проверки может быть осуществлена с помощью команды `hbmqtt_pub`: + +```bash +hbmqtt_pub --url mqtt://192.168.0.1:1883 -t /copters/copter1 -m 'сообщение' +``` + +Где `192.168.0.1` – IP-адрес MQTT-брокера, `сообщение` – сообщение для публикации, `/copters/copter1` – необходимый топик для публикации. + ## Работа с Клевером Для выполнения команд на Клевере: From 6535943cc8d200d136e6e80c8c7a72c298ab0a33 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Sun, 7 Apr 2019 19:15:05 +0300 Subject: [PATCH 5/7] docs: fix --- docs/ru/nti2019.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/ru/nti2019.md b/docs/ru/nti2019.md index 727deeeb..e5034a80 100644 --- a/docs/ru/nti2019.md +++ b/docs/ru/nti2019.md @@ -101,4 +101,4 @@ land() rosservice call /navigate "{x: 0.0, y: 0.0, z: 2, yaw: 0.0, yaw_rate: 0.0, speed: 0.5, frame_id: 'body', auto_arm: true}" ``` -Для более подробной информации и описания других команд смотрите [API simple_offboard](simple_offboad.md) и [примеры кода](snippets.md). +Для более подробной информации и описания других команд смотрите [API simple_offboard](simple_offboard.md) и [примеры кода](snippets.md). From e9e8c84ddf2351b5888e498699362f7137acac15 Mon Sep 17 00:00:00 2001 From: sfalexrog Date: Sun, 7 Apr 2019 22:46:38 +0300 Subject: [PATCH 6/7] aruco_pose: Try to draw as much of axes as possible --- aruco_pose/src/draw.cpp | 58 +++++++++++++++++++++++++++-------------- 1 file changed, 39 insertions(+), 19 deletions(-) diff --git a/aruco_pose/src/draw.cpp b/aruco_pose/src/draw.cpp index 65ace1c3..70754b5c 100644 --- a/aruco_pose/src/draw.cpp +++ b/aruco_pose/src/draw.cpp @@ -102,6 +102,42 @@ void _drawPlanarBoard(Board *_board, Size outSize, OutputArray _img, int marginS } } +/* Draw a (potentially partially visible) line. */ +static void linePartial(InputOutputArray image, Point3f p1, Point3f p2, const Scalar& color, + int thickness = 1, int lineType = LINE_8, int shift = 0) +{ + // If both points are behind the screen, don't draw anything + if (p1.z <= 0 && p2.z <= 0) + { + return; + } + Point2f p1p{p1.x, p1.y}; + Point2f p2p{p2.x, p2.y}; + // If points are on the different sides of the plane, compute intersection point + if (p1.z * p2.z < 0) + { + // Compute intersection point with the screen + // We denote alpha as such: + // xi = (1 - alpha) * x1 + alpha * x2 + // yi = (1 - alpha) * y1 + alpha * y2 + // zi = (1 - alpha) * z1 + alpha * z2 = 0 + // Thus, alpha can be expressed as + // alpha = z1 / (z1 - z2) + float alpha = p1.z / (p1.z - p2.z); + Point2f pi{(1 - alpha) * p1.x + alpha * p2.x, (1 - alpha) * p1.y + alpha * p2.y}; + // Now, if z1 is negative, we draw the line from (xi, yi) to (x2, y2), else we draw from (x1, y1) to (xi, yi) + if (p1.z < 0) + { + p1p = pi; + } + else + { + p2p = pi; + } + } + line(image, p1p, p2p, color, thickness, lineType, shift); +} + void _drawAxis(InputOutputArray _image, InputArray _cameraMatrix, InputArray _distCoeffs, InputArray _rvec, InputArray _tvec, float length) { @@ -118,26 +154,10 @@ void _drawAxis(InputOutputArray _image, InputArray _cameraMatrix, InputArray _di std::vector< Point3f > imagePointsZ; _projectPoints(axisPoints, _rvec, _tvec, _cameraMatrix, _distCoeffs, imagePointsZ); - if (imagePointsZ[0].z < 0 || - imagePointsZ[1].z < 0 || - imagePointsZ[2].z < 0 || - imagePointsZ[3].z < 0) - { - // Any axis point is behind screen plane -> don't draw anything - return; - } - - // Intersect axis lines with screen plane (they may be outside) - std::vector imagePoints(4); - imagePoints[0] = Point2f{imagePointsZ[0].x, imagePointsZ[0].y}; - imagePoints[1] = Point2f{imagePointsZ[1].x, imagePointsZ[1].y}; - imagePoints[2] = Point2f{imagePointsZ[2].x, imagePointsZ[2].y}; - imagePoints[3] = Point2f{imagePointsZ[3].x, imagePointsZ[3].y}; - // draw axis lines - line(_image, imagePoints[0], imagePoints[1], Scalar(0, 0, 255), 3); - line(_image, imagePoints[0], imagePoints[2], Scalar(0, 255, 0), 3); - line(_image, imagePoints[0], imagePoints[3], Scalar(255, 0, 0), 3); + linePartial(_image, imagePointsZ[0], imagePointsZ[1], Scalar(0, 0, 255), 3); + linePartial(_image, imagePointsZ[0], imagePointsZ[2], Scalar(0, 255, 0), 3); + linePartial(_image, imagePointsZ[0], imagePointsZ[3], Scalar(255, 0, 0), 3); } static CvMat _cvMat(const cv::Mat& m) From be7624b3092d4a71a903868eb2b73e763e800e61 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Mon, 8 Apr 2019 16:11:09 +0300 Subject: [PATCH 7/7] docs: edit nti article --- docs/ru/nti2019.md | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/docs/ru/nti2019.md b/docs/ru/nti2019.md index e5034a80..3a7f4a7e 100644 --- a/docs/ru/nti2019.md +++ b/docs/ru/nti2019.md @@ -2,9 +2,11 @@ ## Работа с MQTT -### Работа с MQTT +[MQTT](https://ru.wikipedia.org/wiki/MQTT) – протокол для обмена сообщениями между различными устройствами. Этот протокол используется для отправки команд дрону на Олимпиаде НТИ 2019. Для отправки сообщения оно публикуется в определенный топик; все подписчики этого топика получают это сообщение. -В образе Клевера предустановлена библиотека `paho-mqtt` для Python. Пример работы с этой библиотекой описан ниже: +### Подписка на топики + +В образе Клевера для Олимпиады НТИ 2019 предустановлена библиотека `paho-mqtt` для Python. Пример работы с этой библиотекой описан ниже: ```python import paho.mqtt.client as mqtt # Импортирование библиотеки mqtt @@ -22,7 +24,7 @@ def on_connect(client, userdata, flags, rc): def on_message(client, userdata, msg): # В объекте msg хранится топик, в который пришло сообщение (в поле topic) # и само сообщение (в поле payload) - print (msg.topic+" "+str(msg.payload)) + print(msg.topic, str(msg.payload)) # Инициализация клиента MQTT client = mqtt.Client() @@ -33,7 +35,7 @@ client.on_message = on_message # Подключение к MQTT-брокеру. Первый параметр - имя или адрес брокера, второй - порт # (по умолчанию 1883), третий - максимальное время между сообщениями в секундах # (по умолчанию 60). -client.connect("192.168.11.162", 1883, 60) +client.connect('192.168.11.162', 1883, 60) # Метод loop_start создаёт поток, в котором будет производиться опрос сервера и # вызов callback'ов. @@ -45,7 +47,7 @@ client.loop_start() ### Проверка -Публикация сообщений в топик для проверки может быть осуществлена с помощью команды `hbmqtt_pub`: +Для проверки вы можете опубликовать любое сообщение в топик с помощью команды `hbmqtt_pub`: ```bash hbmqtt_pub --url mqtt://192.168.0.1:1883 -t /copters/copter1 -m 'сообщение' @@ -79,8 +81,8 @@ get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry) navigate = rospy.ServiceProxy('navigate', srv.Navigate) land = rospy.ServiceProxy('land', Trigger) -# Взлет на 1 метр со скоростью 0.5 метров в секунду -navigate(x=0, y=0, z=1, speed=0.5, frame_id='body', auto_arm=True) +# Взлет на 1 метр со скоростью 1 метр в секунду +navigate(x=0, y=0, z=1, speed=1, frame_id='body', auto_arm=True) # Ждем 5 секунд rospy.sleep(5)