cv_camera 0.5.1

(from documentation)

.github/workflows/build-image.yaml

@@ -0,0 +1,29 @@
+name: Build RPi image
+
+on:
+  push:
+    branches: [ '*' ]
+  pull_request:
+    branches: [ master ]
+  release:
+    types: [ created ]
+
+jobs:
+  build-image:
+    runs-on: ubuntu-latest
+    steps:
+      - uses: actions/checkout@v2
+      - name: Build image
+        run: |
+          docker run --privileged --rm -v /dev:/dev -v $(pwd):/builder/repo -e TRAVIS_TAG="${{ github.event.release.tag_name }}" sfalexrog/img-tool:qemu-update
+      - name: Compress image
+        run: |
+          sudo chmod -R 777 images && zip -9 $(echo images/clover_*).zip images/clover_* && ls -l images
+      - name: Upload image
+        uses: softprops/action-gh-release@v1
+        if: ${{ github.event_name == 'release' }}
+        with:
+          files: images/clover_*.zip
+          prerelease: true
+        env:
+          GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }}

.github/workflows/build.yml

@@ -0,0 +1,23 @@
+name: Build
+
+on:
+  push:
+    branches: [ '*' ]
+  pull_request:
+    branches: [ master ]
+
+jobs:
+  build-melodic:
+    runs-on: ubuntu-latest
+    steps:
+      - uses: actions/checkout@v2
+      - name: Native Melodic build
+        run: |
+          docker run --rm -v $(pwd):/root/catkin_ws/src/clover ros:melodic-ros-base /root/catkin_ws/src/clover/builder/standalone-install.sh
+  build-noetic:
+    runs-on: ubuntu-latest
+    steps:
+      - uses: actions/checkout@v2
+      - name: Native Noetic build
+        run: |
+          docker run --rm -v $(pwd):/root/catkin_ws/src/clover ros:noetic-ros-base /root/catkin_ws/src/clover/builder/standalone-install.sh

.github/workflows/docs.yml

@@ -0,0 +1,54 @@
+name: Documentation
+
+on:
+  push:
+    branches: [ '*' ]
+  pull_request:
+    branches: [ master ]
+
+jobs:
+  documentation:
+    runs-on: ubuntu-18.04
+    steps:
+      - uses: actions/checkout@v2
+      - name: Use Node.js
+        uses: actions/setup-node@v1
+        with: { node-version: '10' }
+      - name: Setup tools
+        run: |
+          sudo sh -c "echo ttf-mscorefonts-installer msttcorefonts/accepted-mscorefonts-eula select true | debconf-set-selections"
+          sudo apt-get update && sudo apt-get install -y calibre msttcorefonts
+          npm install gitbook-cli -g
+          gitbook fetch 3.2.3 && npm i npm@3.10.10 --prefix=~/.gitbook/versions/3.2.3/ # fixing https://travis-ci.org/github/CopterExpress/clover/jobs/766541125#L932
+          npm install markdownlint-cli -g
+          npm install svgexport -g
+          gitbook -V
+          markdownlint -V
+      - name: Run markdownlint
+        run: markdownlint docs
+      - name: Check Assets
+        run: |
+          ./check_assets_size.py
+          ./check_unused_assets.py
+      - name: Build GitBook
+        run: |
+         gitbook install
+         gitbook build
+      - name: Generate PDF
+        if: ${{ github.event_name == 'push' && github.ref == 'refs/heads/master' }}
+        run: |
+          for i in 1 2 3 4; do gitbook pdf ./ _book/clover.pdf && break || sleep 1; done
+          sudo apt-get -q install ghostscript
+          gs -sDEVICE=pdfwrite -dCompatibilityLevel=1.4 -dPDFSETTINGS=/default -dNOPAUSE -dQUIET -dBATCH -dDetectDuplicateImages -dCompressFonts=true -r150 -sOutputFile=_book/clover_ru_compressed.pdf _book/clover_ru.pdf
+          gs -sDEVICE=pdfwrite -dCompatibilityLevel=1.4 -dPDFSETTINGS=/default -dNOPAUSE -dQUIET -dBATCH -dDetectDuplicateImages -dCompressFonts=true -r150 -sOutputFile=_book/clover_en_compressed.pdf _book/clover_en.pdf
+          rm _book/clover_ru.pdf && mv _book/clover_ru_compressed.pdf _book/clover_ru.pdf
+          rm _book/clover_en.pdf && mv _book/clover_en_compressed.pdf _book/clover_en.pdf
+          ls -lah _book/clover*.pdf
+      - name: Deploy
+        uses: JamesIves/github-pages-deploy-action@4.1.3
+        if: ${{ github.event_name == 'push' && github.ref == 'refs/heads/master' }}
+        with:
+          branch: gh-pages
+          folder: _book
+          clean: true
+          single-commit: true # to avoid multiple copies of large pdf files

.github/workflows/editorconfig.yaml

@@ -0,0 +1,18 @@
+name: Editorconfig lint
+
+on:
+  push:
+    branches: [ '*' ]
+  pull_request:
+    branches: [ master ]
+
+jobs:
+  editorconfig:
+    runs-on: ubuntu-latest
+    steps:
+      - uses: actions/checkout@v2
+      - name: .editorconfig Linter
+        run: |
+          wget --no-verbose https://github.com/okalachev/editorconfig-checker/releases/download/1.2.1-disable-spaces-amount/ec-linux-amd64
+          chmod +x ec-linux-amd64
+          ./ec-linux-amd64 -spaces-after-tabs -e "roslib.js|ros3d.js|eventemitter2.js|draw.cpp|BinUtils.swift|\.idea|apps/android/app|blockly/|clover_blocks/programs/|highlight/|python.js|Assets.xcassets|test_parser_pass.txt|test_node_failure.txt|aruco_pose/vendor|\.stl|\.dxf|\.dae"

.markdownlint.json

@@ -21,6 +21,7 @@
             "ROS",
             "ROS Kinetic",
             "ROS Melodic",
+            "ROS Noetic",
             "OpenCV",
             "OpenVPN",
             "Gazebo",

.travis.yml

@@ -1,123 +0,0 @@
-os: linux
-dist: xenial
-language: generic
-services:
-  - docker
-env:
-  global:
-    - DOCKER="sfalexrog/img-tool:qemu-update"
-    - TARGET_REPO="https://github.com/${TRAVIS_REPO_SLUG}.git"
-    - IMAGE_VERSION=${TRAVIS_TAG:-${TRAVIS_COMMIT:0:7}}
-    - IMAGE_NAME="$(basename -s '.git' ${TARGET_REPO})_${IMAGE_VERSION}.img"
-git:
-  depth: 1
-jobs:
-  fast_finish: true
-  include:
-    - stage: Build
-      name: "Raspberry Pi Image Build"
-      cache:
-        directories:
-        - imgcache
-      before_script:
-        - docker pull ${DOCKER}
-# Check if there are any cached images, copy them to our "images" directory
-        - if [ -n "$(ls -A imgcache/*.zip)" ]; then mkdir -p images && cp imgcache/*.zip images; fi
-      script:
-        - if [[ -z ${TRAVIS_TAG} && "${TRAVIS_PULL_REQUEST}" != "false" ]]; then
-            echo "Commit range is ${TRAVIS_COMMIT_RANGE}" &&
-            if [ $(git diff --name-only ${TRAVIS_COMMIT_RANGE} | grep -v ^"docs/" | wc -l) -eq 0 ]; then
-              echo " === Docs-only change; skipping build ===" &&
-              export SKIP_BUILD=true;
-            fi;
-          fi
-        - if [ -z ${SKIP_BUILD} ]; then
-            docker run --privileged --rm -v /dev:/dev -v $(pwd):/builder/repo -e TRAVIS_TAG="${TRAVIS_TAG}" ${DOCKER};
-          fi
-      before_cache:
-        - cp images/*.zip imgcache
-      after_success:
-        - sudo chmod -R 777 *
-        - cd images && zip -9 ${IMAGE_NAME}.zip ${IMAGE_NAME} && stat --printf="Compressed image size:%s\n" ${IMAGE_NAME}.zip
-      before_deploy:
-        # Set up git user name and tag this commit
-        - git config --local user.name "goldarte"
-        - git config --local user.email "goldartt@gmail.com"
-      deploy:
-        provider: releases
-        token: ${GITHUB_OAUTH_TOKEN}
-        file: ${IMAGE_NAME}.zip
-        skip_cleanup: true
-        on:
-          tags: true
-        draft: true
-        name: ${TRAVIS_TAG}
-    - stage: Build
-      name: "Native Kinetic build"
-      env:
-        - NATIVE_DOCKER=ros:kinetic-ros-base
-      before_script:
-        - docker pull ${NATIVE_DOCKER}
-      script:
-        - docker run --rm -v $(pwd):/root/catkin_ws/src/clover ${NATIVE_DOCKER} /root/catkin_ws/src/clover/builder/standalone-install.sh
-    - stage: Build
-      name: "Native Melodic build"
-      env:
-        - NATIVE_DOCKER=ros:melodic-ros-base
-      before_script:
-        - docker pull ${NATIVE_DOCKER}
-      script:
-        - docker run --rm -v $(pwd):/root/catkin_ws/src/clover ${NATIVE_DOCKER} /root/catkin_ws/src/clover/builder/standalone-install.sh
-    - stage: Build
-      name: "Documentation"
-      language: node_js
-      node_js:
-        - "10"
-      before_script:
-        - sudo sh -c "echo ttf-mscorefonts-installer msttcorefonts/accepted-mscorefonts-eula select true | debconf-set-selections"
-        - sudo apt update && sudo apt install -y calibre msttcorefonts
-        - npm install gitbook-cli -g
-        - npm install markdownlint-cli -g
-        - npm install svgexport -g
-        - gitbook -V
-        - markdownlint -V
-      script:
-        - markdownlint docs
-        - ./check_assets_size.py
-        - ./check_unused_assets.py
-        - gitbook install
-        - gitbook build
-        - for i in 1 2 3 4; do gitbook pdf ./ _book/clover.pdf && break || sleep 1; done
-        - sudo apt-get install ghostscript
-        - gs -sDEVICE=pdfwrite -dCompatibilityLevel=1.4 -dPDFSETTINGS=/default -dNOPAUSE -dQUIET -dBATCH -dDetectDuplicateImages -dCompressFonts=true -r150 -sOutputFile=_book/clover_ru_compressed.pdf _book/clover_ru.pdf
-        - gs -sDEVICE=pdfwrite -dCompatibilityLevel=1.4 -dPDFSETTINGS=/default -dNOPAUSE -dQUIET -dBATCH -dDetectDuplicateImages -dCompressFonts=true -r150 -sOutputFile=_book/clover_en_compressed.pdf _book/clover_en.pdf
-        - rm _book/clover_ru.pdf && mv _book/clover_ru_compressed.pdf _book/clover_ru.pdf
-        - rm _book/clover_en.pdf && mv _book/clover_en_compressed.pdf _book/clover_en.pdf
-        - ls -lah _book/clover*.pdf
-      deploy:
-        provider: pages
-        local_dir: _book
-        skip_cleanup: true
-        token: ${GITHUB_OAUTH_TOKEN}
-        keep_history: true
-        target_branch: master
-        repo: CopterExpress/clover.coex.tech
-        fqdn: clover.coex.tech
-        verbose: true
-        on:
-          branch: master
-    - stage: Build
-      name: Editorconfig-lint
-      language: generic
-      before_script:
-        - wget https://github.com/okalachev/editorconfig-checker/releases/download/1.2.1-disable-spaces-amount/ec-linux-amd64
-        - chmod +x ec-linux-amd64
-      script:
-        - ./ec-linux-amd64 -spaces-after-tabs -e "roslib.js|ros3d.js|eventemitter2.js|draw.cpp|BinUtils.swift|\.idea|apps/android/app|blockly/|clover_blocks/programs/|highlight/|python.js|Assets.xcassets|test_parser_pass.txt|test_node_failure.txt|aruco_pose/vendor|\.stl|\.dxf|\.dae"
-stages:
-  - Build
-# More info there
-# https://github.com/travis-ci/travis-ci/issues/6893
-# https://docs.travis-ci.com/user/customizing-the-build/
-# https://docs.travis-ci.com/user/deployment/releases
-# https://docs.travis-ci.com/user/environment-variables/

README.md

@@ -20,12 +20,13 @@ Clover drone is used on a wide range of educational events, including [Copter Ha
 
 Preconfigured image for Raspberry Pi with installed and configured software, ready to fly, is available [in the Releases section](https://github.com/CopterExpress/clover/releases).
 
-[![Build Status](https://travis-ci.org/CopterExpress/clover.svg?branch=master)](https://travis-ci.org/CopterExpress/clover)
+![GitHub Workflow Status](https://img.shields.io/github/workflow/status/CopterExpress/clover/CI)
+![GitHub all releases](https://img.shields.io/github/downloads/CopterExpress/clover/total)
 
 Image features:
 
 * Raspbian Buster
-* [ROS Melodic](http://wiki.ros.org/melodic)
+* [ROS Noetic](http://wiki.ros.org/noetic)
 * Configured networking
 * OpenCV
 * [`mavros`](http://wiki.ros.org/mavros)
@@ -37,6 +38,10 @@ API description for autonomous flights is available [on GitBook](https://clover.
 
 For manual package installation and running see [`clover` package documentation](clover/README.md).
 
+## Support
+
+[![Telegram Support Chat](https://img.shields.io/endpoint?label=Support%20Chat&url=https%3A%2F%2Ftelegram-badge-4mbpu8e0fit4.runkit.sh%2F%3Furl%3Dhttps%3A%2F%2Ft.me%2FCOEXHelpDesk)](https://t.me/COEXHelpdesk)
+
 ## License
 
 While the Clover platform source code is available under the MIT License, note, that the [documentation](docs/) is licensed under the Creative Commons Attribution-NonCommercial-ShareAlike 4.0 International License.

aruco_pose/CMakeLists.txt

@@ -22,13 +22,21 @@ find_package(catkin REQUIRED COMPONENTS
   dynamic_reconfigure
 )
 
-find_package(OpenCV 3 REQUIRED COMPONENTS core imgproc calib3d)
+# Workaround for OpenCV 3/4 support
+set(_opencv_version 4)
+find_package(OpenCV ${_opencv_version} QUIET COMPONENTS core imgproc calib3d)
+if (NOT OpenCV_FOUND)
+  message(STATUS "Did not find OpenCV 4, searching for OpenCV 3")
+  set(_opencv_version 3)
+endif()
+
+find_package(OpenCV ${_opencv_version} REQUIRED COMPONENTS core imgproc calib3d)
 if ("${OpenCV_VERSION_MINOR}" LESS "9")
   message(STATUS "OpenCV version too low, using vendored ArUco package")
   include(vendor/VendorOpenCV.cmake)
 else()
   message(STATUS "Using system OpenCV ArUco package")
-  find_package(OpenCV 3 REQUIRED COMPONENTS aruco)
+  find_package(OpenCV ${_opencv_version} REQUIRED COMPONENTS aruco)
 endif()
 message(STATUS "OpenCV include dirs: ${OpenCV_INCLUDE_DIRS}")
 message(STATUS "OpenCV libraries: ${OpenCV_LIBRARIES}")
@@ -111,7 +119,7 @@ generate_messages(
 
 ## Generate dynamic reconfigure parameters in the 'cfg' folder
 generate_dynamic_reconfigure_options(
-  cfg/DetectorParams.cfg
+  cfg/Detector.cfg
 )
 
 ###################################
@@ -172,6 +180,13 @@ target_link_libraries(aruco_pose
   ${OpenCV_LIBRARIES}
 )
 
+# Prevent aruco_pose from having undefined symbols
+set_property(TARGET aruco_pose
+  APPEND
+  PROPERTY LINK_FLAGS
+  -Wl,--no-undefined
+)
+
 #############
 ## Install ##
 #############
@@ -207,6 +222,10 @@ target_link_libraries(aruco_pose
 #   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 # )
 
+catkin_install_python(PROGRAMS src/genmap.py
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
 #############
 ## Testing ##
 #############

aruco_pose/src/aruco_detect.cpp

@@ -111,17 +111,14 @@ public:
 		image_transport::ImageTransport it(nh_);
 		image_transport::ImageTransport it_priv(nh_priv_);
 
-		map_markers_sub_ = nh_.subscribe("map_markers", 1, &ArucoDetect::mapMarkersCallback, this);
+		dyn_srv_ = std::make_shared<dynamic_reconfigure::Server<aruco_pose::DetectorConfig>>(nh_priv_);
+		dyn_srv_->setCallback(std::bind(&ArucoDetect::paramCallback, this, std::placeholders::_1, std::placeholders::_2));
+
 		debug_pub_ = it_priv.advertise("debug", 1);
 		markers_pub_ = nh_priv_.advertise<aruco_pose::MarkerArray>("markers", 1);
 		vis_markers_pub_ = nh_priv_.advertise<visualization_msgs::MarkerArray>("visualization", 1);
 		img_sub_ = it.subscribeCamera("image_raw", 1, &ArucoDetect::imageCallback, this);
-
-		dyn_srv_ = std::make_shared<dynamic_reconfigure::Server<aruco_pose::DetectorConfig>>(nh_priv_);
-		dynamic_reconfigure::Server<aruco_pose::DetectorConfig>::CallbackType cb;
-
-		cb = std::bind(&ArucoDetect::paramCallback, this, std::placeholders::_1, std::placeholders::_2);
-		dyn_srv_->setCallback(cb);
+		map_markers_sub_ = nh_.subscribe("map_markers", 1, &ArucoDetect::mapMarkersCallback, this);
 
 		NODELET_INFO("ready");
 	}

aruco_pose/src/aruco_map.cpp

@@ -124,6 +124,11 @@ public:
 		vis_markers_pub_ = nh_priv_.advertise<visualization_msgs::MarkerArray>("visualization", 1, true);
 		debug_pub_ = it_priv.advertise("debug", 1);
 
+		publishMarkersFrames();
+		publishMarkers();
+		publishMapImage();
+		vis_markers_pub_.publish(vis_array_);
+
 		image_sub_.subscribe(nh_, "image_raw", 1);
 		info_sub_.subscribe(nh_, "camera_info", 1);
 		markers_sub_.subscribe(nh_, "markers", 1);
@@ -131,11 +136,6 @@ public:
 		sync_.reset(new message_filters::Synchronizer<SyncPolicy>(SyncPolicy(10), image_sub_, info_sub_, markers_sub_));
 		sync_->registerCallback(boost::bind(&ArucoMap::callback, this, _1, _2, _3));
 
-		publishMarkersFrames();
-		publishMarkers();
-		publishMapImage();
-		vis_markers_pub_.publish(vis_array_);
-
 		NODELET_INFO("ready");
 	}
 

aruco_pose/src/draw.cpp

@@ -3,26 +3,11 @@
 
 #include "draw.h"
 #include <math.h>
+#include <vector>
 
 using namespace cv;
 using namespace cv::aruco;
 
-static void _cvProjectPoints2( const CvMat* object_points, const CvMat* rotation_vector,
-                       const CvMat* translation_vector, const CvMat* camera_matrix,
-                       const CvMat* distortion_coeffs, CvMat* image_points,
-                       CvMat* dpdrot CV_DEFAULT(NULL), CvMat* dpdt CV_DEFAULT(NULL),
-                       CvMat* dpdf CV_DEFAULT(NULL), CvMat* dpdc CV_DEFAULT(NULL),
-                       CvMat* dpddist CV_DEFAULT(NULL),
-                       double aspect_ratio CV_DEFAULT(0));
-
-static void _projectPoints( InputArray objectPoints,
-                    InputArray rvec, InputArray tvec,
-                    InputArray cameraMatrix, InputArray distCoeffs,
-                    OutputArray imagePoints,
-                    OutputArray jacobian = noArray(),
-                    double aspectRatio = 0 );
-
-
 void _drawPlanarBoard(Board *_board, Size outSize, OutputArray _img, int marginSize,
                       int borderBits, bool drawAxis) {
 
@@ -142,35 +127,194 @@ 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)
+/**
+ * @brief Convert point coordinates from world space to camera space.
+ *
+ * @param points A vector of points in world space.
+ * @param rvec Rotation matrix or Rodrigues rotation vector.
+ * @param tvec Translation vector from world to camera space.
+ *
+ * @return A vector of points in camera space.
+ */
+template<typename CvPointType>
+static std::vector<CvPointType> worldToCamera(const std::vector<CvPointType>& points,
+                                              const cv::Mat& rvec, const cv::Mat& tvec)
 {
-	// If both points are behind the screen, don't draw anything
-	if (p1.z <= 0 && p2.z <= 0) {
-		return;
+	// We operate with CV_64F matrices internally to avoid precision loss
+	cv::Mat rvec_64f;
+	cv::Mat tvec_64f;
+	rvec.convertTo(rvec_64f, CV_64F);
+	tvec.convertTo(tvec_64f, CV_64F);
+
+	// Convert Rodrigues vector to rotation matrix
+	cv::Mat rmat;
+	if ((rvec_64f.cols == 3 && rvec_64f.rows == 1) ||
+	    (rvec_64f.cols == 1 && rvec_64f.rows == 3))
+	{
+		Rodrigues(rvec_64f, rmat);
 	}
-	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;
-		}
+	else
+	{
+		rmat = rvec_64f.clone();
 	}
-	line(image, p1p, p2p, color, thickness, lineType, shift);
+	// Make sure tvec has a size of (3, 1)
+	if (tvec_64f.rows == 1)
+	{
+		tvec_64f = tvec_64f.t();
+	}
+	std::vector<CvPointType> result;
+	result.reserve(points.size());
+	for(const auto& point : points)
+	{
+		// Calculate point coordinates in camera frame
+		// static_casts are here to silence potential narrowing conversion warnings
+		CvPointType camPoint{
+			static_cast<decltype(CvPointType::x)>(point.x * rmat.at<double>(0,0) + point.y * rmat.at<double>(0,1) + point.z * rmat.at<double>(0,2) + tvec_64f.at<double>(0)),
+			static_cast<decltype(CvPointType::y)>(point.x * rmat.at<double>(1,0) + point.y * rmat.at<double>(1,1) + point.z * rmat.at<double>(1,2) + tvec_64f.at<double>(1)),
+			static_cast<decltype(CvPointType::z)>(point.x * rmat.at<double>(2,0) + point.y * rmat.at<double>(2,1) + point.z * rmat.at<double>(2,2) + tvec_64f.at<double>(2))
+		};
+		result.push_back(camPoint);
+	}
+	return result;
+}
+
+/**
+ * @brief Project points from camera space to screen space, applying distortion in the process.
+ *
+ * @param points A vector of points in camera space.
+ * @param cameraMatrix OpenCV intrinsic camera matrix.
+ * @param distCoeffs OpenCV distortion model coefficients.
+ *
+ * @return A vector of points in screen space.
+ */
+template<typename CvPointType>
+static std::vector<CvPointType> cameraToScreen(const std::vector<CvPointType>& points,
+                                               const cv::Mat& cameraMatrix,
+                                               const cv::Mat& distCoeffs)
+{
+	// We operate with CV_64F matrices internally to avoid precision loss
+	cv::Mat cm_64f; // camera matrix, CV_64F
+	cv::Mat dc_64f; // distortion coefficients, CV_64F
+	cameraMatrix.convertTo(cm_64f, CV_64F);
+	distCoeffs.convertTo(dc_64f, CV_64F);
+
+	// Make sure distortion vector has a size of (N, 1)
+	if (dc_64f.rows == 1)
+	{
+		dc_64f = dc_64f.t();
+	}
+
+	// We will always use 12 distortion coefficients,
+	// and we can safely pad missing ones with zeroes
+	dc_64f.resize(12, 0.0);
+
+	std::vector<CvPointType> result;
+	result.reserve(points.size());
+
+	for(const auto& point : points)
+	{
+		// Apply perspective projection, preserving initial Z coordinate
+		// Always use double-precision
+		cv::Point3d camPoint{
+			point.x / point.z,
+			point.y / point.z,
+			point.z
+		};
+
+		// Apply distortion
+		// Note that we do not consider tilted sensor distortion
+		// r^2 - distance from the image center squared
+		double r2 = camPoint.x * camPoint.x + camPoint.y * camPoint.y;
+		// r^4 - same, but to the 4th power
+		double r4 = r2 * r2;
+		// r^6 - same, but to the 6th power
+		double r6 = r4 * r2;
+		// tg1 - first tangential shift factor (2 * x * y)
+		double tg1 = 2 * camPoint.x * camPoint.y;
+		// tg2 - second tangential shift factor (r^2 + 2 * x^2)
+		double tg2 = r2 + 2 * camPoint.x * camPoint.x;
+		// tg3 - third tangential shift factor (r^2 + 2 * y^2)
+		double tg3 = r2 + 2 * camPoint.y * camPoint.y;
+		// polynomial distortion factor (numerator)
+		double pndist = 1 + dc_64f.at<double>(0) * r2 + dc_64f.at<double>(1) * r4 + dc_64f.at<double>(4) * r6;
+		// polynomial distortion factror (denominator)
+		double pddist = 1.0 / (1 + dc_64f.at<double>(5) * r2 + dc_64f.at<double>(6) * r4 + dc_64f.at<double>(7) * r6);
+		// Distorted point coordinates (always double-precision)
+		cv::Point3d distortedPoint{
+			camPoint.x * pndist * pddist + dc_64f.at<double>(2) * tg1 + dc_64f.at<double>(3) * tg2 + dc_64f.at<double>(8) * r2 + dc_64f.at<double>(9) * r4,
+			camPoint.y * pndist * pddist + dc_64f.at<double>(2) * tg3 + dc_64f.at<double>(3) * tg1 + dc_64f.at<double>(10) * r2 + dc_64f.at<double>(11) * r4,
+			camPoint.z
+		};
+
+		// Convert to screen space
+		// We use static_cast here to silence potential warnings about narrowing conversions
+		// (we expect that to be the case)
+		CvPointType screenPoint{
+			static_cast<decltype(CvPointType::x)>(distortedPoint.x * cm_64f.at<double>(0, 0) + cm_64f.at<double>(0, 2)),
+			static_cast<decltype(CvPointType::y)>(distortedPoint.y * cm_64f.at<double>(1, 1) + cm_64f.at<double>(1, 2)),
+			static_cast<decltype(CvPointType::z)>(distortedPoint.z)
+		};
+
+		result.push_back(screenPoint);
+	}
+	return result;
+}
+
+/**
+ * @brief Clip a line against a clip plane.
+ *
+ * This function "clips" a line (described by two points in *camera space*)
+ * against a clip plane that is `clipPlaneDistance` meters away from the
+ * camera focal point. If both points are further away from the focal point
+ * than `clipPlaneDistance`, they will be returned unmodified. If one of the
+ * points is behind the clipping plane, a point *on* the clipping plane will
+ * be computed and returned as one of the points.
+ *
+ * If none of the points are visible, an empty vector will be returned.
+ *
+ * @param p1 First point on the line, in camera space.
+ * @param p2 Second point on the line, in camera space.
+ * @param clipPlaneDistance Distance from the focal point to the clipping plane.
+ * @return A vector of zero or two points on the clipped line, in camera space.
+ */
+static std::vector<Point3f> lineClip(Point3f p1, Point3f p2, float clipPlaneDistance = 0.1f)
+{
+	// We don't need to compute an intersection if both points are
+	// behind us
+	if (p1.z < clipPlaneDistance && p2.z < clipPlaneDistance)
+	{
+		return {};
+	}
+	// We don't need to compute an intersection if both points are
+	// in front of us
+	if (p1.z > clipPlaneDistance && p2.z > clipPlaneDistance)
+	{
+		return {p1, p2};
+	}
+	// We don't really want to compute an intersection if both Z coordinates
+	// are sufficiently close to each other
+	if (std::abs(p1.z - p2.z) < 0.0001) // The number here is chosen arbitrarily
+	{
+		return {p1, p2};
+	}
+	// We compute the intersection as such:
+	// zi = (1 - alpha) * p1.z + alpha * p2.z = clipPlaneDistance
+	// alpha = (p1.z - clipPlaneDistance) / (p1.z - p2.z)
+	double alpha = (p1.z - clipPlaneDistance) / (p1.z - p2.z);
+	Point3f clipPlanePoint{
+		static_cast<float>((1 - alpha) * p1.x + alpha * p2.x),
+		static_cast<float>((1 - alpha) * p1.y + alpha * p2.y),
+		clipPlaneDistance
+	};
+	if (p1.z < clipPlaneDistance)
+	{
+		return {clipPlanePoint, p2};
+	}
+	else
+	{
+		return {p1, clipPlanePoint};
+	}
+	// Unreachable?
 }
 
 void _drawAxis(InputOutputArray _image, InputArray _cameraMatrix, InputArray _distCoeffs,
@@ -186,647 +330,23 @@ void _drawAxis(InputOutputArray _image, InputArray _cameraMatrix, InputArray _di
 	axisPoints.push_back(Point3f(length, 0, 0));
 	axisPoints.push_back(Point3f(0, length, 0));
 	axisPoints.push_back(Point3f(0, 0, length));
-	std::vector<Point3f> imagePointsZ;
-	_projectPoints(axisPoints, _rvec, _tvec, _cameraMatrix, _distCoeffs, imagePointsZ);
-
-	// draw axis lines
-	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)
-{
-	CvMat self;
-	CV_DbgAssert(m.dims <= 2);
-	self = cvMat(m.rows, m.dims == 1 ? 1 : m.cols, m.type(), m.data);
-	self.step = (int)m.step[0];
-	self.type = (self.type & ~cv::Mat::CONTINUOUS_FLAG) | (m.flags & cv::Mat::CONTINUOUS_FLAG);
-	return self;
-}
-
-static void _projectPoints( InputArray _opoints,
-                        InputArray _rvec,
-                        InputArray _tvec,
-                        InputArray _cameraMatrix,
-                        InputArray _distCoeffs,
-                        OutputArray _ipoints,
-                        OutputArray _jacobian,
-                        double aspectRatio )
-{
-	Mat opoints = _opoints.getMat();
-	int npoints = opoints.checkVector(3), depth = opoints.depth();
-	CV_Assert(npoints >= 0 && (depth == CV_32F || depth == CV_64F));
-
-	CvMat dpdrot, dpdt, dpdf, dpdc, dpddist;
-	CvMat *pdpdrot = 0, *pdpdt = 0, *pdpdf = 0, *pdpdc = 0, *pdpddist = 0;
-
-	CV_Assert(_ipoints.needed());
-
-	_ipoints.create(npoints, 1, CV_MAKETYPE(depth, 3), -1, true);
-	Mat imagePoints = _ipoints.getMat();
-	CvMat c_imagePoints = _cvMat(imagePoints);
-	CvMat c_objectPoints = _cvMat(opoints);
-	Mat cameraMatrix = _cameraMatrix.getMat();
-
-	Mat rvec = _rvec.getMat(), tvec = _tvec.getMat();
-	CvMat c_cameraMatrix = _cvMat(cameraMatrix);
-	CvMat c_rvec = _cvMat(rvec), c_tvec = _cvMat(tvec);
-
-	double dc0buf[5] = {0};
-	Mat dc0(5, 1, CV_64F, dc0buf);
-	Mat distCoeffs = _distCoeffs.getMat();
-	if (distCoeffs.empty())
-		distCoeffs = dc0;
-	CvMat c_distCoeffs = _cvMat(distCoeffs);
-	int ndistCoeffs = distCoeffs.rows + distCoeffs.cols - 1;
-
-	Mat jacobian;
-	if (_jacobian.needed())
+	auto camAxisPoints = worldToCamera(axisPoints, _rvec.getMat(), _tvec.getMat());
+	auto axisX = cameraToScreen(lineClip(camAxisPoints[0], camAxisPoints[1]), _cameraMatrix.getMat(), _distCoeffs.getMat());
+	auto axisY = cameraToScreen(lineClip(camAxisPoints[0], camAxisPoints[2]), _cameraMatrix.getMat(), _distCoeffs.getMat());
+	auto axisZ = cameraToScreen(lineClip(camAxisPoints[0], camAxisPoints[3]), _cameraMatrix.getMat(), _distCoeffs.getMat());
+	if (axisX.size() > 0)
 	{
-		_jacobian.create(npoints * 2, 3 + 3 + 2 + 2 + ndistCoeffs, CV_64F);
-		jacobian = _jacobian.getMat();
-		pdpdrot = &(dpdrot = _cvMat(jacobian.colRange(0, 3)));
-		pdpdt = &(dpdt = _cvMat(jacobian.colRange(3, 6)));
-		pdpdf = &(dpdf = _cvMat(jacobian.colRange(6, 8)));
-		pdpdc = &(dpdc = _cvMat(jacobian.colRange(8, 10)));
-		pdpddist = &(dpddist = _cvMat(jacobian.colRange(10, 10 + ndistCoeffs)));
+		line(_image, Point2f{axisX[0].x, axisX[0].y}, Point2f{axisX[1].x, axisX[1].y},
+			Scalar(0, 0, 255), 3);
+	}
+	if (axisY.size() > 0)
+	{
+		line(_image, Point2f{axisY[0].x, axisY[0].y}, Point2f{axisY[1].x, axisY[1].y},
+			Scalar(0, 255, 0), 3);
+	}
+	if (axisZ.size() > 0)
+	{
+		line(_image, Point2f{axisZ[0].x, axisZ[0].y}, Point2f{axisZ[1].x, axisZ[1].y},
+			Scalar(255, 0, 0), 3);
 	}
-
-	_cvProjectPoints2(&c_objectPoints, &c_rvec, &c_tvec, &c_cameraMatrix, &c_distCoeffs,
-					  &c_imagePoints, pdpdrot, pdpdt, pdpdf, pdpdc, pdpddist, aspectRatio);
-}
-
-namespace _detail
-{
-    template <typename FLOAT>
-    void computeTiltProjectionMatrix(FLOAT tauX,
-                                     FLOAT tauY,
-                                     Matx<FLOAT, 3, 3>* matTilt = 0,
-                                     Matx<FLOAT, 3, 3>* dMatTiltdTauX = 0,
-                                     Matx<FLOAT, 3, 3>* dMatTiltdTauY = 0,
-                                     Matx<FLOAT, 3, 3>* invMatTilt = 0)
-    {
-        FLOAT cTauX = cos(tauX);
-        FLOAT sTauX = sin(tauX);
-        FLOAT cTauY = cos(tauY);
-        FLOAT sTauY = sin(tauY);
-        Matx<FLOAT, 3, 3> matRotX = Matx<FLOAT, 3, 3>(1,0,0,0,cTauX,sTauX,0,-sTauX,cTauX);
-        Matx<FLOAT, 3, 3> matRotY = Matx<FLOAT, 3, 3>(cTauY,0,-sTauY,0,1,0,sTauY,0,cTauY);
-        Matx<FLOAT, 3, 3> matRotXY = matRotY * matRotX;
-        Matx<FLOAT, 3, 3> matProjZ = Matx<FLOAT, 3, 3>(matRotXY(2,2),0,-matRotXY(0,2),0,matRotXY(2,2),-matRotXY(1,2),0,0,1);
-        if (matTilt)
-        {
-            // Matrix for trapezoidal distortion of tilted image sensor
-            *matTilt = matProjZ * matRotXY;
-        }
-        if (dMatTiltdTauX)
-        {
-            // Derivative with respect to tauX
-            Matx<FLOAT, 3, 3> dMatRotXYdTauX = matRotY * Matx<FLOAT, 3, 3>(0,0,0,0,-sTauX,cTauX,0,-cTauX,-sTauX);
-            Matx<FLOAT, 3, 3> dMatProjZdTauX = Matx<FLOAT, 3, 3>(dMatRotXYdTauX(2,2),0,-dMatRotXYdTauX(0,2),
-                                                                 0,dMatRotXYdTauX(2,2),-dMatRotXYdTauX(1,2),0,0,0);
-            *dMatTiltdTauX = (matProjZ * dMatRotXYdTauX) + (dMatProjZdTauX * matRotXY);
-        }
-        if (dMatTiltdTauY)
-        {
-            // Derivative with respect to tauY
-            Matx<FLOAT, 3, 3> dMatRotXYdTauY = Matx<FLOAT, 3, 3>(-sTauY,0,-cTauY,0,0,0,cTauY,0,-sTauY) * matRotX;
-            Matx<FLOAT, 3, 3> dMatProjZdTauY = Matx<FLOAT, 3, 3>(dMatRotXYdTauY(2,2),0,-dMatRotXYdTauY(0,2),
-                                                                 0,dMatRotXYdTauY(2,2),-dMatRotXYdTauY(1,2),0,0,0);
-            *dMatTiltdTauY = (matProjZ * dMatRotXYdTauY) + (dMatProjZdTauY * matRotXY);
-        }
-        if (invMatTilt)
-        {
-            FLOAT inv = 1./matRotXY(2,2);
-            Matx<FLOAT, 3, 3> invMatProjZ = Matx<FLOAT, 3, 3>(inv,0,inv*matRotXY(0,2),0,inv,inv*matRotXY(1,2),0,0,1);
-            *invMatTilt = matRotXY.t()*invMatProjZ;
-        }
-    }
-}
-
-static const char* cvDistCoeffErr = "Distortion coefficients must be 1x4, 4x1, 1x5, 5x1, 1x8, 8x1, 1x12, 12x1, 1x14 or 14x1 floating-point vector";
-
-static void _cvProjectPoints2Internal( const CvMat* objectPoints,
-                                      const CvMat* r_vec,
-                                      const CvMat* t_vec,
-                                      const CvMat* A,
-                                      const CvMat* distCoeffs,
-                                      CvMat* imagePoints, CvMat* dpdr CV_DEFAULT(NULL),
-                                      CvMat* dpdt CV_DEFAULT(NULL), CvMat* dpdf CV_DEFAULT(NULL),
-                                      CvMat* dpdc CV_DEFAULT(NULL), CvMat* dpdk CV_DEFAULT(NULL),
-                                      CvMat* dpdo CV_DEFAULT(NULL),
-                                      double aspectRatio CV_DEFAULT(0) )
-{
-    Ptr<CvMat> matM, _m;
-    Ptr<CvMat> _dpdr, _dpdt, _dpdc, _dpdf, _dpdk;
-    Ptr<CvMat> _dpdo;
-
-    int i, j, count;
-    int calc_derivatives;
-    const CvPoint3D64f* M;
-    CvPoint3D64f* m;
-    double r[3], R[9], dRdr[27], t[3], a[9], k[14] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, fx, fy, cx, cy;
-    Matx33d matTilt = Matx33d::eye();
-    Matx33d dMatTiltdTauX(0,0,0,0,0,0,0,-1,0);
-    Matx33d dMatTiltdTauY(0,0,0,0,0,0,1,0,0);
-    CvMat _r, _t, _a = cvMat( 3, 3, CV_64F, a ), _k;
-    CvMat matR = cvMat( 3, 3, CV_64F, R ), _dRdr = cvMat( 3, 9, CV_64F, dRdr );
-    double *dpdr_p = 0, *dpdt_p = 0, *dpdk_p = 0, *dpdf_p = 0, *dpdc_p = 0;
-    double* dpdo_p = 0;
-    int dpdr_step = 0, dpdt_step = 0, dpdk_step = 0, dpdf_step = 0, dpdc_step = 0;
-    int dpdo_step = 0;
-    bool fixedAspectRatio = aspectRatio > FLT_EPSILON;
-
-    if( !CV_IS_MAT(objectPoints) || !CV_IS_MAT(r_vec) ||
-        !CV_IS_MAT(t_vec) || !CV_IS_MAT(A) ||
-        /*!CV_IS_MAT(distCoeffs) ||*/ !CV_IS_MAT(imagePoints) )
-        CV_Error( CV_StsBadArg, "One of required arguments is not a valid matrix" );
-
-    int total = objectPoints->rows * objectPoints->cols * CV_MAT_CN(objectPoints->type);
-    if(total % 3 != 0)
-    {
-        //we have stopped support of homogeneous coordinates because it cause ambiguity in interpretation of the input data
-        CV_Error( CV_StsBadArg, "Homogeneous coordinates are not supported" );
-    }
-    count = total / 3;
-
-    if( CV_IS_CONT_MAT(objectPoints->type) &&
-        (CV_MAT_DEPTH(objectPoints->type) == CV_32F || CV_MAT_DEPTH(objectPoints->type) == CV_64F)&&
-        ((objectPoints->rows == 1 && CV_MAT_CN(objectPoints->type) == 3) ||
-         (objectPoints->rows == count && CV_MAT_CN(objectPoints->type)*objectPoints->cols == 3) ||
-         (objectPoints->rows == 3 && CV_MAT_CN(objectPoints->type) == 1 && objectPoints->cols == count)))
-    {
-        matM.reset(cvCreateMat( objectPoints->rows, objectPoints->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(objectPoints->type)) ));
-        cvConvert(objectPoints, matM);
-    }
-    else
-    {
-//        matM = cvCreateMat( 1, count, CV_64FC3 );
-//        cvConvertPointsHomogeneous( objectPoints, matM );
-        CV_Error( CV_StsBadArg, "Homogeneous coordinates are not supported" );
-    }
-
-    if( CV_IS_CONT_MAT(imagePoints->type) &&
-        (CV_MAT_DEPTH(imagePoints->type) == CV_32F || CV_MAT_DEPTH(imagePoints->type) == CV_64F) &&
-        ((imagePoints->rows == 1 && CV_MAT_CN(imagePoints->type) == 3) ||
-         (imagePoints->rows == count && CV_MAT_CN(imagePoints->type)*imagePoints->cols == 3) ||
-         (imagePoints->rows == 3 && CV_MAT_CN(imagePoints->type) == 1 && imagePoints->cols == count)))
-    {
-        _m.reset(cvCreateMat( imagePoints->rows, imagePoints->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(imagePoints->type)) ));
-        cvConvert(imagePoints, _m);
-    }
-    else
-    {
-//        _m = cvCreateMat( 1, count, CV_64FC2 );
-        CV_Error( CV_StsBadArg, "Homogeneous coordinates are not supported" );
-    }
-
-    M = (CvPoint3D64f*)matM->data.db;
-    m = (CvPoint3D64f*)_m->data.db;
-
-    if( (CV_MAT_DEPTH(r_vec->type) != CV_64F && CV_MAT_DEPTH(r_vec->type) != CV_32F) ||
-        (((r_vec->rows != 1 && r_vec->cols != 1) ||
-          r_vec->rows*r_vec->cols*CV_MAT_CN(r_vec->type) != 3) &&
-         ((r_vec->rows != 3 && r_vec->cols != 3) || CV_MAT_CN(r_vec->type) != 1)))
-        CV_Error( CV_StsBadArg, "Rotation must be represented by 1x3 or 3x1 "
-                                "floating-point rotation vector, or 3x3 rotation matrix" );
-
-    if( r_vec->rows == 3 && r_vec->cols == 3 )
-    {
-        _r = cvMat( 3, 1, CV_64FC1, r );
-        cvRodrigues2( r_vec, &_r );
-        cvRodrigues2( &_r, &matR, &_dRdr );
-        cvCopy( r_vec, &matR );
-    }
-    else
-    {
-        _r = cvMat( r_vec->rows, r_vec->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(r_vec->type)), r );
-        cvConvert( r_vec, &_r );
-        cvRodrigues2( &_r, &matR, &_dRdr );
-    }
-
-    if( (CV_MAT_DEPTH(t_vec->type) != CV_64F && CV_MAT_DEPTH(t_vec->type) != CV_32F) ||
-        (t_vec->rows != 1 && t_vec->cols != 1) ||
-        t_vec->rows*t_vec->cols*CV_MAT_CN(t_vec->type) != 3 )
-        CV_Error( CV_StsBadArg,
-                  "Translation vector must be 1x3 or 3x1 floating-point vector" );
-
-    _t = cvMat( t_vec->rows, t_vec->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(t_vec->type)), t );
-    cvConvert( t_vec, &_t );
-
-    if( (CV_MAT_TYPE(A->type) != CV_64FC1 && CV_MAT_TYPE(A->type) != CV_32FC1) ||
-        A->rows != 3 || A->cols != 3 )
-        CV_Error( CV_StsBadArg, "Instrinsic parameters must be 3x3 floating-point matrix" );
-
-    cvConvert( A, &_a );
-    fx = a[0]; fy = a[4];
-    cx = a[2]; cy = a[5];
-
-    if( fixedAspectRatio )
-        fx = fy*aspectRatio;
-
-    if( distCoeffs )
-    {
-        if( !CV_IS_MAT(distCoeffs) ||
-            (CV_MAT_DEPTH(distCoeffs->type) != CV_64F &&
-             CV_MAT_DEPTH(distCoeffs->type) != CV_32F) ||
-            (distCoeffs->rows != 1 && distCoeffs->cols != 1) ||
-            (distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 4 &&
-             distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 5 &&
-             distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 8 &&
-             distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 12 &&
-             distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 14) )
-            CV_Error( CV_StsBadArg, cvDistCoeffErr );
-
-        _k = cvMat( distCoeffs->rows, distCoeffs->cols,
-                    CV_MAKETYPE(CV_64F,CV_MAT_CN(distCoeffs->type)), k );
-        cvConvert( distCoeffs, &_k );
-        if(k[12] != 0 || k[13] != 0)
-        {
-            _detail::computeTiltProjectionMatrix(k[12], k[13],
-                                                &matTilt, &dMatTiltdTauX, &dMatTiltdTauY);
-        }
-    }
-
-    if( dpdr )
-    {
-        if( !CV_IS_MAT(dpdr) ||
-            (CV_MAT_TYPE(dpdr->type) != CV_32FC1 &&
-             CV_MAT_TYPE(dpdr->type) != CV_64FC1) ||
-            dpdr->rows != count*2 || dpdr->cols != 3 )
-            CV_Error( CV_StsBadArg, "dp/drot must be 2Nx3 floating-point matrix" );
-
-        if( CV_MAT_TYPE(dpdr->type) == CV_64FC1 )
-        {
-            _dpdr.reset(cvCloneMat(dpdr));
-        }
-        else
-            _dpdr.reset(cvCreateMat( 2*count, 3, CV_64FC1 ));
-        dpdr_p = _dpdr->data.db;
-        dpdr_step = _dpdr->step/sizeof(dpdr_p[0]);
-    }
-
-    if( dpdt )
-    {
-        if( !CV_IS_MAT(dpdt) ||
-            (CV_MAT_TYPE(dpdt->type) != CV_32FC1 &&
-             CV_MAT_TYPE(dpdt->type) != CV_64FC1) ||
-            dpdt->rows != count*2 || dpdt->cols != 3 )
-            CV_Error( CV_StsBadArg, "dp/dT must be 2Nx3 floating-point matrix" );
-
-        if( CV_MAT_TYPE(dpdt->type) == CV_64FC1 )
-        {
-            _dpdt.reset(cvCloneMat(dpdt));
-        }
-        else
-            _dpdt.reset(cvCreateMat( 2*count, 3, CV_64FC1 ));
-        dpdt_p = _dpdt->data.db;
-        dpdt_step = _dpdt->step/sizeof(dpdt_p[0]);
-    }
-
-    if( dpdf )
-    {
-        if( !CV_IS_MAT(dpdf) ||
-            (CV_MAT_TYPE(dpdf->type) != CV_32FC1 && CV_MAT_TYPE(dpdf->type) != CV_64FC1) ||
-            dpdf->rows != count*2 || dpdf->cols != 2 )
-            CV_Error( CV_StsBadArg, "dp/df must be 2Nx2 floating-point matrix" );
-
-        if( CV_MAT_TYPE(dpdf->type) == CV_64FC1 )
-        {
-            _dpdf.reset(cvCloneMat(dpdf));
-        }
-        else
-            _dpdf.reset(cvCreateMat( 2*count, 2, CV_64FC1 ));
-        dpdf_p = _dpdf->data.db;
-        dpdf_step = _dpdf->step/sizeof(dpdf_p[0]);
-    }
-
-    if( dpdc )
-    {
-        if( !CV_IS_MAT(dpdc) ||
-            (CV_MAT_TYPE(dpdc->type) != CV_32FC1 && CV_MAT_TYPE(dpdc->type) != CV_64FC1) ||
-            dpdc->rows != count*2 || dpdc->cols != 2 )
-            CV_Error( CV_StsBadArg, "dp/dc must be 2Nx2 floating-point matrix" );
-
-        if( CV_MAT_TYPE(dpdc->type) == CV_64FC1 )
-        {
-            _dpdc.reset(cvCloneMat(dpdc));
-        }
-        else
-            _dpdc.reset(cvCreateMat( 2*count, 2, CV_64FC1 ));
-        dpdc_p = _dpdc->data.db;
-        dpdc_step = _dpdc->step/sizeof(dpdc_p[0]);
-    }
-
-    if( dpdk )
-    {
-        if( !CV_IS_MAT(dpdk) ||
-            (CV_MAT_TYPE(dpdk->type) != CV_32FC1 && CV_MAT_TYPE(dpdk->type) != CV_64FC1) ||
-            dpdk->rows != count*2 || (dpdk->cols != 14 && dpdk->cols != 12 && dpdk->cols != 8 && dpdk->cols != 5 && dpdk->cols != 4 && dpdk->cols != 2) )
-            CV_Error( CV_StsBadArg, "dp/df must be 2Nx14, 2Nx12, 2Nx8, 2Nx5, 2Nx4 or 2Nx2 floating-point matrix" );
-
-        if( !distCoeffs )
-            CV_Error( CV_StsNullPtr, "distCoeffs is NULL while dpdk is not" );
-
-        if( CV_MAT_TYPE(dpdk->type) == CV_64FC1 )
-        {
-            _dpdk.reset(cvCloneMat(dpdk));
-        }
-        else
-            _dpdk.reset(cvCreateMat( dpdk->rows, dpdk->cols, CV_64FC1 ));
-        dpdk_p = _dpdk->data.db;
-        dpdk_step = _dpdk->step/sizeof(dpdk_p[0]);
-    }
-
-    if( dpdo )
-    {
-        if( !CV_IS_MAT( dpdo ) || ( CV_MAT_TYPE( dpdo->type ) != CV_32FC1
-                                    && CV_MAT_TYPE( dpdo->type ) != CV_64FC1 )
-            || dpdo->rows != count * 2 || dpdo->cols != count * 3 )
-            CV_Error( CV_StsBadArg, "dp/do must be 2Nx3N floating-point matrix" );
-
-        if( CV_MAT_TYPE( dpdo->type ) == CV_64FC1 )
-        {
-            _dpdo.reset( cvCloneMat( dpdo ) );
-        }
-        else
-            _dpdo.reset( cvCreateMat( 2 * count, 3 * count, CV_64FC1 ) );
-        cvZero(_dpdo);
-        dpdo_p = _dpdo->data.db;
-        dpdo_step = _dpdo->step / sizeof( dpdo_p[0] );
-    }
-
-    calc_derivatives = dpdr || dpdt || dpdf || dpdc || dpdk || dpdo;
-
-    for( i = 0; i < count; i++ )
-    {
-        double X = M[i].x, Y = M[i].y, Z = M[i].z;
-        double x = R[0]*X + R[1]*Y + R[2]*Z + t[0];
-        double y = R[3]*X + R[4]*Y + R[5]*Z + t[1];
-        double z = R[6]*X + R[7]*Y + R[8]*Z + t[2];
-        double r2, r4, r6, a1, a2, a3, cdist, icdist2;
-        double xd, yd, xd0, yd0, invProj;
-        Vec3d vecTilt;
-        Vec3d dVecTilt;
-        Matx22d dMatTilt;
-        Vec2d dXdYd;
-
-        double z0 = z;
-        z = z ? 1./z : 1;
-        x *= z; y *= z;
-
-        r2 = x*x + y*y;
-        r4 = r2*r2;
-        r6 = r4*r2;
-        a1 = 2*x*y;
-        a2 = r2 + 2*x*x;
-        a3 = r2 + 2*y*y;
-        cdist = 1 + k[0]*r2 + k[1]*r4 + k[4]*r6;
-        icdist2 = 1./(1 + k[5]*r2 + k[6]*r4 + k[7]*r6);
-        xd0 = x*cdist*icdist2 + k[2]*a1 + k[3]*a2 + k[8]*r2+k[9]*r4;
-        yd0 = y*cdist*icdist2 + k[2]*a3 + k[3]*a1 + k[10]*r2+k[11]*r4;
-
-        // additional distortion by projecting onto a tilt plane
-        vecTilt = matTilt*Vec3d(xd0, yd0, 1);
-        invProj = vecTilt(2) ? 1./vecTilt(2) : 1;
-        xd = invProj * vecTilt(0);
-        yd = invProj * vecTilt(1);
-
-        m[i].x = xd*fx + cx;
-        m[i].y = yd*fy + cy;
-        m[i].z = z; // Just put the projected Z coordinate here, we mainly care about the sign
-
-        if( calc_derivatives )
-        {
-            if( dpdc_p )
-            {
-                dpdc_p[0] = 1; dpdc_p[1] = 0; // dp_xdc_x; dp_xdc_y
-                dpdc_p[dpdc_step] = 0;
-                dpdc_p[dpdc_step+1] = 1;
-                dpdc_p += dpdc_step*2;
-            }
-
-            if( dpdf_p )
-            {
-                if( fixedAspectRatio )
-                {
-                    dpdf_p[0] = 0; dpdf_p[1] = xd*aspectRatio; // dp_xdf_x; dp_xdf_y
-                    dpdf_p[dpdf_step] = 0;
-                    dpdf_p[dpdf_step+1] = yd;
-                }
-                else
-                {
-                    dpdf_p[0] = xd; dpdf_p[1] = 0;
-                    dpdf_p[dpdf_step] = 0;
-                    dpdf_p[dpdf_step+1] = yd;
-                }
-                dpdf_p += dpdf_step*2;
-            }
-            for (int row = 0; row < 2; ++row)
-                for (int col = 0; col < 2; ++col)
-                    dMatTilt(row,col) = matTilt(row,col)*vecTilt(2)
-                                        - matTilt(2,col)*vecTilt(row);
-            double invProjSquare = (invProj*invProj);
-            dMatTilt *= invProjSquare;
-            if( dpdk_p )
-            {
-                dXdYd = dMatTilt*Vec2d(x*icdist2*r2, y*icdist2*r2);
-                dpdk_p[0] = fx*dXdYd(0);
-                dpdk_p[dpdk_step] = fy*dXdYd(1);
-                dXdYd = dMatTilt*Vec2d(x*icdist2*r4, y*icdist2*r4);
-                dpdk_p[1] = fx*dXdYd(0);
-                dpdk_p[dpdk_step+1] = fy*dXdYd(1);
-                if( _dpdk->cols > 2 )
-                {
-                    dXdYd = dMatTilt*Vec2d(a1, a3);
-                    dpdk_p[2] = fx*dXdYd(0);
-                    dpdk_p[dpdk_step+2] = fy*dXdYd(1);
-                    dXdYd = dMatTilt*Vec2d(a2, a1);
-                    dpdk_p[3] = fx*dXdYd(0);
-                    dpdk_p[dpdk_step+3] = fy*dXdYd(1);
-                    if( _dpdk->cols > 4 )
-                    {
-                        dXdYd = dMatTilt*Vec2d(x*icdist2*r6, y*icdist2*r6);
-                        dpdk_p[4] = fx*dXdYd(0);
-                        dpdk_p[dpdk_step+4] = fy*dXdYd(1);
-
-                        if( _dpdk->cols > 5 )
-                        {
-                            dXdYd = dMatTilt*Vec2d(
-                                    x*cdist*(-icdist2)*icdist2*r2, y*cdist*(-icdist2)*icdist2*r2);
-                            dpdk_p[5] = fx*dXdYd(0);
-                            dpdk_p[dpdk_step+5] = fy*dXdYd(1);
-                            dXdYd = dMatTilt*Vec2d(
-                                    x*cdist*(-icdist2)*icdist2*r4, y*cdist*(-icdist2)*icdist2*r4);
-                            dpdk_p[6] = fx*dXdYd(0);
-                            dpdk_p[dpdk_step+6] = fy*dXdYd(1);
-                            dXdYd = dMatTilt*Vec2d(
-                                    x*cdist*(-icdist2)*icdist2*r6, y*cdist*(-icdist2)*icdist2*r6);
-                            dpdk_p[7] = fx*dXdYd(0);
-                            dpdk_p[dpdk_step+7] = fy*dXdYd(1);
-                            if( _dpdk->cols > 8 )
-                            {
-                                dXdYd = dMatTilt*Vec2d(r2, 0);
-                                dpdk_p[8] = fx*dXdYd(0); //s1
-                                dpdk_p[dpdk_step+8] = fy*dXdYd(1); //s1
-                                dXdYd = dMatTilt*Vec2d(r4, 0);
-                                dpdk_p[9] = fx*dXdYd(0); //s2
-                                dpdk_p[dpdk_step+9] = fy*dXdYd(1); //s2
-                                dXdYd = dMatTilt*Vec2d(0, r2);
-                                dpdk_p[10] = fx*dXdYd(0);//s3
-                                dpdk_p[dpdk_step+10] = fy*dXdYd(1); //s3
-                                dXdYd = dMatTilt*Vec2d(0, r4);
-                                dpdk_p[11] = fx*dXdYd(0);//s4
-                                dpdk_p[dpdk_step+11] = fy*dXdYd(1); //s4
-                                if( _dpdk->cols > 12 )
-                                {
-                                    dVecTilt = dMatTiltdTauX * Vec3d(xd0, yd0, 1);
-                                    dpdk_p[12] = fx * invProjSquare * (
-                                            dVecTilt(0) * vecTilt(2) - dVecTilt(2) * vecTilt(0));
-                                    dpdk_p[dpdk_step+12] = fy*invProjSquare * (
-                                            dVecTilt(1) * vecTilt(2) - dVecTilt(2) * vecTilt(1));
-                                    dVecTilt = dMatTiltdTauY * Vec3d(xd0, yd0, 1);
-                                    dpdk_p[13] = fx * invProjSquare * (
-                                            dVecTilt(0) * vecTilt(2) - dVecTilt(2) * vecTilt(0));
-                                    dpdk_p[dpdk_step+13] = fy * invProjSquare * (
-                                            dVecTilt(1) * vecTilt(2) - dVecTilt(2) * vecTilt(1));
-                                }
-                            }
-                        }
-                    }
-                }
-                dpdk_p += dpdk_step*2;
-            }
-
-            if( dpdt_p )
-            {
-                double dxdt[] = { z, 0, -x*z }, dydt[] = { 0, z, -y*z };
-                for( j = 0; j < 3; j++ )
-                {
-                    double dr2dt = 2*x*dxdt[j] + 2*y*dydt[j];
-                    double dcdist_dt = k[0]*dr2dt + 2*k[1]*r2*dr2dt + 3*k[4]*r4*dr2dt;
-                    double dicdist2_dt = -icdist2*icdist2*(k[5]*dr2dt + 2*k[6]*r2*dr2dt + 3*k[7]*r4*dr2dt);
-                    double da1dt = 2*(x*dydt[j] + y*dxdt[j]);
-                    double dmxdt = (dxdt[j]*cdist*icdist2 + x*dcdist_dt*icdist2 + x*cdist*dicdist2_dt +
-                                    k[2]*da1dt + k[3]*(dr2dt + 4*x*dxdt[j]) + k[8]*dr2dt + 2*r2*k[9]*dr2dt);
-                    double dmydt = (dydt[j]*cdist*icdist2 + y*dcdist_dt*icdist2 + y*cdist*dicdist2_dt +
-                                    k[2]*(dr2dt + 4*y*dydt[j]) + k[3]*da1dt + k[10]*dr2dt + 2*r2*k[11]*dr2dt);
-                    dXdYd = dMatTilt*Vec2d(dmxdt, dmydt);
-                    dpdt_p[j] = fx*dXdYd(0);
-                    dpdt_p[dpdt_step+j] = fy*dXdYd(1);
-                }
-                dpdt_p += dpdt_step*2;
-            }
-
-            if( dpdr_p )
-            {
-                double dx0dr[] =
-                        {
-                                X*dRdr[0] + Y*dRdr[1] + Z*dRdr[2],
-                                X*dRdr[9] + Y*dRdr[10] + Z*dRdr[11],
-                                X*dRdr[18] + Y*dRdr[19] + Z*dRdr[20]
-                        };
-                double dy0dr[] =
-                        {
-                                X*dRdr[3] + Y*dRdr[4] + Z*dRdr[5],
-                                X*dRdr[12] + Y*dRdr[13] + Z*dRdr[14],
-                                X*dRdr[21] + Y*dRdr[22] + Z*dRdr[23]
-                        };
-                double dz0dr[] =
-                        {
-                                X*dRdr[6] + Y*dRdr[7] + Z*dRdr[8],
-                                X*dRdr[15] + Y*dRdr[16] + Z*dRdr[17],
-                                X*dRdr[24] + Y*dRdr[25] + Z*dRdr[26]
-                        };
-                for( j = 0; j < 3; j++ )
-                {
-                    double dxdr = z*(dx0dr[j] - x*dz0dr[j]);
-                    double dydr = z*(dy0dr[j] - y*dz0dr[j]);
-                    double dr2dr = 2*x*dxdr + 2*y*dydr;
-                    double dcdist_dr = (k[0] + 2*k[1]*r2 + 3*k[4]*r4)*dr2dr;
-                    double dicdist2_dr = -icdist2*icdist2*(k[5] + 2*k[6]*r2 + 3*k[7]*r4)*dr2dr;
-                    double da1dr = 2*(x*dydr + y*dxdr);
-                    double dmxdr = (dxdr*cdist*icdist2 + x*dcdist_dr*icdist2 + x*cdist*dicdist2_dr +
-                                    k[2]*da1dr + k[3]*(dr2dr + 4*x*dxdr) + (k[8] + 2*r2*k[9])*dr2dr);
-                    double dmydr = (dydr*cdist*icdist2 + y*dcdist_dr*icdist2 + y*cdist*dicdist2_dr +
-                                    k[2]*(dr2dr + 4*y*dydr) + k[3]*da1dr + (k[10] + 2*r2*k[11])*dr2dr);
-                    dXdYd = dMatTilt*Vec2d(dmxdr, dmydr);
-                    dpdr_p[j] = fx*dXdYd(0);
-                    dpdr_p[dpdr_step+j] = fy*dXdYd(1);
-                }
-                dpdr_p += dpdr_step*2;
-            }
-
-            if( dpdo_p )
-            {
-                double dxdo[] = { z * ( R[0] - x * z * z0 * R[6] ),
-                                  z * ( R[1] - x * z * z0 * R[7] ),
-                                  z * ( R[2] - x * z * z0 * R[8] ) };
-                double dydo[] = { z * ( R[3] - y * z * z0 * R[6] ),
-                                  z * ( R[4] - y * z * z0 * R[7] ),
-                                  z * ( R[5] - y * z * z0 * R[8] ) };
-                for( j = 0; j < 3; j++ )
-                {
-                    double dr2do = 2 * x * dxdo[j] + 2 * y * dydo[j];
-                    double dr4do = 2 * r2 * dr2do;
-                    double dr6do = 3 * r4 * dr2do;
-                    double da1do = 2 * y * dxdo[j] + 2 * x * dydo[j];
-                    double da2do = dr2do + 4 * x * dxdo[j];
-                    double da3do = dr2do + 4 * y * dydo[j];
-                    double dcdist_do
-                            = k[0] * dr2do + k[1] * dr4do + k[4] * dr6do;
-                    double dicdist2_do = -icdist2 * icdist2
-                                         * ( k[5] * dr2do + k[6] * dr4do + k[7] * dr6do );
-                    double dxd0_do = cdist * icdist2 * dxdo[j]
-                                     + x * icdist2 * dcdist_do + x * cdist * dicdist2_do
-                                     + k[2] * da1do + k[3] * da2do + k[8] * dr2do
-                                     + k[9] * dr4do;
-                    double dyd0_do = cdist * icdist2 * dydo[j]
-                                     + y * icdist2 * dcdist_do + y * cdist * dicdist2_do
-                                     + k[2] * da3do + k[3] * da1do + k[10] * dr2do
-                                     + k[11] * dr4do;
-                    dXdYd = dMatTilt * Vec2d( dxd0_do, dyd0_do );
-                    dpdo_p[i * 3 + j] = fx * dXdYd( 0 );
-                    dpdo_p[dpdo_step + i * 3 + j] = fy * dXdYd( 1 );
-                }
-                dpdo_p += dpdo_step * 2;
-            }
-        }
-    }
-
-    if( _m != imagePoints )
-        cvConvert( _m, imagePoints );
-
-    if( _dpdr != dpdr )
-        cvConvert( _dpdr, dpdr );
-
-    if( _dpdt != dpdt )
-        cvConvert( _dpdt, dpdt );
-
-    if( _dpdf != dpdf )
-        cvConvert( _dpdf, dpdf );
-
-    if( _dpdc != dpdc )
-        cvConvert( _dpdc, dpdc );
-
-    if( _dpdk != dpdk )
-        cvConvert( _dpdk, dpdk );
-
-    if( _dpdo != dpdo )
-        cvConvert( _dpdo, dpdo );
-}
-
-static void _cvProjectPoints2( const CvMat* objectPoints,
-                        const CvMat* r_vec,
-                        const CvMat* t_vec,
-                        const CvMat* A,
-                        const CvMat* distCoeffs,
-                        CvMat* imagePoints, CvMat* dpdr,
-                        CvMat* dpdt, CvMat* dpdf,
-                        CvMat* dpdc, CvMat* dpdk,
-                        double aspectRatio )
-{
-    _cvProjectPoints2Internal( objectPoints, r_vec, t_vec, A, distCoeffs, imagePoints, dpdr, dpdt,
-                               dpdf, dpdc, dpdk, NULL, aspectRatio );
 }

aruco_pose/src/genmap.py

@@ -13,7 +13,7 @@
 Generate map file for aruco_map nodelet.
 
 Usage:
-  genmap.py <length> <x> <y> <dist_x> <dist_y> [<first>] [<x0>] [<y0>] [--top-left | --bottom-left]
+  genmap.py <length> <x> <y> <dist_x> <dist_y> [<first>] [<x0>] [<y0>] [--top-left | --bottom-left] [-o <filename>]
   genmap.py (-h | --help)
 
 Options:
@@ -27,6 +27,7 @@ Options:
   <y0>           Y coordinate for the first marker [default: 0]
   --top-left     First marker is on top-left (default)
   --bottom-left  First marker is on bottom-left
+  -o <filename>  Output map file name in the 'map' subdirectory of aruco_pose package
 
 Example:
   rosrun aruco_pose genmap.py 0.33 2 4 1 1 0 > $(catkin_find aruco_pose map)/test_map.txt
@@ -34,6 +35,8 @@ Example:
 
 from __future__ import print_function
 
+import sys
+from os import path
 from docopt import docopt
 
 
@@ -49,14 +52,19 @@ dist_x = float(arguments['<dist_x>'])
 dist_y = float(arguments['<dist_y>'])
 bottom_left = arguments['--bottom-left']
 
+if arguments['-o'] is None:
+    output = sys.stdout
+else:
+    output = open(path.join(path.dirname(__file__), '..', 'map', arguments['-o']), 'w')
+
 max_y = y0 + (markers_y - 1) * dist_y
 
-print('# id\tlength\tx\ty\tz\trot_z\trot_y\trot_x')
+output.write('# id\tlength\tx\ty\tz\trot_z\trot_y\trot_x\n')
 for y in range(markers_y):
     for x in range(markers_x):
         pos_x = x0 + x * dist_x
         pos_y = y0 + y * dist_y
         if not bottom_left:
             pos_y = max_y - pos_y
-        print('{}\t{}\t{}\t{}\t{}\t{}\t{}\t{}'.format(first, length, pos_x, pos_y, 0, 0, 0, 0))
+        output.write('{}\t{}\t{}\t{}\t{}\t{}\t{}\t{}\n'.format(first, length, pos_x, pos_y, 0, 0, 0, 0))
         first += 1

aruco_pose/vendor/VendorOpenCV.cmake

@@ -7,6 +7,7 @@ endif()
 
 message(STATUS "Adding vendored aruco_pose OpenCV module")
 add_library(_opencv_aruco STATIC
+  vendor/aruco/src/apriltag_quad_thresh.cpp
   vendor/aruco/src/aruco.cpp
   vendor/aruco/src/charuco.cpp
   vendor/aruco/src/dictionary.cpp
@@ -23,7 +24,7 @@ target_compile_definitions(_opencv_aruco PRIVATE
   CV_OVERRIDE=override
 )
 target_compile_options(_opencv_aruco PRIVATE
-  -fpic -fPIC
+  -fpic -fPIC -fvisibility=hidden
 )
 
 target_include_directories(_opencv_aruco PUBLIC

aruco_pose/vendor/aruco/src/apriltag_quad_thresh.cpp

@@ -94,7 +94,7 @@ void ptsort_(struct pt *pts, int sz)
 
     // Use stack storage if it's not too big.
     cv::AutoBuffer<struct pt, 1024> _tmp_stack(sz);
-    memcpy(_tmp_stack.data(), pts, sizeof(struct pt) * sz);
+    memcpy(_tmp_stack, pts, sizeof(struct pt) * sz);
 
     int asz = sz/2;
     int bsz = sz - asz;
@@ -470,11 +470,11 @@ int quad_segment_agg(int sz, struct line_fit_pt *lfps, int indices[4]){
     int rvalloc_pos = 0;
     int rvalloc_size = 3*sz;
     cv::AutoBuffer<struct remove_vertex, 0> rvalloc_(std::max(1, rvalloc_size));
-    memset(rvalloc_.data(), 0, sizeof(rvalloc_[0]) * rvalloc_.size()); // TODO Add AutoBuffer zero fill
-    struct remove_vertex *rvalloc = rvalloc_.data();
+    memset(rvalloc_, 0, sizeof(rvalloc_[0]) * rvalloc_.size()); // TODO Add AutoBuffer zero fill
+    struct remove_vertex *rvalloc = rvalloc_;
     cv::AutoBuffer<struct segment, 0> segs_(std::max(1, sz)); // TODO Add AutoBuffer zero fill
-    memset(segs_.data(), 0, sizeof(segs_[0]) * segs_.size());
-    struct segment *segs = segs_.data();
+    memset(segs_, 0, sizeof(segs_[0]) * segs_.size());
+    struct segment *segs = segs_;
 
     // populate with initial entries
     for (int i = 0; i < sz; i++) {
@@ -753,8 +753,8 @@ int fit_quad(const Ptr<DetectorParameters> &_params, const Mat im, zarray_t *clu
     // efficiently computed for any contiguous range of indices.
 
     cv::AutoBuffer<struct line_fit_pt, 64> lfps_(sz);
-    memset(lfps_.data(), 0, sizeof(lfps_[0]) * lfps_.size()); // TODO Add AutoBuffer zero fill
-    struct line_fit_pt *lfps = lfps_.data();
+    memset(lfps_, 0, sizeof(lfps_[0]) * lfps_.size()); // TODO Add AutoBuffer zero fill
+    struct line_fit_pt *lfps = lfps_;
 
     for (int i = 0; i < sz; i++) {
         struct pt *p;

builder/assets/clever/setup.py

@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
 
 from distutils.core import setup
 

builder/assets/examples/get_telemetry.py

@@ -0,0 +1,11 @@
+# Information: https://clover.coex.tech/en/simple_offboard.html#gettelemetry
+
+import rospy
+from clover import srv
+
+rospy.init_node('flight')
+
+get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry)
+
+# Print drone's state
+print(get_telemetry())

builder/assets/hardware_setup.sh

@@ -65,7 +65,7 @@ echo_stamp "#6 Turn on UART"
 # https://github.com/RPi-Distro/raspi-config/pull/75
 /usr/bin/raspi-config nonint do_serial 1
 /usr/bin/raspi-config nonint set_config_var enable_uart 1 /boot/config.txt
-/usr/bin/raspi-config nonint set_config_var dtoverlay pi3-disable-bt /boot/config.txt
+echo dtoverlay=pi3-disable-bt >> /boot/config.txt
 systemctl disable hciuart.service
 
 # After adding to Raspbian OS

builder/assets/noetic-rosdep-clover.yaml

@@ -0,0 +1,18 @@
+async_web_server_cpp:
+  debian:
+    buster: [ros-noetic-async-web-server-cpp]
+led_msgs:
+  debian:
+    buster: [ros-noetic-led-msgs]
+ros_pytest:
+  debian:
+    buster: [ros-noetic-ros-pytest]
+tf2_web_republisher:
+  debian:
+    buster: [ros-noetic-tf2-web-republisher]
+web_video_server:
+  debian:
+    buster: [ros-noetic-web-video-server]
+ws281x:
+  debian:
+    buster: [ros-noetic-ws281x]

builder/assets/roscore.service

@@ -3,7 +3,7 @@ Description=Launcher for the ROS master, parameter server and rosout logging nod
 
 [Service]
 User=pi
-ExecStart=/bin/sh -c ". /opt/ros/melodic/setup.sh; ROS_HOSTNAME=`hostname`.local exec roscore"
+ExecStart=/bin/sh -c ". /opt/ros/noetic/setup.sh; ROS_HOSTNAME=`hostname`.local exec roscore"
 Restart=on-failure
 RestartSec=3
 

builder/image-build.sh

@@ -15,7 +15,8 @@
 
 set -e # Exit immidiately on non-zero result
 
-SOURCE_IMAGE="https://downloads.raspberrypi.org/raspios_lite_armhf/images/raspios_lite_armhf-2021-01-12/2021-01-11-raspios-buster-armhf-lite.zip"
+# https://www.raspberrypi.org/software/operating-systems/#raspberry-pi-os-32-bit
+SOURCE_IMAGE="https://downloads.raspberrypi.org/raspios_lite_armhf/images/raspios_lite_armhf-2021-05-28/2021-05-07-raspios-buster-armhf-lite.zip"
 
 export DEBIAN_FRONTEND=${DEBIAN_FRONTEND:='noninteractive'}
 export LANG=${LANG:='C.UTF-8'}
@@ -116,7 +117,7 @@ ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/avahi-s
 # Clover
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/clover.service' '/lib/systemd/system/'
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/roscore.service' '/lib/systemd/system/'
-${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/melodic-rosdep-clover.yaml' '/etc/ros/rosdep/'
+${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/noetic-rosdep-clover.yaml' '/etc/ros/rosdep/'
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/ros_python_paths' '/etc/sudoers.d/'
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/pigpiod.service' '/lib/systemd/system/'
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/launch.nanorc' '/usr/share/nano/'

builder/image-ros.sh

@@ -21,6 +21,9 @@ INSTALL_ROS_PACK_SOURCES=$3
 DISCOVER_ROS_PACK=$4
 NUMBER_THREADS=$5
 
+# Current ROS distribution
+ROS_DISTRO=noetic
+
 echo_stamp() {
   # TEMPLATE: echo_stamp <TEXT> <TYPE>
   # TYPE: SUCCESS, ERROR, INFO
@@ -68,7 +71,8 @@ my_travis_retry() {
 # TODO: 'kinetic-rosdep-clover.yaml' should add only if we use our repo?
 echo_stamp "Init rosdep"
 my_travis_retry rosdep init
-echo "yaml file:///etc/ros/rosdep/melodic-rosdep-clover.yaml" >> /etc/ros/rosdep/sources.list.d/20-default.list
+# FIXME: Re-add this after missing packages are built
+echo "yaml file:///etc/ros/rosdep/${ROS_DISTRO}-rosdep-clover.yaml" >> /etc/ros/rosdep/sources.list.d/20-default.list
 my_travis_retry rosdep update
 
 echo_stamp "Populate rosdep for ROS user"
@@ -76,22 +80,39 @@ my_travis_retry sudo -u pi rosdep update
 
 export ROS_IP='127.0.0.1' # needed for running tests
 
-# echo_stamp "Reconfiguring Clover repository for simplier unshallowing" # TODO: bring back
-# cd /home/pi/catkin_ws/src/clover
-# git config remote.origin.fetch "+refs/heads/*:refs/remotes/origin/*"
-echo_stamp "Remove .git from Clover to reduce the size"
-rm -rf /home/pi/catkin_ws/src/clover/.git # TODO: remove
+# echo_stamp "Reconfiguring Clover repository for simplier unshallowing"
+cd /home/pi/catkin_ws/src/clover
+git config remote.origin.fetch "+refs/heads/*:refs/remotes/origin/*"
+
+# This is sort of a hack to force "custom" packages to be installed - the ones built by COEX, linked against OpenCV 4.2
+# I **wish** OpenCV would not be such a mess, but, well, here we are.
+echo_stamp "Installing OpenCV 4.2-compatible ROS packages"
+apt install -y --no-install-recommends \
+ros-${ROS_DISTRO}-compressed-image-transport=1.14.0-0buster \
+ros-${ROS_DISTRO}-cv-bridge=1.15.0-0buster \
+ros-${ROS_DISTRO}-cv-camera=0.5.1-0buster \
+ros-${ROS_DISTRO}-image-publisher=1.15.3-0buster \
+ros-${ROS_DISTRO}-web-video-server=0.2.1-0buster
+apt-mark hold \
+ros-${ROS_DISTRO}-compressed-image-transport \
+ros-${ROS_DISTRO}-cv-bridge \
+ros-${ROS_DISTRO}-cv-camera \
+ros-${ROS_DISTRO}-image-publisher \
+ros-${ROS_DISTRO}-web-video-server
+
+echo_stamp "Installing libboost-dev" # https://travis-ci.org/github/CopterExpress/clover/jobs/766318908#L6536
+my_travis_retry apt-get install -y --no-install-recommends libboost-dev libboost-all-dev
 
 echo_stamp "Build and install Clover"
 cd /home/pi/catkin_ws
 # Don't try to install gazebo_ros
-my_travis_retry rosdep install -y --from-paths src --ignore-src --rosdistro melodic --os=debian:buster \
+my_travis_retry rosdep install -y --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} --os=debian:buster \
   --skip-keys=gazebo_ros --skip-keys=gazebo_plugins
-my_travis_retry pip install wheel
-my_travis_retry pip install -r /home/pi/catkin_ws/src/clover/clover/requirements.txt
-source /opt/ros/melodic/setup.bash
+my_travis_retry pip3 install wheel
+my_travis_retry pip3 install -r /home/pi/catkin_ws/src/clover/clover/requirements.txt
+source /opt/ros/${ROS_DISTRO}/setup.bash
 # Don't build simulation plugins for actual drone
-catkin_make -j2 -DCMAKE_BUILD_TYPE=Release -DCATKIN_BLACKLIST_PACKAGES=clover_gazebo_plugins
+catkin_make -j2 -DCMAKE_BUILD_TYPE=RelWithDebInfo -DCATKIN_BLACKLIST_PACKAGES=clover_gazebo_plugins
 
 echo_stamp "Install clever package (for backwards compatibility)"
 cd /home/pi/catkin_ws/src/clover/builder/assets/clever
@@ -101,29 +122,27 @@ rm -rf build  # remove build artifacts
 echo_stamp "Build Clover documentation"
 cd /home/pi/catkin_ws/src/clover
 NPM_CONFIG_UNSAFE_PERM=true npm install gitbook-cli -g
+NPM_CONFIG_UNSAFE_PERM=true gitbook fetch 3.2.3 && npm i npm@3.10.10 --prefix=~/.gitbook/versions/3.2.3/ # fixing https://travis-ci.org/github/CopterExpress/clover/jobs/766541125#L932
 NPM_CONFIG_UNSAFE_PERM=true gitbook install
 gitbook build
 touch node_modules/CATKIN_IGNORE docs/CATKIN_IGNORE _book/CATKIN_IGNORE clover/www/CATKIN_IGNORE apps/CATKIN_IGNORE # ignore documentation files by catkin
 
 echo_stamp "Installing additional ROS packages"
 my_travis_retry apt-get install -y --no-install-recommends \
-    ros-melodic-dynamic-reconfigure \
-    ros-melodic-compressed-image-transport \
-    ros-melodic-rosbridge-suite \
-    ros-melodic-rosserial \
-    ros-melodic-usb-cam \
-    ros-melodic-vl53l1x \
-    ros-melodic-ws281x \
-    ros-melodic-rosshow
+    ros-${ROS_DISTRO}-dynamic-reconfigure \
+    ros-${ROS_DISTRO}-rosbridge-suite \
+    ros-${ROS_DISTRO}-rosserial \
+    ros-${ROS_DISTRO}-usb-cam \
+    ros-${ROS_DISTRO}-vl53l1x \
+    ros-${ROS_DISTRO}-ws281x \
+    ros-${ROS_DISTRO}-rosshow \
+    ros-${ROS_DISTRO}-cmake-modules \
+    ros-${ROS_DISTRO}-image-view
 
 # TODO move GeographicLib datasets to Mavros debian package
 echo_stamp "Install GeographicLib datasets (needed for mavros)" \
 && wget -qO- https://raw.githubusercontent.com/mavlink/mavros/master/mavros/scripts/install_geographiclib_datasets.sh | bash
 
-# FIXME: Buster comes with tornado==5.1.1 but we need tornado==4.2.1 for rosbridge_suite
-# (note that Python 3 will still have a more recent version)
-pip install tornado==4.2.1
-
 echo_stamp "Running tests"
 cd /home/pi/catkin_ws
 # FIXME: Investigate failing tests
@@ -140,7 +159,7 @@ cat << EOF >> /home/pi/.bashrc
 LANG='C.UTF-8'
 LC_ALL='C.UTF-8'
 export ROS_HOSTNAME=\`hostname\`.local
-source /opt/ros/melodic/setup.bash
+source /opt/ros/${ROS_DISTRO}/setup.bash
 source /home/pi/catkin_ws/devel/setup.bash
 EOF
 

builder/image-software.sh

@@ -64,15 +64,14 @@ echo "APT::Acquire::Retries \"3\";" > /etc/apt/apt.conf.d/80-retries
 echo_stamp "Install apt keys & repos"
 
 # TODO: This STDOUT consist 'OK'
-curl http://deb.coex.tech/aptly_repo_signing.key 2> /dev/null | apt-key add -
 apt-get update \
 && apt-get install --no-install-recommends -y dirmngr > /dev/null \
 && apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654
 
 echo "deb http://packages.ros.org/ros/ubuntu buster main" > /etc/apt/sources.list.d/ros-latest.list
-echo "deb http://deb.coex.tech/opencv3 buster main" > /etc/apt/sources.list.d/opencv3.list
-echo "deb http://deb.coex.tech/rpi-ros-melodic buster main" > /etc/apt/sources.list.d/rpi-ros-melodic.list
-echo "deb http://deb.coex.tech/clover buster main" > /etc/apt/sources.list.d/clover.list
+
+wget -O - 'http://packages.coex.tech/key.asc' | apt-key add - 
+echo 'deb http://packages.coex.tech buster main' >> /etc/apt/sources.list
 
 echo_stamp "Update apt cache"
 
@@ -99,18 +98,18 @@ tree \
 vim \
 libjpeg8 \
 tcpdump \
-ltrace \
 libpoco-dev \
 libzbar0 \
-python-rosdep \
-python-rosinstall-generator \
-python-wstool \
-python-rosinstall \
+python3-rosdep \
+python3-rosinstall-generator \
+python3-wstool \
+python3-rosinstall \
 build-essential \
 libffi-dev \
 monkey \
 pigpio python-pigpio python3-pigpio \
 i2c-tools \
+espeak espeak-data python-espeak python3-espeak \
 ntpdate \
 python-dev \
 python3-dev \
@@ -144,16 +143,17 @@ my_travis_retry pip3 install butterfly[systemd]
 systemctl enable butterfly.socket
 
 echo_stamp "Install ws281x library"
-my_travis_retry pip install --prefer-binary rpi_ws281x
+my_travis_retry pip3 install --prefer-binary rpi_ws281x
 
 echo_stamp "Setup Monkey"
 mv /etc/monkey/sites/default /etc/monkey/sites/default.orig
 mv /root/monkey /etc/monkey/sites/default
+sed -i 's/SymLink Off/SymLink On/' /etc/monkey/monkey.conf
 systemctl enable monkey.service
 
 echo_stamp "Install Node.js"
 cd /home/pi
-wget https://nodejs.org/dist/v10.15.0/node-v10.15.0-linux-armv6l.tar.gz
+wget --no-verbose https://nodejs.org/dist/v10.15.0/node-v10.15.0-linux-armv6l.tar.gz
 tar -xzf node-v10.15.0-linux-armv6l.tar.gz
 cp -R node-v10.15.0-linux-armv6l/* /usr/local/
 rm -rf node-v10.15.0-linux-armv6l/

builder/image-validate.sh

@@ -16,16 +16,20 @@ set -ex
 
 echo "Run image tests"
 
-export ROS_DISTRO='melodic'
+export ROS_DISTRO='noetic'
 export ROS_IP='127.0.0.1'
-source /opt/ros/melodic/setup.bash
+source /opt/ros/${ROS_DISTRO}/setup.bash
 source /home/pi/catkin_ws/devel/setup.bash
+systemctl start roscore
 
 cd /home/pi/catkin_ws/src/clover/builder/test/
 ./tests.sh
 ./tests.py
 ./tests_py3.py
+[[ $(./test_qr.py) == "Found QRCODE with data Проверка Unicode with center at x=66.0, y=66.0" ]] 
 [[ $(./tests_clever.py) == "Warning: clever package is renamed to clover" ]]  # test backwards compatibility
 
+systemctl stop roscore
+
 echo "Move /etc/ld.so.preload back to its original position"
 mv /etc/ld.so.preload.disabled-for-build /etc/ld.so.preload

builder/standalone-install.sh

@@ -3,25 +3,45 @@
 # Perform a "standalone install" in a Docker container
 set -e
 # Step 1: Install pip
-apt update
-apt install -y curl
-curl https://bootstrap.pypa.io/pip/2.7/get-pip.py -o get-pip.py
-python ./get-pip.py
+apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 # https://github.com/osrf/docker_images/issues/535
+apt-get update
+apt-get install -y curl
+if [ "x${ROS_PYTHON_VERSION}" = "x3" ]; then
+  PYTHON=python3
+  curl https://bootstrap.pypa.io/get-pip.py -o get-pip.py
+else
+  PYTHON=python
+  curl https://bootstrap.pypa.io/pip/2.7/get-pip.py -o get-pip.py
+fi
+${PYTHON} ./get-pip.py
 
 # Step 1.5: Add deb.coex.tech to apt
 curl http://deb.coex.tech/aptly_repo_signing.key 2> /dev/null | apt-key add -
 echo "deb http://deb.coex.tech/ros xenial main" > /etc/apt/sources.list.d/coex.tech.list
 echo "yaml file:///etc/ros/rosdep/coex.yaml" > /etc/ros/rosdep/sources.list.d/99-coex.list
+CODENAME=$(lsb_release -sc)
+
 cat <<EOF > /etc/ros/rosdep/coex.yaml
 led_msgs:
   ubuntu:
-    xenial: ros-kinetic-led-msgs
-    bionic: ros-melodic-led-msgs
-  debian:
-    stretch: ros-kinetic-led-msgs
-    buster: ros-melodic-led-msgs
+    ${CODENAME}: [ros-${ROS_DISTRO}-led-msgs]
+async_web_server_cpp:
+  ubuntu:
+    ${CODENAME}: [ros-${ROS_DISTRO}-async-web-server-cpp]
+ros_pytest:
+  ubuntu:
+    ${CODENAME}: [ros-${ROS_DISTRO}-ros-pytest]
+tf2_web_republisher:
+  ubuntu:
+    ${CODENAME}: [ros-${ROS_DISTRO}-tf2-web-republisher]
+web_video_server:
+  ubuntu:
+    ${CODENAME}: [ros-${ROS_DISTRO}-web-video-server]
+ws281x:
+  ubuntu:
+    ${CODENAME}: [ros-${ROS_DISTRO}-ws281x]
 EOF
-apt update
+apt-get update
 rosdep update
 
 # Step 2: Run rosdep to install all dependencies
@@ -37,7 +57,10 @@ cd /root/catkin_ws
 catkin_make
 
 # Step 4: Run tests
-pip install --upgrade pytest
+${PYTHON} -m pip install --upgrade pytest
 cd /root/catkin_ws
 source devel/setup.bash
 catkin_make run_tests && catkin_test_results
+
+# Step 5: Install packages
+catkin_make install

builder/test/test_qr.py

@@ -0,0 +1,42 @@
+#!/usr/bin/env python3
+
+# Test QG recognition example
+# Should be synced with the documentation: /docs/en/camera.md, /docs/ru/camera.md
+# TODO: use real ROS topics
+
+import rospy
+from pyzbar import pyzbar
+from cv_bridge import CvBridge
+from sensor_msgs.msg import Image
+
+bridge = CvBridge()
+
+# rospy.init_node('barcode_test')
+
+# Image subscriber callback function
+def image_callback(data):
+    cv_image = bridge.imgmsg_to_cv2(data, 'bgr8')  # OpenCV image
+    barcodes = pyzbar.decode(cv_image)
+    for barcode in barcodes:
+        b_data = barcode.data.decode("utf-8")
+        b_type = barcode.type
+        (x, y, w, h) = barcode.rect
+        xc = x + w/2
+        yc = y + h/2
+        print("Found {} with data {} with center at x={}, y={}".format(b_type, b_data, xc, yc))
+    # rospy.signal_shutdown('done')
+
+# image_sub = rospy.Subscriber('main_camera/image_raw', Image, image_callback, queue_size=1)
+
+# ==============================================================================
+# Publish test image
+# rospy.sleep(2)
+
+import cv2
+img = cv2.imread('qr.png')
+image_callback(bridge.cv2_to_imgmsg(img, 'bgr8'))
+
+# image_pub = rospy.Publisher('/main_camera/image_raw', Image, queue_size=1, latch=True)
+# image_pub.publish(bridge.cv2_to_imgmsg(img, 'bgr8'))
+
+# rospy.spin()

builder/test/tests.py

@@ -1,21 +1,29 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
 
 # validate all required modules installed
 
 import rospy
 from geometry_msgs.msg import PoseStamped
+from sensor_msgs.msg import Range, BatteryState
 
 import cv2
 import cv2.aruco
+from sensor_msgs.msg import Image
+from cv_bridge import CvBridge
 
 import numpy
 import mavros
-from mavros_msgs.msg import State, StatusText, ExtendedState
+from mavros_msgs.msg import State, StatusText, ExtendedState, RCIn, Mavlink
 from mavros_msgs.srv import CommandBool, CommandLong, SetMode
 
 from std_srvs.srv import Trigger
 from clover.srv import GetTelemetry, Navigate, NavigateGlobal, SetPosition, SetVelocity, \
     SetAttitude, SetRates, SetLEDEffect
+from led_msgs.srv import SetLEDs
+from led_msgs.msg import LEDStateArray, LEDState
+from aruco_pose.msg import Marker, MarkerArray, Point2D
+
+import dynamic_reconfigure.client
 
 import tf2_ros
 import tf2_geometry_msgs
@@ -28,4 +36,4 @@ import pigpio
 # from espeak import espeak
 from pyzbar import pyzbar
 
-print cv2.getBuildInformation()
+print(cv2.getBuildInformation())

builder/test/tests.sh

@@ -54,6 +54,8 @@ rosversion usb_cam
 rosversion cv_camera
 rosversion web_video_server
 rosversion rosshow
+rosversion nodelet
+rosversion image_view
 
 # validate examples are present
 [[ $(ls /home/pi/examples/*) ]]

builder/test/tests_clever.py

@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
 
 # test backwards compatibility
 

clover/CMakeLists.txt

@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.0)
 project(clover)
 
 ## Compile as C++11, supported in ROS Kinetic and newer
@@ -30,7 +30,15 @@ list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/cmake")
 
 find_package(GeographicLib REQUIRED)
 
-find_package(OpenCV 3 REQUIRED
+# Workaround for OpenCV 3/4 support
+set(_opencv_version 4)
+find_package(OpenCV ${_opencv_version} QUIET COMPONENTS calib3d imgproc)
+if (NOT OpenCV_FOUND)
+  message(STATUS "Did not find OpenCV 4, searching for OpenCV 3")
+  set(_opencv_version 3)
+endif()
+
+find_package(OpenCV ${_opencv_version} REQUIRED
   COMPONENTS
     calib3d
     imgproc
@@ -254,6 +262,10 @@ target_link_libraries(${PROJECT_NAME}
 #   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 # )
 
+catkin_install_python(PROGRAMS src/selfcheck.py
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
 # Only install udev rules when building a Debian package
 # FIXME: Other operating systems may have other prefixes
 string(FIND ${CMAKE_INSTALL_PREFIX} "/opt/ros" _PREFIX_INDEX)

clover/README.md

@@ -4,7 +4,7 @@ A bundle for autonomous navigation and drone control.
 
 ## Manual installation
 
-Install ROS Melodic according to the [documentation](http://wiki.ros.org/melodic/Installation), then [create a Catkin workspace](http://wiki.ros.org/catkin/Tutorials/create_a_workspace).
+Install ROS Noetic according to the [documentation](http://wiki.ros.org/noetic/Installation), then [create a Catkin workspace](http://wiki.ros.org/catkin/Tutorials/create_a_workspace).
 
 Clone this repo to directory `~/catkin_ws/src/clover`:
 

clover/launch/aruco.launch

@@ -2,30 +2,37 @@
     <arg name="aruco_detect" default="true"/>
     <arg name="aruco_map" default="false"/>
     <arg name="aruco_vpe" default="false"/>
+    <arg name="placement" default="floor"/> <!-- markers placement: floor, ceiling, unknown -->
+    <arg name="length" default="0.33"/> <!-- not-in-map markers length, m -->
+    <arg name="map" default="map.txt"/> <!-- markers map file name -->
 
     <!-- For additional help go to https://clover.coex.tech/aruco -->
 
     <!-- aruco_detect: detect aruco markers, estimate poses -->
-    <node name="aruco_detect" pkg="nodelet" if="$(arg aruco_detect)" type="nodelet" args="load aruco_pose/aruco_detect nodelet_manager" output="screen" clear_params="true">
+    <node name="aruco_detect" pkg="nodelet" if="$(arg aruco_detect)" type="nodelet" args="load aruco_pose/aruco_detect main_camera_nodelet_manager" output="screen" clear_params="true" respawn="true">
         <remap from="image_raw" to="main_camera/image_raw"/>
         <remap from="camera_info" to="main_camera/camera_info"/>
         <remap from="map_markers" to="aruco_map/markers" if="$(arg aruco_map)"/>
         <param name="estimate_poses" value="true"/>
         <param name="send_tf" value="true"/>
-        <param name="known_tilt" value="map"/>
-        <param name="length" value="0.33"/>
+        <param name="known_tilt" value="map" if="$(eval placement == 'floor')"/>
+        <param name="known_tilt" value="map_flipped" if="$(eval placement == 'ceiling')"/>
+        <param name="length" value="$(arg length)"/>
         <!-- aruco detector parameters -->
         <param name="cornerRefinementMethod" value="2"/> <!-- contour refinement -->
         <param name="minMarkerPerimeterRate" value="0.075"/> <!-- 0.075 for 320x240, 0.0375 for 640x480 -->
+        <!-- length override example: -->
+        <!-- <param name="length_override/3" value="0.1"/> -->
     </node>
 
     <!-- aruco_map: estimate aruco map pose -->
-    <node name="aruco_map" pkg="nodelet" type="nodelet" if="$(arg aruco_map)" args="load aruco_pose/aruco_map nodelet_manager" output="screen" clear_params="true">
+    <node name="aruco_map" pkg="nodelet" type="nodelet" if="$(arg aruco_map)" args="load aruco_pose/aruco_map main_camera_nodelet_manager" output="screen" clear_params="true" respawn="true">
         <remap from="image_raw" to="main_camera/image_raw"/>
         <remap from="camera_info" to="main_camera/camera_info"/>
         <remap from="markers" to="aruco_detect/markers"/>
-        <param name="map" value="$(find aruco_pose)/map/map.txt"/>
-        <param name="known_tilt" value="map"/>
+        <param name="map" value="$(find aruco_pose)/map/$(arg map)"/>
+        <param name="known_tilt" value="map" if="$(eval placement == 'floor')"/>
+        <param name="known_tilt" value="map_flipped" if="$(eval placement == 'ceiling')"/>
         <param name="image_axis" value="true"/>
         <param name="frame_id" value="aruco_map_detected" if="$(arg aruco_vpe)"/>
         <param name="frame_id" value="aruco_map" unless="$(arg aruco_vpe)"/>

clover/launch/clover.launch

@@ -11,8 +11,7 @@
     <arg name="rangefinder_vl53l1x" default="true"/>
     <arg name="led" default="true"/>
     <arg name="blocks" default="false"/>
-    <arg name="rc" default="true"/>
-    <arg name="shell" default="true"/>
+    <arg name="rc" default="false"/>
 
     <arg name="simulator" default="false"/> <!-- flag that we are operating on a simulated drone -->
 
@@ -37,18 +36,13 @@
     <include file="$(find clover)/launch/aruco.launch" if="$(arg aruco)"/>
 
     <!-- optical flow -->
-    <node pkg="nodelet" type="nodelet" name="optical_flow" args="load clover/optical_flow nodelet_manager" if="$(arg optical_flow)" clear_params="true" output="screen">
+    <node pkg="nodelet" type="nodelet" name="optical_flow" args="load clover/optical_flow main_camera_nodelet_manager" if="$(arg optical_flow)" clear_params="true" output="screen" respawn="true">
         <remap from="image_raw" to="main_camera/image_raw"/>
         <remap from="camera_info" to="main_camera/camera_info"/>
         <param name="calc_flow_gyro" value="true"/>
         <param name="roi_rad" value="0.8"/>
     </node>
 
-    <!-- main nodelet manager -->
-    <node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager" output="screen" clear_params="true">
-        <param name="num_worker_threads" value="2"/>
-    </node>
-
     <node pkg="tf2_ros" type="static_transform_publisher" name="map_flipped_frame" args="0 0 0 3.1415926 3.1415926 0 map map_flipped"/>
 
     <!-- simplified offboard control -->
@@ -91,9 +85,6 @@
         <param name="use_fake_gcs" value="false"/>
     </node>
 
-    <!-- Shell access through ROS service -->
-    <node name="shell" pkg="clover" type="shell" output="screen" if="$(arg shell)"/>
-
     <!-- Update static directory -->
     <node pkg="roswww_static" name="roswww_static" type="main.py" clear_params="true">
         <param name="default_package" value="clover"/>

clover/launch/led.launch

@@ -36,7 +36,7 @@
             posctl: { r: 50, g: 100, b: 220 }
             offboard: { r: 220, g: 20, b: 250 }
             low_battery: { threshold: 3.6, effect: blink_fast, r: 255, g: 0, b: 0 }
-            error: { effect: flash, r: 255, g: 0, b: 0 }
+            error: { effect: flash, r: 255, g: 0, b: 0, ignore: [ "[lpe] vision position timeout" ]}
         </rosparam>
     </node>
 </launch>

clover/launch/main_camera.launch

@@ -3,6 +3,7 @@
 
     <arg name="direction_z" default="down"/> <!-- direction the camera points: down, up -->
     <arg name="direction_y" default="backward"/> <!-- direction the camera cable points: backward, forward -->
+    <arg name="device" default="/dev/video0"/> <!-- v4l2 device -->
     <arg name="simulator" default="false"/>
 
     <node if="$(eval direction_z == 'down' and direction_y == 'backward')" pkg="tf2_ros" type="static_transform_publisher" name="main_camera_frame" args="0.05 0 -0.07 -1.5707963 0 3.1415926 base_link main_camera_optical"/>
@@ -17,9 +18,14 @@
     <!-- static_transform_publisher arguments: x y z yaw pitch roll frame_id child_frame_id -->
     <!-- <node pkg="tf2_ros" type="static_transform_publisher" name="main_camera_frame" args="0.05 0 -0.07 -1.5707963 0 3.1415926 base_link main_camera_optical"/> -->
 
+    <!-- camera nodelet manager -->
+    <node pkg="nodelet" type="nodelet" name="main_camera_nodelet_manager" args="manager" output="screen" clear_params="true" respawn="true">
+        <param name="num_worker_threads" value="2"/>
+    </node>
+
     <!-- camera node -->
-    <node pkg="nodelet" type="nodelet" name="main_camera" args="load cv_camera/CvCameraNodelet nodelet_manager" clear_params="true" unless="$(arg simulator)">
-        <param name="device_path" value="/dev/video0"/> <!-- v4l2 device -->
+    <node pkg="nodelet" type="nodelet" name="main_camera" args="load cv_camera/CvCameraNodelet main_camera_nodelet_manager" launch-prefix="rosrun clover waitfile $(arg device)" clear_params="true" unless="$(arg simulator)" respawn="true">
+        <param name="device_path" value="$(arg device)"/>
         <param name="frame_id" value="main_camera_optical"/>
         <param name="camera_info_url" value="file://$(find clover)/camera_info/fisheye_cam.yaml"/>
 

clover/launch/mavros.launch

@@ -6,13 +6,16 @@
     <arg name="viz" default="true"/>
     <arg name="respawn" default="true"/>
     <arg name="distance_sensor_remap" default="rangefinder/range"/>
+    <arg name="usb_device" default="/dev/px4fmu"/>
+    <arg name="prefix" default="" unless="$(eval fcu_conn == 'usb')"/>
+    <arg name="prefix" default="rosrun clover waitfile $(arg usb_device)" if="$(eval fcu_conn == 'usb')"/>
 
-    <node pkg="mavros" type="mavros_node" name="mavros" required="false" clear_params="true" respawn="$(arg respawn)" unless="$(eval fcu_conn == 'none')" respawn_delay="1" output="screen">
+    <node pkg="mavros" type="mavros_node" name="mavros" launch-prefix="$(arg prefix)" required="false" clear_params="true" respawn="$(arg respawn)" unless="$(eval fcu_conn == 'none')" respawn_delay="1" output="screen">
         <!-- UART connection -->
         <param name="fcu_url" value="/dev/ttyAMA0:921600" if="$(eval fcu_conn is None or fcu_conn == 'uart')"/>
 
         <!-- USB connection -->
-        <param name="fcu_url" value="/dev/px4fmu" if="$(eval fcu_conn == 'usb')"/>
+        <param name="fcu_url" value="$(arg usb_device)" if="$(eval fcu_conn == 'usb')"/>
 
         <!-- sitl before PX4 1.9.0 -->
         <param name="fcu_url" value="udp://@$(arg fcu_ip):14557" if="$(eval fcu_conn == 'udp')"/>

clover/package.xml

@@ -1,5 +1,5 @@
 <?xml version="1.0"?>
-<package format="2">
+<package format="3">
   <name>clover</name>
   <version>0.21.1</version>
   <description>The Clover package</description>
@@ -37,7 +37,8 @@
   <depend>rosbridge_server</depend>
   <depend>web_video_server</depend>
   <depend>tf2_web_republisher</depend>
-  <depend>python-lxml</depend>
+  <depend condition="$ROS_PYTHON_VERSION == 2">python-lxml</depend>
+  <depend condition="$ROS_PYTHON_VERSION == 3">python3-lxml</depend>
   <exec_depend>python-pymavlink</exec_depend>
   <!-- Use test_depend for packages you need only for testing: -->
   <!--   <test_depend>gtest</test_depend> -->

clover/src/led.cpp

@@ -12,6 +12,7 @@
 
 #include <ros/ros.h>
 #include <string>
+#include <vector>
 #include <boost/algorithm/string.hpp>
 
 #include <clover/SetLEDEffect.h>
@@ -29,6 +30,7 @@ ros::Timer timer;
 ros::Time start_time;
 double blink_rate, blink_fast_rate, flash_delay, fade_period, wipe_period, rainbow_period;
 double low_battery_threshold;
+std::vector<std::string> error_ignore;
 bool blink_state;
 led_msgs::SetLEDs set_leds;
 led_msgs::LEDStateArray state, start_state;
@@ -274,6 +276,10 @@ void handleMavrosState(const mavros_msgs::State& msg)
 void handleLog(const rosgraph_msgs::Log& log)
 {
 	if (log.level >= rosgraph_msgs::Log::ERROR) {
+		// check if ignored
+		for (auto const& str : error_ignore) {
+			if (log.msg.find(str) != std::string::npos) return;
+		}
 		notify("error");
 	}
 }
@@ -302,6 +308,7 @@ int main(int argc, char **argv)
 	nh_priv.param("rainbow_period", rainbow_period, 5.0);
 
 	nh_priv.param("notify/low_battery/threshold", low_battery_threshold, 3.7);
+	nh_priv.param("notify/error/ignore", error_ignore, {});
 
 	ros::service::waitForService("set_leds"); // cannot work without set_leds service
 	set_leds_srv = nh.serviceClient<led_msgs::SetLEDs>("set_leds", true);

clover/src/optical_flow.cpp

@@ -70,7 +70,6 @@ private:
 		roi_rad_ = nh_priv.param("roi_rad", 0.0);
 		calc_flow_gyro_ = nh_priv.param("calc_flow_gyro", false);
 
-		img_sub_ = it.subscribeCamera("image_raw", 1, &OpticalFlow::flow, this);
 		img_pub_ = it_priv.advertise("debug", 1);
 		flow_pub_ = nh.advertise<mavros_msgs::OpticalFlowRad>("mavros/px4flow/raw/send", 1);
 		velo_pub_ = nh_priv.advertise<geometry_msgs::TwistStamped>("angular_velocity", 1);
@@ -83,6 +82,8 @@ private:
 		flow_.distance = -1; // no distance sensor available
 		flow_.temperature = 0;
 
+		img_sub_ = it.subscribeCamera("image_raw", 1, &OpticalFlow::flow, this);
+
 		NODELET_INFO("Optical Flow initialized");
 	}
 
@@ -152,7 +153,7 @@ private:
 			cv::Point2d shift = cv::phaseCorrelate(prev_, curr_, hann_, &response);
 
 			// Publish raw shift in pixels
-			static geometry_msgs::Vector3Stamped shift_vec;
+			geometry_msgs::Vector3Stamped shift_vec;
 			shift_vec.header.stamp = msg->header.stamp;
 			shift_vec.header.frame_id = msg->header.frame_id;
 			shift_vec.vector.x = shift.x;
@@ -179,7 +180,7 @@ private:
 			double flow_y = atan2(points_undist[0].y, focal_length_y);
 
 			// // Convert to FCU frame
-			static geometry_msgs::Vector3Stamped flow_camera, flow_fcu;
+			geometry_msgs::Vector3Stamped flow_camera, flow_fcu;
 			flow_camera.header.frame_id = msg->header.frame_id;
 			flow_camera.header.stamp = msg->header.stamp;
 			flow_camera.vector.x = flow_y; // +y means counter-clockwise rotation around Y axis
@@ -198,7 +199,7 @@ private:
 			if (calc_flow_gyro_) {
 				try {
 					auto flow_gyro_camera = calcFlowGyro(msg->header.frame_id, prev_stamp_, msg->header.stamp);
-					static geometry_msgs::Vector3Stamped flow_gyro_fcu;
+					geometry_msgs::Vector3Stamped flow_gyro_fcu;
 					tf_buffer_->transform(flow_gyro_camera, flow_gyro_fcu, fcu_frame_id_);
 					flow_.integrated_xgyro = flow_gyro_fcu.vector.x;
 					flow_.integrated_ygyro = flow_gyro_fcu.vector.y;
@@ -206,7 +207,7 @@ private:
 				} catch (const tf2::TransformException& e) {
 					// Invalidate previous frame
 					prev_.release();
-					return;
+					goto publish_debug;
 				}
 			}
 
@@ -218,6 +219,10 @@ private:
 			flow_.quality = (uint8_t)(response * 255);
 			flow_pub_.publish(flow_);
 
+			prev_ = curr_.clone();
+			prev_stamp_ = msg->header.stamp;
+
+publish_debug:
 			// Publish debug image
 			if (img_pub_.getNumSubscribers() > 0) {
 				// publish debug image
@@ -231,15 +236,12 @@ private:
 			}
 
 			// Publish estimated angular velocity
-			static geometry_msgs::TwistStamped velo;
+			geometry_msgs::TwistStamped velo;
 			velo.header.stamp = msg->header.stamp;
 			velo.header.frame_id = fcu_frame_id_;
-			velo.twist.angular.x = flow_.integrated_x / integration_time.toSec();
-			velo.twist.angular.y = flow_.integrated_y / integration_time.toSec();
+			velo.twist.angular.x = flow_fcu.vector.x / integration_time.toSec();
+			velo.twist.angular.y = flow_fcu.vector.y / integration_time.toSec();
 			velo_pub_.publish(velo);
-
-			prev_ = curr_.clone();
-			prev_stamp_ = msg->header.stamp;
 		}
 	}
 

clover/src/selfcheck.py

@@ -138,7 +138,7 @@ def mavlink_exec(cmd, timeout=3.0):
         timeout=3,
         baudrate=0,
         count=len(cmd),
-        data=map(ord, cmd.ljust(70, '\0')))
+        data=[ord(c) for c in cmd.ljust(70, '\0')])
     msg.pack(link)
     ros_msg = mavlink.convert_to_rosmsg(msg)
     mavlink_pub.publish(ros_msg)
@@ -609,7 +609,7 @@ def check_rangefinder():
 
 @check('Boot duration')
 def check_boot_duration():
-    output = subprocess.check_output('systemd-analyze')
+    output = subprocess.check_output('systemd-analyze').decode()
     r = re.compile(r'([\d\.]+)s\s*$', flags=re.MULTILINE)
     duration = float(r.search(output).groups()[0])
     if duration > 15:
@@ -620,7 +620,7 @@ def check_boot_duration():
 def check_cpu_usage():
     WHITELIST = 'nodelet',
     CMD = "top -n 1 -b -i | tail -n +8 | awk '{ printf(\"%-8s\\t%-8s\\t%-8s\\n\", $1, $9, $12); }'"
-    output = subprocess.check_output(CMD, shell=True)
+    output = subprocess.check_output(CMD, shell=True).decode()
     processes = output.split('\n')
     for process in processes:
         if not process:
@@ -636,7 +636,7 @@ def check_cpu_usage():
 def check_clover_service():
     try:
         output = subprocess.check_output('systemctl show -p ActiveState --value clover.service'.split(),
-                                         stderr=subprocess.STDOUT)
+                                         stderr=subprocess.STDOUT).decode()
     except subprocess.CalledProcessError as e:
         failure('systemctl returned %s: %s', e.returncode, e.output)
         return
@@ -751,7 +751,7 @@ def check_rpi_health():
         # <parameter>=<value>
         # In case of `get_throttled`, <value> is a hexadecimal number
         # with some of the FLAGs OR'ed together
-        output = subprocess.check_output(['vcgencmd', 'get_throttled'])
+        output = subprocess.check_output(['vcgencmd', 'get_throttled']).decode()
     except OSError:
         failure('could not call vcgencmd binary; not a Raspberry Pi?')
         return

clover/src/simple_offboard.cpp

@@ -712,7 +712,7 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl
 		}
 
 		if (sp_type == VELOCITY) {
-			static Vector3Stamped vel;
+			Vector3Stamped vel;
 			vel.header.frame_id = frame_id;
 			vel.header.stamp = stamp;
 			vel.vector.x = vx;

clover/src/vpe_publisher.cpp

@@ -53,7 +53,7 @@ void publishZero(const ros::TimerEvent& e)
 	}
 
 	ROS_INFO_THROTTLE(10, "publish zero");
-	static geometry_msgs::PoseStamped zero;
+	geometry_msgs::PoseStamped zero;
 	zero.header.frame_id = local_frame_id;
 	zero.header.stamp = e.current_real;
 	zero.pose.orientation.w = 1;

clover/src/waitfile

@@ -0,0 +1,9 @@
+#!/usr/bin/env bash
+
+# $ ./waitfile <file> <command> <args...> 
+# wait until <file> appears and then invoke <command> with <args>
+
+echo "wait for file $1"
+while [ ! -e "$1" ]; do sleep 1; done;
+echo "file $1 appeared"
+exec "${@:2}"

clover/test/basic.py

@@ -26,21 +26,10 @@ def test_simple_offboard_services_available():
     rospy.wait_for_service('land', timeout=5)
 
 def test_web_video_server(node):
-    import urllib2
-    urllib2.urlopen("http://localhost:8080").read()
-
-def test_shell(node):
-    execute = rospy.ServiceProxy('exec', srv.Execute)
-    execute.wait_for_service(5)
-
-    res = execute(cmd='echo foo')
-    assert res.code == 0
-    assert res.output == 'foo\n'
-
-    res = execute(cmd='foo')
-    assert res.code == 32512
-    assert res.output == ''
-
-    res = execute(cmd='ls foo')
-    assert res.code == 512
-    assert res.output == ''
+    try:
+        # Python 2
+        import urllib2 as urllib
+    except ModuleNotFoundError:
+        # Python 3
+        import urllib.request as urllib
+    urllib.urlopen("http://localhost:8080").read()

clover/www/clover.err

@@ -0,0 +1 @@
+/tmp/clover.err

clover/www/clover_version

@@ -0,0 +1 @@
+/etc/clover_version

clover/www/index.html

@@ -9,19 +9,20 @@
 	<li><a href="viz.html">View 3D visualization</a> (<code>ros3djs</code>)</li>
 	<li><a href="aruco_map.html">3D visualization for markers map</a> (<code>ros3djs</code>)</li>
 	<li><a href="../clover_blocks/">Blocks programming</a> (<code>Blockly</code>)</li>
+	<li><a href="clover.err">Clover console</a> (<code>/tmp/clover.err</code>)</li>
 </ul>
 
 <div class="version"></div>
 
-<script src="js/roslib.js"></script>
 <script type="text/javascript">
 	document.querySelector("#wvs").href = location.protocol + '//' + location.hostname + ':8080';
 	document.querySelector("#butterfly").href = location.protocol + '//' + location.hostname + ':57575';
 
 	// Determine image version
-	var ros = new ROSLIB.Ros({ url: 'ws://' + location.hostname + ':9090' });
-	var exec = new ROSLIB.Service({ ros: ros, name : '/exec', serviceType : 'clover/Execute' });
-	exec.callService(new ROSLIB.ServiceRequest({ cmd: 'cat /etc/clover_version' }), function(result) {
-		document.querySelector('.version').innerHTML = 'Version: ' + result.output;
+	fetch('clover_version').then(function(response) {
+		if (response.status !== 200) return;
+		response.text().then(function(text) {
+			document.querySelector('.version').innerHTML = 'Version: ' + text;
+		});
 	});
 </script>

clover_blocks/src/clover_blocks

@@ -11,7 +11,8 @@
 from __future__ import print_function
 
 import rospy
-import os
+import os, sys
+import traceback
 import threading
 import re
 import uuid
@@ -111,12 +112,17 @@ def run(req):
                 'print': _print,
                 'raw_input': _input}
             try:
-                exec req.code in g
+                exec(req.code, g)
             except Stop:
                 rospy.loginfo('Program forced to stop')
             except Exception as e:
                 rospy.logerr(str(e))
-                error_pub.publish(str(e))
+                traceback.print_exc()
+                etype, value, tb = sys.exc_info()
+                fmt = traceback.format_exception(etype, value, tb)
+                fmt.pop(1) # remove 'clover_blocks' file frame
+                exc_info = ''.join(fmt)
+                error_pub.publish(str(e) + '\n\n' + exc_info)
 
             rospy.loginfo('Program terminated')
             running_lock.release()

clover_blocks/www/python.js

@@ -391,7 +391,7 @@ Blockly.Python.set_led = function(block) {
 
 	if (/^'(.*)'$/.test(colorCode)) { // is simple string
 		let color = parseColor(colorCode);
-		return `set_leds([LEDState(index=${index}, r=${color.r}, g=${color.g}, b=${color.b})])\n`;
+		return `set_leds([LEDState(index=int(${index}), r=${color.r}, g=${color.g}, b=${color.b})])\n`; // TODO: check for simple int
 	} else {
 		let parseColor = Blockly.Python.provideFunction_('parse_color', [PARSE_COLOR]);
 		return `set_leds([LEDState(index=${index}, **${parseColor}(${colorCode}))])\n`;

clover_simulation/CMakeLists.txt

@@ -52,7 +52,7 @@ target_compile_options(throttling_camera PRIVATE -std=c++11)
 add_dependencies(throttling_camera ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
 install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
-install(DIRECTORY meshes DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+install(DIRECTORY models DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
 install(DIRECTORY resources DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
 
 catkin_install_python(PROGRAMS scripts/aruco_gen

clover_simulation/src/clover_simulation/__init__.py

@@ -88,6 +88,6 @@ def aruco_gen():
                     off_x + marker.x, off_y + marker.y, off_z + marker.z,
                     marker.roll, marker.pitch, marker.yaw)
 
-        output = open(source_world, 'w') if inplace else stdout
+        output = open(source_world, 'wb') if inplace else stdout
 
         save_world(world_tree, output)

docs/en/SUMMARY.md

@@ -91,7 +91,9 @@
   * [Soldering safety](tb.md)
   * [LED strip (legacy)](leds_old.md)
   * [Contribution Guidelines](contributing.md)
+  * [COEX packages repository](packages.md)
   * [Migration to v0.20](migrate20.md)
+  * [Migration to v0.22](migrate22.md)
 * [Events](events.md)
   * [CopterHack-2021](copterhack2021.md)
   * [CopterHack-2019](copterhack2019.md)

docs/en/arduino.md

@@ -66,7 +66,7 @@ The set of services and topics is similar to the regular set in [simple_offboard
 An example of a program that controls the copter by position using the `navigate` and `set_mode` services:
 
 ```cpp
-// Connecting libraries for working with rosseral
+// Connecting libraries for working with rosserial
 #include <ros.h>
 
 // Connecting Clover and MAVROS package message header files

docs/en/aruco_map.md

@@ -1,5 +1,9 @@
 # Map-based navigation with ArUco markers
 
+> **Note** The following applies to [image versions](image.md) **0.22** and up. Older documentation is still available for [for version **0.20**](https://github.com/CopterExpress/clover/blob/v0.20/docs/en/aruco_map.md).
+
+<!-- -->
+
 > **Info** Marker detection requires the camera module to be correctly plugged in and [configured](camera_setup.md).
 
 <!-- -->
@@ -39,18 +43,19 @@ marker_id marker_size x y z z_angle y_angle x_angle
 
 `N_angle` is the angle of rotation along the `N` axis in radians.
 
-Map path is defined in the `map` parameter:
+Файлы карт располагаются в каталоге `~/catkin_ws/src/clover/aruco_pose/map`. Название файла с картой задается в аргументе `map`:
+Map files are located at the `~/catkin_ws/src/clover/aruco_pose/map` directory. Map file name is defined in the `map` argument:
 
 ```xml
-<param name="map" value="$(find aruco_pose)/map/map.txt"/>
+<arg name="map" default="map.txt"/>
 ```
 
-Some map examples are provided in [`~/catkin_ws/src/clover/aruco_pose/map`](https://github.com/CopterExpress/clover/tree/master/aruco_pose/map).
+Some map examples are provided in [the directory](https://github.com/CopterExpress/clover/tree/master/aruco_pose/map).
 
 Grid maps may be generated using the `genmap.py` script:
 
 ```bash
-rosrun aruco_pose genmap.py length x y dist_x dist_y first > ~/catkin_ws/src/clover/aruco_pose/map/test_map.txt
+rosrun aruco_pose genmap.py length x y dist_x dist_y first -o test_map.txt
 ```
 
 `length` is the size of each marker, `x` is the marker count along the *x* axis, `y` is the marker count along the *y* axis, `dist_x` is the distance between the centers of adjacent markers along the *x* axis, `dist_y` is the distance between the centers of the *y* axis, `first` is the ID of the first marker (top left marker, unless `--bottom-left` is specified), `test_map.txt` is the name of the generated map file. The optional `--bottom-left` parameter changes the numbering of markers, making the bottom left marker the first one.
@@ -58,7 +63,7 @@ rosrun aruco_pose genmap.py length x y dist_x dist_y first > ~/catkin_ws/src/clo
 Usage example:
 
 ```bash
-rosrun aruco_pose genmap.py 0.33 2 4 1 1 0 > ~/catkin_ws/src/clover/aruco_pose/map/test_map.txt
+rosrun aruco_pose genmap.py 0.33 2 4 1 1 0 -o test_map.txt
 ```
 
 Additional information on the utility can be obtained using `-h` key: `rosrun aruco_pose genmap.py -h`.
@@ -134,7 +139,7 @@ navigate(x=2, y=2, z=2, speed=1, frame_id='aruco_map')
 
 Starting with the [image](image.md) version 0.18, the drone also can fly relative to a marker in the map, even if it is not currently visible. Like with [single-marker navigation](aruco_marker.md#working-with-detected-markers), this works by setting the frame_id parameter to aruco_ID, where ID is the desired marker number.
 
-The folloding code will move the drone to the point 1 meter above the center of marker 5:
+The following code will move the drone to the point 1 meter above the center of marker 5:
 
 ```python
 navigate(frame_id='aruco_5', x=0, y=0, z=1)
@@ -152,10 +157,10 @@ If the drone's altitude is not stable, try increasing the `MPC_Z_VEL_P` paramete
 
 In order to navigate using markers on the ceiling, mount the onboard camera so that it points up and [adjust the camera frame accordingly](camera_setup.md).
 
-You should also set the `known_tilt` parameter to `map_flipped` in both `aruco_detect` and `aruco_map` sections of `~/catkin_ws/src/clover/clover/launch/aruco.launch`:
+You should also set the `placement` parameter to `ceilin` in `~/catkin_ws/src/clover/clover/launch/aruco.launch`:
 
 ```xml
-<param name="known_tilt" value="map_flipped"/>
+<arg name="placement" default="ceiling"/>
 ```
 
 This will flip the `aruco_map` frame (making its **<font color=blue>z</font>** axis point downward). Thus, in order to fly 2 metres below ceiling, the `z` argument for the `navigate` service should be set to 2:

docs/en/aruco_marker.md

@@ -1,5 +1,9 @@
 # ArUco marker detection
 
+> **Note** The following applies to [image versions](image.md) **0.22** and up. Older documentation is still available for [for version **0.20**](https://github.com/CopterExpress/clover/blob/v0.20/docs/en/aruco_marker.md).
+
+<!-- -->
+
 > **Info** Marker detection requires the camera module to be correctly plugged in and [configured](camera.md).
 
 `aruco_detect` module detects ArUco markers and publishes their positions in ROS topics and as [TF frames](frames.md).
@@ -22,22 +26,20 @@ For enabling detection set the `aruco_detect` argument in `~/catkin_ws/src/clove
 <arg name="aruco_detect" default="true"/>
 ```
 
-For the module to work correctly the following parameters should be set:
+For the module to work correctly the following arguments should also be set:
 
 ```xml
-<param name="length" value="0.32"/>          <!-- length of a single marker, in meters (excluding the white border) -->
-<param name="estimate_poses" value="true"/>  <!-- position estimation for single markers -->
-<param name="send_tf" value="true"/>         <!-- TF frame creation for markers -->
-<param name="known_tilt" value="map"/>       <!-- Marker tilt, explained below -->
+<arg name="placement" default="floor"/> <!-- markers' placement, explained below  -->
+<arg name="length" default="0.33"/>     <!-- length of a single marker, in meters (excluding the white border) -->
 ```
 
-`known_tilt` should be set to:
+`placement` argument should be set to:
 
-* `map` if *all* markers are on the ground;
-* `map_flipped` if *all* markers are on the ceiling;
+* `floor` if *all* markers are on the ground;
+* `ceiling` if *all* markers are on the ceiling;
 * an empty string otherwise.
 
-You may specify length for each marker individually by using the `length_override` parameter:
+You may specify length for each marker individually by using the `length_override` parameter of the node `aruco_detect`:
 
 ```xml
 <param name="length_override/3" value="0.1"/>    <!-- marker with id=3 has a side of 0.1m -->
@@ -98,9 +100,9 @@ rospy.init_node('my_node')
 # ...
 
 def markers_callback(msg):
-    print 'Detected markers:':
+    print('Detected markers:'):
     for marker in msg.markers:
-        print 'Marker: %s' % marker
+        print('Marker: %s' % marker)
 
 # Create a Subscription object. Each time a message is posted in aruco_detect/markers, the markers_callback function is called with this message as its argument.
 rospy.Subscriber('aruco_detect/markers', MarkerArray, markers_callback)

docs/en/auto_setup.md

@@ -126,7 +126,7 @@ Ctrl+C
 Start a program `myprogram.py` using Python:
 
 ```bash
-python myprogram.py
+python3 myprogram.py
 ```
 
 Journal of the events related to `clover` package. Scroll the list by pressing Enter or Ctrl+V (scrolls faster):
@@ -411,7 +411,7 @@ The easiest way to send the program is to copy the content of the program, creat
 - Run the program:
 
   ```bash
-  python my_program.py
+  python3 my_program.py
   ```
 
   > **Warning** After completion of the program , the drone can land incorrectly and continue to fly over the floor. In this case, you need to intercept control.

docs/en/camera.md

@@ -133,12 +133,12 @@ def image_callback(data):
     cv_image = bridge.imgmsg_to_cv2(data, 'bgr8')  # OpenCV image
     barcodes = pyzbar.decode(cv_image)
     for barcode in barcodes:
-        b_data = barcode.data.encode("utf-8")
+        b_data = barcode.data.decode("utf-8")
         b_type = barcode.type
         (x, y, w, h) = barcode.rect
         xc = x + w/2
         yc = y + h/2
-        print ("Found {} with data {} with center at x={}, y={}".format(b_type, b_data, xc, yc))
+        print("Found {} with data {} with center at x={}, y={}".format(b_type, b_data, xc, yc))
 
 image_sub = rospy.Subscriber('main_camera/image_raw', Image, image_callback, queue_size=1)
 
@@ -153,3 +153,13 @@ The script will take up to 100% CPU capacity. To slow down the script artificial
 ```
 
 The topic for the subscriber in this case should be changed for `main_camera/image_raw_throttled`.
+
+## Video recording
+
+To record a video you can use [`video_recorder`](http://wiki.ros.org/image_view#image_view.2Fdiamondback.video_recorder) node from `image_view` package:
+
+```bash
+rosrun image_view video_recorder image:=/main_camera/image_raw
+```
+
+The video file will be saved to a file `output.avi`. The `image` argument contains the name of the topic to record.

docs/en/cli.md

@@ -39,7 +39,7 @@ cat file.py
 Run `file.py` as a Python script:
 
 ```bash
-python file.py
+python3 file.py
 ```
 
 Reboot Raspberry Pi:

docs/en/coex_gps.md

@@ -2,6 +2,8 @@
 
 The GNSS receiver **COEX GPS** is compatible with the [COEX Pix](coex_pix.md) flight controller. This receiver comes with a COEX Clover Drone Kit.
 
+> **Hint** The source files of the COEX GPS board are [published](https://github.com/CopterExpress/hardware/tree/master/COEX%20GPS) under the CC BY-NC-SA license.
+
 ## Port pinouts
 
 ### Top view

docs/en/coex_pdb.md

@@ -4,6 +4,8 @@
 
 Board size: 35x35 mm.
 
+> **Hint** The source files of the COEX PDB board are [published](https://github.com/CopterExpress/hardware/tree/master/COEX%20PDB) under the CC BY-NC-SA license.
+
 ## Port pinouts
 
 ### Top view

docs/en/coex_pix.md

@@ -2,6 +2,8 @@
 
 The **COEX Pix** flight controller is a modified [Pixracer](https://docs.px4.io/v1.9.0/en/flight_controller/pixracer.html) FCU. It is a part of the **Clover 4** quadrotor kit.
 
+> **Hint** The source files of the COEX Pix flight controller are [published](https://github.com/CopterExpress/hardware/tree/master/COEX%20Pix) under the CC BY-NC-SA license.
+
 ## Revision 1.1
 
 ### Physical specs

docs/en/contributing.md

@@ -96,3 +96,7 @@ Prepare your article and send it as a pull request to the [Clover repository](ht
 ## Easy way
 
 If the above instructions are too difficult for you, send your fixes and new articles by e-mail (<a href="mailto:okalachev@gmail.com">okalachev@gmail.com</a>) or in Telegram messenger (user <a href="tg://resolve?domain=okalachev">@okalachev</a>).
+
+## Publishing packages
+
+You also can publish a package, that extends Clover functionality, into the official [COEX Debian repository](packages.md).

docs/en/copterhack2021.md

@@ -1,6 +1,6 @@
 # CopterHack 2021
 
-CopterHack 2021 is a team competition for the development of open source projects for the Clover quadcopter platform.
+CopterHack 2021 is a team competition for the development of open source projects for the Clover quadcopter platform. Fifty-four teams from 12 countries took part in the competition.
 
 All information about the event can be found on the official website: https://coex.tech/copterhack.
 

docs/en/events.md

@@ -1,5 +1,5 @@
 # Events
 
-Clover is being used in a lot of educational events and competitions, such as WorldSkills, NTI Olypics, Copter Hack, Innopolis Open Robotics, etc.
+Clover is being used in a lot of educational events and competitions, such as WorldSkills, NTI Olympics, Copter Hack, Innopolis Open Robotics, etc.
 
 This section contains articles written specifically for a particular event.

docs/en/fpv_clover_4_2.md

@@ -34,7 +34,7 @@
         <img src="../assets/fpv/fpv_9.png" width=300 class="zoom border">
     </div>
 
-    > **Hint** Сheck what you are wearing shrink tubes before soldering the wires.
+    > **Hint** Check what you are wearing shrink tubes before soldering the wires.
 
 6. Solder the JST male connector to the transmitter.
 

docs/en/laser.md

@@ -59,7 +59,7 @@ rospy.init_node('flight')
 
 def range_callback(msg):
     # Process data from the rangefinder
-    print 'Rangefinder distance:', msg.range
+    print('Rangefinder distance:', msg.range)
 
 rospy.Subscriber('rangefinder/range', Range, range_callback)
 

docs/en/leds_old.md

@@ -98,7 +98,7 @@ Main strip control methods:
 
 + `numPixels()` returns the number of pixels in the strip. Convenient for whole strip operations.
 + `setPixelColor(pos, color)` sets the pixel color at `pos` to `color`. Color should be a 24-bit value, where the first 8 bits are for the red channel, the next 8 bits are for the green channel, and the last 8 bits are for the blue channel. You may use the `Color(red, green, blue)` convenience function to convert colors to this format. Each color value should be an integer in the \[0..255\] range, where 0 means zero brightness and 255 means full brightness.
-+ `SetPixelColorRGB(pos, red, green, blue)` sets the pixel at `pos` to the color value with components `red`, `green` and `blue`. Each component value shoule be an integer in the \[0..255\] range, where 0 means zero brighness and 255 means full brightness.
++ `SetPixelColorRGB(pos, red, green, blue)` sets the pixel at `pos` to the color value with components `red`, `green` and `blue`. Each component value should be an integer in the \[0..255\] range, where 0 means zero brightness and 255 means full brightness.
 + `show()` updates the strip state. Any changes to the strip state are only pushed to the actual strip after calling this method.
 
 ## Does it have to be this way?

docs/en/migrate20.md

@@ -70,56 +70,6 @@ The `~/catkin_ws/src/clever/` directory is renamed to `~/catkin_ws/src/clover`.
 
 For example, `~/catkin_ws/src/clever/clever/launch/clever.launch` file is now `~/catkin_ws/src/clover/clover/launch/clover.launch`.
 
-<!--
-## Python 3 transition
-
-Python 2 is depracated since, January 1st, 2020. The Clover platform moves to Python 3.
-
-For running flight script instead of `python` command:
-
-```bash
-python flight.py
-```
-
-use `python3` command:
-
-```bash
-python3 flight.py
-```
-
-Python 3 has certain syntax differences in comparison with the old version. Instead of `print` *operator*:
-
-```python
-print 'Clover is the best'
-```
-
-use `print` *function*:
-
-```python
-print('Clover is the best')
-```
-
-The division operator operates floating points by default (instead of integer). Python 2:
-
-```python
->>> 10 / 4
-2
-```
-
-Python 3:
-
-```python
->>> 10 / 4
-2.5
-```
-
-For strings `unicode` type is used by default (instead of `str` type).
-
-Encoding specification (`# coding: utf8`) is not necessary any more.
-
-More details on all the language changes see in [appropriate article](https://sebastianraschka.com/Articles/2014_python_2_3_key_diff.html).
--->
-
 ## Wi-Fi network configuration
 
 Wi-Fi networks' SSID is changed to `clover-XXXX` (where X is a random number), password is changed to `cloverwifi`.

docs/en/migrate22.md

@@ -0,0 +1,59 @@
+# Migration to version 0.22
+
+## Python 3 transition
+
+Python 2 is [deprecated](https://www.python.org/doc/sunset-python-2/) since January 1st, 2020. The Clover platform moves to Python 3.
+
+For running flight script instead of `python` command:
+
+```bash
+python flight.py
+```
+
+use `python3` command:
+
+```bash
+python3 flight.py
+```
+
+Python 3 has certain syntax differences in comparison with the old version. Instead of `print` *operator*:
+
+```python
+print 'Clover is the best'  # this won't work
+```
+
+use `print` *function*:
+
+```python
+print('Clover is the best')
+```
+
+The division operator operates floating points by default (instead of integer). Python 2:
+
+```python
+>>> 10 / 4
+2
+```
+
+Python 3:
+
+```python
+>>> 10 / 4
+2.5
+```
+
+For strings `unicode` type is used by default (instead of `str` type).
+
+Encoding specification (`# coding: utf8`) is not necessary any more.
+
+More details on all the language changes see in [appropriate article](https://sebastianraschka.com/Articles/2014_python_2_3_key_diff.html).
+
+## Move to ROS Noetic
+
+<img src="../assets/noetic.png" width=200>
+
+ROS Melodic version was updated to ROS Noetic. See the full list of changes in the [ROS official documentation](http://wiki.ros.org/noetic/Migration).
+
+## Changes in launch-files
+
+Configuration of ArUco-markers navigation is simplified. See details in [markers navigation](aruco_marker.md) and [markers map navigation](aruco_map.md) articles.

docs/en/models.md

@@ -96,6 +96,16 @@ This page contains models and drawings of some of the drone parts. They can be u
         </td>
         <td><a href="https://github.com/CopterExpress/clover/raw/master/docs/assets/dxf/4.2/big_leg.dxf"><code>big_leg.dxf</code></a></td>
     </tr>
+    <tr>
+        <td><img src="../assets/dxf/4.2/grip_spacer.png"></td>
+        <td>
+            <b>Grip spacer</b>.<br>
+            Function: spacer for the gripper plates.<br>
+            Material: monolithic polycarbonate 2mm.<br>
+            Quantity: 1 pcs.
+        </td>
+        <td><a href="https://github.com/CopterExpress/clover/raw/master/docs/assets/dxf/4.2/grip_spacer.dxf"><code>grip_spacer.dxf</code></a></td>
+    </tr>
 </table>
 
 ## Clover 4

docs/en/packages.md

@@ -0,0 +1,27 @@
+# COEX packages repository
+
+COEX provides an open [Debian-repository](https://wiki.debian.org/DebianRepository) with ROS Noetic related prebuilt binary pacakges for `armhf` architecture.
+
+> **Info** Repository URL: http://packages.coex.tech.
+
+The repository is already addedd in [RPi image](image.md) and may be used for simple installation of additional ROS packages.
+
+## Packages publishing
+
+You can make a Pull Request in a git repository with packages, adding or updating your package (a file with `.deb` extension), that relates to Clover or ROS. After merging your package will be available for installation with `apt` utility:
+
+```bash
+sudo apt install ros-noetic-clover-some-feature
+```
+
+Packages, that extend Clover functionality are recommended to be named with `clover_` prefix, e. g. `clover_some_feature`.
+
+## Using on a normal Raspberry Pi OS
+
+On a normal Raspberry Pi OS, the repository may be added to the sources list, this way:
+
+```bash
+wget -O - 'http://packages.coex.tech/key.asc' | apt-key add - 
+echo 'deb http://packages.coex.tech buster main' >> /etc/apt/sources.list
+sudo apt update
+```

docs/en/programming.md

@@ -34,10 +34,18 @@ Read more in the [GPS connection](gps.md) article.
 
 > **Info** For studying Python programming language, see [tutorial](https://www.learnpython.org/en/Welcome).
 
-After you've configured your positioning system, you can start writing programs for autonomous flights. Use the [SSH connection to the Raspberry Pi](ssh.md) to run your scripts. In order to run a Python script use the `python` command:
+After you've configured your positioning system, you can start writing programs for autonomous flights. Use the [SSH connection to the Raspberry Pi](ssh.md) to run your scripts.
+
+Before the first flight it's recommended to check the Clover's configuration with [selfcheck.py utility](selfcheck.md):
 
 ```bash
-python flight.py
+rosrun clover selfcheck.py
+```
+
+In order to run a Python script use the `python3` command:
+
+```bash
+python3 flight.py
 ```
 
 Below is a complete flight program that performs a takeoff, flies forward and lands:

docs/en/ros.md

@@ -63,7 +63,7 @@ An example of subscription to topic `/foo`:
 
 ```python
 def foo_callback(msg):
-    print msg.data
+    print(msg.data)
 
 # Subscribing. When a message is received in topic /foo, function foo_callback will be invoked.
 rospy.Subscriber('/foo', String, foo_callback)

docs/en/simple_offboard.md

@@ -75,14 +75,14 @@ Displaying drone coordinates `x`, `y` and `z` in the local system of coordinates
 
 ```python
 telemetry = get_telemetry()
-print telemetry.x, telemetry.y, telemetry.z
+print(telemetry.x, telemetry.y, telemetry.z)
 ```
 
 Displaying drone altitude relative to [the ArUco map](aruco.md):
 
 ```python
 telemetry = get_telemetry(frame_id='aruco_map')
-print telemetry.z
+print(telemetry.z)
 ```
 
 Checking global position availability:
@@ -90,9 +90,9 @@ Checking global position availability:
 ```python
 import math
 if not math.isnan(get_telemetry().lat):
-    print 'Global position is available'
+    print('Global position is available')
 else:
-    print 'No global position'
+    print('No global position')
 ```
 
 Output of current telemetry (command line):
@@ -268,12 +268,6 @@ Flying forward (relative to the drone) at the speed of 1 m/s:
 set_velocity(vx=1, vy=0.0, vz=0, frame_id='body')
 ```
 
-One possible way of flying in a circle:
-
-```python
-set_velocity(vx=0.4, vy=0.0, vz=0, yaw=float('nan'), yaw_rate=0.4, frame_id='body')
-```
-
 ### set_attitude
 
 Set pitch, roll, yaw and throttle level (similar to [the `STABILIZED` mode](modes.md)). This service may be used for lower level control of the drone behavior, or controlling the drone when no reliable data on its position is available.
@@ -309,7 +303,7 @@ Landing the drone:
 res = land()
 
 if res.success:
-    print 'drone is landing'
+    print('drone is landing')
 ```
 
 Landing the drone (command line):

docs/en/snippets.md

@@ -319,7 +319,7 @@ def flip():
     rospy.loginfo('finish flip')
     set_position(x=start.x, y=start.y, z=start.z, yaw=start.yaw)  # finish flip
 
-print navigate(z=2, speed=1, frame_id='body', auto_arm=True)  # take off
+print(navigate(z=2, speed=1, frame_id='body', auto_arm=True))  # take off
 rospy.sleep(10)
 
 rospy.loginfo('flip')
@@ -359,3 +359,28 @@ calibrate_gyro()
 ```
 
 > **Note** In process of calibration the drone should not be moved.
+
+<!-- markdownlint-disable MD044 -->
+
+### # {#aruco-detect-enabled}
+
+<!-- markdownlint-enable MD044 -->
+
+Enable and disable [ArUco markers recognition](aruco_marker.md) dynamically (for example, for saving CPU resources):
+
+```python
+import rospy
+import dynamic_reconfigure.client
+
+# ...
+
+client = dynamic_reconfigure.client.Client('aruco_detect')
+
+# Turn markers recognition off
+client.update_configuration({'enabled': False})
+
+rospy.sleep(5)
+
+# Turn markers recognition on
+client.update_configuration({'enabled': True})
+```

docs/en/sonar.md

@@ -83,7 +83,7 @@ pi.callback(ECHO, pigpio.FALLING_EDGE, fall)
 
 while True:
     # Reading the distance:
-    print read_distance()
+    print(read_distance())
 
 ```
 
@@ -104,7 +104,7 @@ def read_distance_filtered():
     return numpy.median(history)
 
 while True:
-    print read_distance_filtered()
+    print(read_distance_filtered())
 ```
 
 An example of charts of initial and filtered data:

docs/en/ssh.md

@@ -15,7 +15,7 @@ Password: `raspberry`.
 
 For SSH access from Windows, you may use [PuTTY] (https://www.chiark.greenend.org.uk/~sgtatham/putty/latest.html).
 
-You can also gain SSH access from your smart-phone using the [Termius] app (https://www.termius.com).
+You can also gain SSH access from your smart-phone using the [Termius](https://www.termius.com) app.
 
 Read more: https://www.raspberrypi.org/documentation/remote-access/ssh/README.md
 

docs/ru/4in1.md

@@ -1,6 +1,6 @@
 # Подключение регуляторов 4in1
 
-## Распиновка платы реуляторов 4in1
+## Распиновка платы регуляторов 4in1
 
 Одним цветом выделены соответствующие фазные провода (рис. 1a) и управляющий ими сигнал (рис. 1b).
 
@@ -10,7 +10,7 @@
 
 На рис. 2a указана распиновка гребенки:
 
-* **SIGNAL** – подключение регуляторов. Каждый пин имет свой собственный сигнал. На 5 и 6 сигнал можно получать ШИМ сигнал (Например, можно подключить сервопривод).
+* **SIGNAL** – подключение регуляторов. Каждый пин имеет свой собственный сигнал. На 5 и 6 сигнал можно получать ШИМ сигнал (Например, можно подключить сервопривод).
 * **GND** – земля полетного контроллера. Единая шина на всех пинах GND (отмечены черным).
 * 1, 2, 3, 4 – порты для подключения ESC.
 * 1, 2 - порты расширения выходного ШИМ сигнала (настраиваются в QGroundControl, также могут использоваться для управления гексакоптером).
@@ -23,7 +23,7 @@
 
 ## Иллюстрация подключения, исходя из текущей ориентации платы регуляторов 4in1
 
-Используя рис. 1a, 1b, 2a, 2b необходимо сопоставить каждому мотору свой сигнал управления и подключить в соотвествии с порядком нумерации моторов Pixracer.
+Используя рис. 1a, 1b, 2a, 2b необходимо сопоставить каждому мотору свой сигнал управления и подключить в соответствии с порядком нумерации моторов Pixracer.
 
 Например, мотор М3, вращающийся против часовой стрелки (верхний левый угол) управляется сигналом S4 (зеленый провод). Подключается в порт 3.
 

docs/ru/SUMMARY.md

@@ -96,7 +96,9 @@
   * [Подключение регулятора 4 в 1](4in1.md)
   * [Светодиодная лента (legacy)](leds_old.md)
   * [Вклад в Клевер](contributing.md)
+  * [Репозиторий пакетов COEX](packages.md)
   * [Переход на версию 0.20](migrate20.md)
+  * [Переход на версию 0.22](migrate22.md)
   * [COEX Duocam](duocam.md)
     * [Виртуальная MAVLink-камера](duocam_mavlink.md)
 * [Мероприятия](events.md)

docs/ru/arduino.md

@@ -76,7 +76,7 @@ for(int i=0; i<8; i++) {
 Пример программы, контролирующей коптер по позиции, с использованием сервисов `navigate` и `set_mode`:
 
 ```cpp
-// Подключение библиотек для работы с rosseral
+// Подключение библиотек для работы с rosserial
 #include <ros.h>
 
 // Подключение заголовочных файлов сообщений пакета clover и MAVROS

docs/ru/aruco_map.md

@@ -1,5 +1,9 @@
 # Навигация по картам ArUco-маркеров
 
+> **Note** Документация для версий [образа](image.md), начиная с версии **0.22**. Для более ранних версий см. [документацию для версии **0.20**](https://github.com/CopterExpress/clover/blob/v0.20/docs/ru/aruco_map.md).
+
+<!-- -->
+
 > **Info** Для распознавания маркеров модуль камеры должен быть корректно подключен и [сконфигурирован](camera.md).
 
 <!-- -->
@@ -39,18 +43,18 @@ id_маркера размер_маркера x y z угол_z угол_y уго
 
 Где `угол_N` – это угол поворота маркера вокруг оси N в радианах.
 
-Путь к файлу с картой задается в параметре `map`:
+Файлы карт располагаются в каталоге `~/catkin_ws/src/clover/aruco_pose/map`. Название файла с картой задается в аргументе `map`:
 
 ```xml
-<param name="map" value="$(find aruco_pose)/map/map.txt"/>
+<arg name="map" default="map.txt"/>
 ```
 
-Смотрите примеры карт маркеров в каталоге [`~/catkin_ws/src/clover/aruco_pose/map`](https://github.com/CopterExpress/clover/tree/master/aruco_pose/map).
+Смотрите примеры карт маркеров в [`вышеуказанном каталоге`](https://github.com/CopterExpress/clover/tree/master/aruco_pose/map).
 
 Файл карты может быть сгенерирован с помощью инструмента `genmap.py`:
 
 ```bash
-rosrun aruco_pose genmap.py length x y dist_x dist_y first > ~/catkin_ws/src/clover/aruco_pose/map/test_map.txt
+rosrun aruco_pose genmap.py length x y dist_x dist_y first -o test_map.txt
 ```
 
 Где `length` – размер маркера, `x` – количество маркеров по оси *x*, `y` - количество маркеров по оси *y*, `dist_x` – расстояние между центрами маркеров по оси *x*, `y` – расстояние между центрами маркеров по оси *y*, `first` – ID первого (левого нижнего) маркера, `test_map.txt` – название файла с картой. Дополнительный ключ `--bottom-left` позволяет нумеровать маркеры с левого нижнего угла.
@@ -58,7 +62,7 @@ rosrun aruco_pose genmap.py length x y dist_x dist_y first > ~/catkin_ws/src/clo
 Пример:
 
 ```bash
-rosrun aruco_pose genmap.py 0.33 2 4 1 1 0 > ~/catkin_ws/src/clover/aruco_pose/map/test_map.txt
+rosrun aruco_pose genmap.py 0.33 2 4 1 1 0 -o test_map.txt
 ```
 
 Дополнительную информацию по утилите можно получить по ключу `-h`: `rosrun aruco_pose genmap.py -h`.
@@ -154,10 +158,10 @@ navigate(frame_id='aruco_5', x=0, y=0, z=1)
 
 Для навигации по маркерам, расположенным на потолке, необходимо поставить основную камеру так, чтобы она смотрела вверх и [установить соответствующий фрейм камеры](camera_setup.md#frame).
 
-Также в файле `~/catkin_ws/src/clover/clover/launch/aruco.launch` необходимо установить параметр `known_tilt` в секциях `aruco_detect` и `aruco_map` в значение `map_flipped`:
+Также в файле `~/catkin_ws/src/clover/clover/launch/aruco.launch` необходимо выставить аргумент `placement` в значение `ceiling`:
 
 ```xml
-<param name="known_tilt" value="map_flipped"/>
+<arg name="placement" default="ceiling"/>
 ```
 
 При такой конфигурации фрейм `aruco_map` также окажется перевернутым. Таким образом, для полета на высоту 2 метра ниже потолка, аргумент `z` нужно устанавливать в 2:

docs/ru/aruco_marker.md

@@ -1,5 +1,9 @@
 # Распознавание ArUco-маркеров
 
+> **Note** Документация для версий [образа](image.md), начиная с версии **0.22**. Для более ранних версий см. [документацию для версии **0.20**](https://github.com/CopterExpress/clover/blob/v0.20/docs/ru/aruco_marker.md).
+
+<!-- -->
+
 > **Info** Для распознавания маркеров модуль камеры должен быть корректно подключен и [сконфигурирован](camera_setup.md).
 
 Модуль `aruco_detect` распознает ArUco-маркеры и публикует их позиции в ROS-топики и в [TF](frames.md).
@@ -22,22 +26,20 @@
 <arg name="aruco_detect" default="true"/>
 ```
 
-Для правильной работы в этом же файле в секции `aruco_detect` должны быть выставлены параметры:
+Для правильной работы в этом же файле также должны быть выставлены аргументы:
 
 ```xml
-<param name="length" value="0.32"/>          <!-- размер маркеров в метрах (не включая белую рамку) -->
-<param name="estimate_poses" value="true"/>  <!-- включение вычисления позиций маркеров -->
-<param name="send_tf" value="true"/>         <!-- отправлять позиции маркеров в виде TF-фреймов -->
-<param name="known_tilt" value="map"/>       <!-- наклон маркеров, см. далее -->
+<arg name="placement" default="floor"/> <!-- расположение маркеров, см. далее -->
+<arg name="length" default="0.33"/>     <!-- размер маркеров в метрах (не включая белую рамку) -->
 ```
 
-Значение параметра `known_tilt` следует выставлять следующим образом:
+Значение аргумента `placement` следует выставлять следующим образом:
 
-* если *все* маркеры наклеены на полу (земле), выставить значение `map`;
-* если *все* маркеры наклеены на потолке, выставить значение `map_flipped`;
+* если *все* маркеры наклеены на полу (земле), выставить значение `floor`;
+* если *все* маркеры наклеены на потолке, выставить значение `ceiling`;
 * в противном случае удалить строку с параметром.
 
-Если некоторые маркеры имеют размер, отличный значения `length`, их размер может быть переопределен с помощью параметра `length_override`:
+Если некоторые маркеры имеют размер, отличный значения `length`, их размер может быть переопределен с помощью параметра `length_override` ноды `aruco_detect`:
 
 ```xml
 <param name="length_override/3" value="0.1"/>    <!-- маркер c id 3 имеет размер 10 см -->
@@ -110,9 +112,9 @@ rospy.init_node('my_node')
 # ...
 
 def markers_callback(msg):
-    print 'Detected markers:':
+    print('Detected markers:'):
     for marker in msg.markers:
-        print 'Marker: %s' % marker
+        print('Marker: %s' % marker)
 
 # Подписываемся. При получении сообщения в топик aruco_detect/markers будет вызвана функция markers_callback.
 rospy.Subscriber('aruco_detect/markers', MarkerArray, markers_callback)

docs/ru/arucogenmap.md

@@ -6,7 +6,7 @@
 
 ## Создание поля
 
-<div style="display: flex; flex-direction: row"><img src="../assets/fieldsetup.PNG" alt=""><div style="padding-left: 20px">Перед началом работы надо задать размеры поля. Оно нужно только для удобства. Для перемения по "полотну" испольнуйте тачпад или колёсико мыши для перемещения по карте. При использовании мыши зажмите Shift для перемещения в горизонтальном направлении и Ctrl для увеличения/уменьшения поля.</div></div>
+<div style="display: flex; flex-direction: row"><img src="../assets/fieldsetup.PNG" alt=""><div style="padding-left: 20px">Перед началом работы надо задать размеры поля. Оно нужно только для удобства. Для перемещения по "полотну" используйте тачпад или колёсико мыши для перемещения по карте. При использовании мыши зажмите Shift для перемещения в горизонтальном направлении и Ctrl для увеличения/уменьшения поля.</div></div>
 
 ## Инструмент творения
 

docs/ru/assemble_2.md

@@ -204,7 +204,7 @@
 
 ![Постпаячная проверка](../assets/zapPDBtest.jpg)
 
-Чтобы припой аккуратно заполнил всю площадку, необходимо её прогреть. Для этого нужно удерживать жало паяльника на контактной плащадке в течение 2 сек (или больше, если потребуется)
+Чтобы припой аккуратно заполнил всю площадку, необходимо её прогреть. Для этого нужно удерживать жало паяльника на контактной площадке в течение 2 сек (или больше, если потребуется)
 
 #### Пайка силового разъема питания XT60
 

docs/ru/assemble_4.md

@@ -393,7 +393,7 @@
 
 Установите 4 пропеллера, согласно [схеме вращения](#проверка-направления-вращения-моторов). При установке пропеллеров АКБ должна быть отключена.
 
-При установке будте внимательны, чтобы пропеллер не был перевернут. На лицевой стороне пропеллера имеется маркировка его характеристик, а также направление вращения, которое должно совпадать с направлением вращения моторов.
+При установке будьте внимательны, чтобы пропеллер не был перевернут. На лицевой стороне пропеллера имеется маркировка его характеристик, а также направление вращения, которое должно совпадать с направлением вращения моторов.
 
 <div class="image-group">
     <img src="../assets/assembling_clever4/final_2.png" width=300 class="zoom border">
@@ -413,7 +413,7 @@
 
 <img src="../assets/assembling_clever4/final_1.png" width=300 class="zoom border center">
 
-Обязательно установите и настройте индикатор напряжения перед полетом, чтобы не переразрядить аккумулятор. Для настройки индикатора используйте конпку расположенную в его основании. Отображаемые цифры во время настройки обозначают минимально возможное напряжение в каждой [ячейке](glossary.md#ячейка--банка-акб) аккумулятора, рекомендуемое значение **3.5**.
+Обязательно установите и настройте индикатор напряжения перед полетом, чтобы не переразрядить аккумулятор. Для настройки индикатора используйте кнопку расположенную в его основании. Отображаемые цифры во время настройки обозначают минимально возможное напряжение в каждой [ячейке](glossary.md#ячейка--банка-акб) аккумулятора, рекомендуемое значение **3.5**.
 
 > **Info** Звуковая индикация означает, что ваш аккумулятор разряжен и его нужно зарядить.
 

docs/ru/auto_setup.md

@@ -126,7 +126,7 @@ Ctrl+C
 Запустить программу myprogram.py на Питоне:
 
 ```bash
-python myprogram.py
+python3 myprogram.py
 ```
 
 Журнал событий процессов Клевера. Пролистывать список можно нажатием Enter или сочетанием клавиш Ctrl+V (пролистывает быстрее):
@@ -406,7 +406,7 @@ sudo nano /etc/sudoers
 - Запустите программу. Для этого выполните команду:
 
   ```bash
-  python my_program.py
+  python3 my_program.py
   ```
 
   > **Warning** После выполнения программы дрон может некорректно приземлиться и продолжать лететь над полом. В таком случае нужно перехватить управление.

docs/ru/bigchallenges.md

@@ -47,7 +47,7 @@ git clone https://github.com/Tennessium/HUEX
 
 Перед началом работы с системой необходимо перевести коптеры в режим клиента и подключить к сети WiFi. Вы можете воспользоваться [этим мануалом](network.md#переключение-адаптера-в-режим-клиента)
 
-Однако, для упрощения развертывания системы на нескольких коптреах, рекомендуется использование нашего скрипта, лежащего в папке *copter/setup/*
+Однако, для упрощения развертывания системы на нескольких коптерах, рекомендуется использование нашего скрипта, лежащего в папке *copter/setup/*
 
 - Перейдите в папку
 
@@ -91,7 +91,7 @@ pip install -r requirements.txt
 
 В файле *copter/consts.py* укажите IP-адрес сервера.
 
-Для запуска основного скрипта воспользуйтсь нашим systemd-сервисом.
+Для запуска основного скрипта воспользуйтесь нашим systemd-сервисом.
 
 ```bash
 sudo systemctl enable /home/pi/HUEX/clever/setup/taxi.service
@@ -108,7 +108,7 @@ sudo systemctl stop taxi.service
 ## Веб-интерфейс центра управления полётами
 
 <img src="../assets/cup.png" alt=""/>
-В данном веб интерфейсе можно следить за полётами всех дронов на карте (масштабировать с помощью колёсика, передвигаять с помощью Alt). При нажатии на лебедя в правом верхнем углу все коптеры аварийно садятся, А при нажатии на значок "обновить" все коптеры автоматически удаляются, что приводит к удалению всех комманд и посадке активных на текущий момент коптеров.
+В данном веб интерфейсе можно следить за полётами всех дронов на карте (масштабировать с помощью колёсика, передвигать с помощью Alt). При нажатии на лебедя в правом верхнем углу все коптеры аварийно садятся, А при нажатии на значок "обновить" все коптеры автоматически удаляются, что приводит к удалению всех команд и посадке активных на текущий момент коптеров.
 С помощью инструментов в правом нижнем углу можно строить новые основания и рёбра.
 
 ## Веб-интерфейс заказа

docs/ru/blocks.md

@@ -69,7 +69,7 @@
 
 * *body* – координаты относительно коптера: вперед (*forward*), влево (*left*), вверх (*up*).
 * *markers map* – система координат, связанная с [картой ArUco-маркеров](aruco_map.md).
-* *marker* – система координта, связанная с [ArUco-маркером](aruco_marker.md); появляется поле для ввода ID маркеа.
+* *marker* – система координат, связанная с [ArUco-маркером](aruco_marker.md); появляется поле для ввода ID маркеа.
 * *last navigate target* – координаты относительно последней заданной точки для навигации.
 * *map* – локальная система координат коптера, связана с местом его инициализации.
 

docs/ru/camera.md

@@ -135,12 +135,12 @@ def image_callback(data):
     cv_image = bridge.imgmsg_to_cv2(data, 'bgr8')  # OpenCV image
     barcodes = pyzbar.decode(cv_image)
     for barcode in barcodes:
-        b_data = barcode.data.encode("utf-8")
+        b_data = barcode.data.decode("utf-8")
         b_type = barcode.type
         (x, y, w, h) = barcode.rect
         xc = x + w/2
         yc = y + h/2
-        print ("Found {} with data {} with center at x={}, y={}".format(b_type, b_data, xc, yc))
+        print("Found {} with data {} with center at x={}, y={}".format(b_type, b_data, xc, yc))
 
 image_sub = rospy.Subscriber('main_camera/image_raw', Image, image_callback, queue_size=1)
 
@@ -155,3 +155,13 @@ rospy.spin()
 ```
 
 Топик для подписчика в этом случае необходимо поменять на `main_camera/image_raw_throttled`.
+
+## Запись видео
+
+Для записи видео может использована нода [`video_recorder`](http://wiki.ros.org/image_view#image_view.2Fdiamondback.video_recorder) из пакета `image_view`:
+
+```bash
+rosrun image_view video_recorder image:=/main_camera/image_raw
+```
+
+Видео будет сохранено в файл `output.avi`. В аргументе `image` указывается название топика для записи видео.

docs/ru/camera_calib.md

@@ -66,7 +66,7 @@ sudo python setup.py install
 
 ![asd](../assets/cam_calib3.png)
 
-Если перекрестия были распознаныы правильно, нажмите на клавишу ***Add***, и перейдите к получению новых фотографий. В противном же случае, если перекрестия были распознаны некорректно, пропустите данную фотографию при помощи клавиши ***Skip***.
+Если перекрестия были распознаны правильно, нажмите на клавишу ***Add***, и перейдите к получению новых фотографий. В противном же случае, если перекрестия были распознаны некорректно, пропустите данную фотографию при помощи клавиши ***Skip***.
 
 >В большинстве случаев найденные углы будут подсвечиваться разными цветами, но иногда подсветка будет становиться красной. это происходит в том случае, если углы распознаны, но неточно.
 
@@ -183,7 +183,7 @@ Path:  # Путь до папки с изображениями
 
 ![asd](../assets/img2.jpg)
 
-Иcправленные изображения:
+Исправленные изображения:
 
 ![asd](../assets/calibresult.jpg)
 

docs/ru/clever-show.md

@@ -1,6 +1,6 @@
 # clever-show
 
-Програмное обеспечение для запуска шоу дронов под управлением Raspberry Pi с пакетом COEX [Clover](https://github.com/CopterExpress/clever) и полётного контроллера с прошивкой PX4.
+Программное обеспечение для запуска шоу дронов под управлением Raspberry Pi с пакетом COEX [Clover](https://github.com/CopterExpress/clever) и полётного контроллера с прошивкой PX4.
 
 Создайте анимацию в Blender, сконвертируйте её в полётные пути дронов, настройте дроны и запустите своё собственное шоу дронов!
 

docs/ru/cli.md

@@ -39,7 +39,7 @@ cat file.py
 Запустить Python-скрипт `file.py`:
 
 ```bash
-python file.py
+python3 file.py
 ```
 
 Перезагрузить Raspberry Pi:

docs/ru/coex_gps.md

@@ -2,6 +2,8 @@
 
 ГНСС-приемник **COEX GPS** совместим с полетным контроллером [COEX Pix](coex_pix.md). Этот приемник поставляется с наборами COEX Клевер 4 Pro.
 
+> **Hint** Исходные файлы платы COEX GPS [выложены](https://github.com/CopterExpress/hardware/tree/master/COEX%20GPS) в открытый доступ под лицензией CC BY-NC-SA.
+
 ## Схемы расположения контактов
 
 ### Вид сверху

docs/ru/coex_pdb.md

@@ -4,6 +4,8 @@
 
 Габаритные размеры платы: 35x35 мм.
 
+> **Hint** Исходные файлы платы COEX PDB [выложены](https://github.com/CopterExpress/hardware/tree/master/COEX%20PDB) в открытый доступ под лицензией CC BY-NC-SA.
+
 ## Схемы расположения контактов
 
 ### Вид сверху

docs/ru/coex_pix.md

@@ -2,6 +2,8 @@
 
 Полетный контроллер **COEX Pix** является модифицированным аналогом полетного контроллера [Pixracer](https://docs.px4.io/v1.9.0/en/flight_controller/pixracer.html). Этот полетный контроллер поставляется с наборами **Клевер 4** и далее.
 
+> **Hint** Исходные файлы полетного контроллера COEX Pix [выложены](https://github.com/CopterExpress/hardware/tree/master/COEX%20Pix) в открытый доступ под лицензией CC BY-NC-SA.
+
 ## Ревизия 1.1
 
 ### Характеристики
@@ -29,7 +31,7 @@
 * *I2C* (JST-GH 4 pin) – разъем для подключения поддерживаемых I2C устройств.
 * *PWR* (JST-GH 6 pin) – разъем для подключения питания с платы COEX PDB или аналогичной, датчиков напряжения и тока.
 * *RC IN* (JST-GH 4 pin) – разъем для подключения радиоприемника аппаратуры радиоуправления, канала для * снятия показаний RSSI. Поддерживаемые RC протоколы – PPM и SBUS.
-* Разьем Micro USB – для подключения к ПК для настройки и коммуникации по протоколу USB 2.0/1.1
+* Разъем Micro USB – для подключения к ПК для настройки и коммуникации по протоколу USB 2.0/1.1
 * Слот для карты памяти MicroSD, до 32 ГБ.
 * Серворазъемы – для подключения контроллеров моторов и других устройств.