Merge branch 'master' into nti2019

2026-05-31 15:09:32 +00:00 · 2019-04-08 17:21:11 +03:00
parent 8dec500702 be7624b309
commit 60c97d2318
3 changed files with 96 additions and 25 deletions
--- 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<Point2f> 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)
--- a/clever/launch/main_camera.launch
+++ b/clever/launch/main_camera.launch
--- a/docs/ru/nti2019.md
+++ b/docs/ru/nti2019.md
@@ -2,9 +2,52 @@

 ## Работа с MQTT

+[MQTT](https://ru.wikipedia.org/wiki/MQTT) – протокол для обмена сообщениями между различными устройствами. Этот протокол используется для отправки команд дрону на Олимпиаде НТИ 2019. Для отправки сообщения оно публикуется в определенный топик; все подписчики этого топика получают это сообщение.
+
+### Подписка на топики
+
+В образе Клевера для Олимпиады НТИ 2019 предустановлена библиотека `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/).
+
 ### Проверка

-Публикация сообщений в топик для проверки может быть осуществлена с помощью команды `hbmqtt_pub`:
+Для проверки вы можете опубликовать любое сообщение в топик с помощью команды `hbmqtt_pub`:

 ```bash
 hbmqtt_pub --url mqtt://192.168.0.1:1883 -t /copters/copter1 -m 'сообщение'
@@ -79,14 +122,14 @@ 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)

-# Полет на координаты 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)
@@ -95,7 +138,15 @@ rospy.sleep(5)
 land()
 ```

-Для более подробной информации и описания других команд смотрите [API simple_offboard](simple_offboad.md) и [примеры кода](snippets.md).
+Для более подробной информации и описания других команд смотрите [API simple_offboard](simple_offboard.md) и [примеры кода](snippets.md).
+
+Пример взлета на высоту 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_offboard.md) и [примеры кода](snippets.md).

 ### Работа со светодиодной лентой