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) diff --git a/clever/launch/main_camera.launch b/clever/launch/main_camera.launch old mode 100755 new mode 100644 diff --git a/docs/ru/nti2019.md b/docs/ru/nti2019.md index 0108f8c9..5c88af2d 100644 --- 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). ### Работа со светодиодной лентой