image: add image-view package for recording video from topics

* Catkin install testing

* Update standalone-install.sh

* Fix aruco_pose DetectorParams generating

* Update builder/standalone-install.sh

* Fix

* Fix

Co-authored-by: Oleg Kalachev <okalachev@gmail.com>

.markdownlint.json

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

.travis.yml

@@ -68,6 +68,14 @@ jobs:
         - 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 Noetic build"
+      env:
+        - NATIVE_DOCKER=ros:noetic-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
@@ -77,6 +85,7 @@ jobs:
         - 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
+        - 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

README.md

@@ -21,11 +21,12 @@ 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 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)

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

@@ -79,10 +79,4 @@ if ! grep -q "^bcm2835-v4l2" /etc/modules;
 then printf "bcm2835-v4l2\n" >> /etc/modules
 fi
 
-echo_stamp "#8 Check if Compute Module 4"
-if grep -q "Compute Module 4" "/proc/device-tree/model"; then
-  echo_stamp "Enable USB on Compute Module 4"
-  echo "dtoverlay=dwc2,dr_mode=host" >> /boot/config.txt
-fi
-
-echo_stamp "#9 End of configure hardware interfaces"
+echo_stamp "#8 End of configure hardware interfaces"

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-03-25/2021-03-04-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-network.sh

@@ -60,25 +60,4 @@ domain-needed
 quiet-dhcp6
 EOF
 
-echo_stamp "#4 Build the RTL8814AU Wi-Fi adapter driver"
-wget http://archive.raspberrypi.org/debian/pool/main/r/raspberrypi-firmware/raspberrypi-kernel-headers_1.20210108-1_armhf.deb
-dpkg -i raspberrypi-kernel-headers_1.20210108-1_armhf.deb
-cd /home/pi
-git clone https://github.com/aircrack-ng/rtl8812au.git --depth=1
-cd rtl8812au
-echo kernel version: $(uname -r)
-echo kernel version from procfs: $(cat /proc/version)
-echo version: $(git describe --tags --always)
-sed -i 's/CONFIG_PLATFORM_I386_PC = y/CONFIG_PLATFORM_I386_PC = n/g' Makefile # https://github.com/aircrack-ng/rtl8812au#for-raspberry-rpi
-sed -i 's/CONFIG_PLATFORM_ARM_RPI = n/CONFIG_PLATFORM_ARM_RPI = y/g' Makefile
-# sed -i 's/CONFIG_PLATFORM_ARM64_RPI = n/CONFIG_PLATFORM_ARM64_RPI = y/g' Makefile
-apt-cache policy raspberrypi-kernel-headers
-# apt-get install -y raspberrypi-kernel-headers=1.20210108 dkms
-apt-get install -y dkms
-ls /lib/modules
-echo make
-make KERNEL_VER=5.4.83-v7l+ KVER=5.4.83-v7l+ # TODO: determine kernel version from fs
-echo make install
-make install KERNEL_VER=5.4.83-v7l+ KVER=5.4.83-v7l+
-
-echo_stamp "#5 End of network installation"
+echo_stamp "#4 End of network installation"

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.0-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,11 +143,12 @@ 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"

builder/image-validate.sh

@@ -16,9 +16,9 @@ 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
 
 cd /home/pi/catkin_ws/src/clover/builder/test/

builder/standalone-install.sh

@@ -5,21 +5,34 @@ 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
+curl https://bootstrap.pypa.io/get-pip.py -o get-pip.py
+python3 ./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
 rosdep update
@@ -37,7 +50,10 @@ cd /root/catkin_ws
 catkin_make
 
 # Step 4: Run tests
-pip install --upgrade pytest
+python3 -m pip install --upgrade pytest # TODO: https://github.com/CopterExpress/clover/commit/5b970d51970cfa6f46e5c0b34acb7889d36b89c8
 cd /root/catkin_ws
 source devel/setup.bash
 catkin_make run_tests && catkin_test_results
+
+# Step 5: Install packages
+catkin_make install

builder/test/tests.py

@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
 
 # validate all required modules installed
 
@@ -28,4 +28,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 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 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,7 +36,7 @@
     <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 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"/>
@@ -45,7 +44,7 @@
     </node>
 
     <!-- main nodelet manager -->
-    <node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager" output="screen" clear_params="true">
+    <node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager" output="screen" clear_params="true" respawn="true">
         <param name="num_worker_threads" value="2"/>
     </node>
 
@@ -91,9 +90,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/main_camera.launch

@@ -18,7 +18,7 @@
     <!-- <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 node -->
-    <node pkg="nodelet" type="nodelet" name="main_camera" args="load cv_camera/CvCameraNodelet nodelet_manager" clear_params="true" unless="$(arg simulator)">
+    <node pkg="nodelet" type="nodelet" name="main_camera" args="load cv_camera/CvCameraNodelet nodelet_manager" clear_params="true" unless="$(arg simulator)" respawn="true">
         <param name="device_path" value="/dev/video0"/> <!-- v4l2 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/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");
 	}
 
@@ -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
@@ -234,12 +239,9 @@ private:
 			static 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/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"
+"${@: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/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`.
@@ -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

@@ -138,7 +138,7 @@ def image_callback(data):
         (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/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/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/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):
@@ -309,7 +309,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/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/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/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/camera.md

@@ -140,7 +140,7 @@ def image_callback(data):
         (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/cli.md

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

docs/ru/contributing.md

@@ -96,3 +96,7 @@
 ## Простой способ
 
 Если вышеприведенные инструкции для вас оказываются слишком сложными, отправляйте правки или новые статьи по e-mail (<a href="mailto:okalachev@gmail.com">okalachev@gmail.com</a>) или в Telegram (пользователь <a href="tg://resolve?domain=okalachev">@okalachev</a>).
+
+## Публикация пакетов
+
+Вы также можете опубликовать собственный пакет, расширяющий функциональность Клевера, в [Debian-репозитории COEX](packages.md).

docs/ru/copterhack2021.md

@@ -1,6 +1,6 @@
 # CopterHack 2021
 
-CopterHack 2021 – это командный конкурс по разработке проектов с открытым исходным кодом для платформы квадрокоптера "Клевер".
+CopterHack 2021 – это командный конкурс по разработке проектов с открытым исходным кодом для платформы квадрокоптера "Клевер". В конкурсе приняло участие 54 команды из 12 стран.
 
 Все информацию о мероприятии смотрите на официальном сайте: https://ru.coex.tech/copterhack.
 

docs/ru/laser.md

@@ -59,7 +59,7 @@ rospy.init_node('flight')
 
 def range_callback(msg):
     # Обработка новых данных с дальномера
-    print 'Rangefinder distance:', msg.range
+    print('Rangefinder distance:', msg.range)
 
 rospy.Subscriber('rangefinder/range', Range, range_callback)
 

docs/ru/migrate20.md

@@ -72,56 +72,6 @@ sudo systemctl restart clover
 
 Например, файл `~/catkin_ws/src/clever/clever/launch/clever.launch` теперь называется `~/catkin_ws/src/clover/clover/launch/clover.launch`.
 
-<!--
-## Переход на Python 3
-
-Python 2 был признан [устаревшим](https://www.python.org/doc/sunset-python-2/), начиная с 1 января 2020 года. Платформа Клевера переходит на использование Python 3.
-
-Для запуска полетных скриптов вместо команды `python`:
-
-```bash
-python flight.py
-```
-
-теперь следует использовать команду `python3`:
-
-```bash
-python3 flight.py
-```
-
-Синтаксис языка Python 3 имеет определенные изменения по сравнения со второй версией. Вместо *оператора* `print`:
-
-```python
-print 'Clover is the best'
-```
-
-теперь используется *функция* `print`:
-
-```python
-print('Clover is the best')
-```
-
-Оператор деления по умолчанию выполняет деление с плавающей точкой (вместо целочисленного). Python 2:
-
-```python
->>> 10 / 4
-2
-```
-
-Python 3:
-
-```python
->>> 10 / 4
-2.5
-```
-
-Для строк по умолчанию теперь используется тип `unicode` (вместо типа `str`).
-
-Указание кодировки файла (`# coding: utf8`) перестало быть необходимым.
-
-Полное описание всех изменений языка смотрите в [соответствующей статье](https://pythonworld.ru/osnovy/python2-vs-python3-razlichiya-sintaksisa.html).
--->
-
 ## Настройки Wi-Fi сети
 
 SSID Wi-Fi сети изменен на `clover-XXXX` (где X – случайная цифра), пароль изменен на `cloverwifi`.

docs/ru/migrate22.md

@@ -0,0 +1,59 @@
+# Переход на версию 0.22
+
+## Переход на Python 3
+
+Python 2 был признан [устаревшим](https://www.python.org/doc/sunset-python-2/), начиная с 1 января 2020 года. Платформа Клевера переходит на использование Python 3.
+
+Для запуска полетных скриптов вместо команды `python`:
+
+```bash
+python flight.py
+```
+
+теперь следует использовать команду `python3`:
+
+```bash
+python3 flight.py
+```
+
+Синтаксис языка Python 3 имеет определенные изменения по сравнения со второй версией. Вместо *оператора* `print`:
+
+```python
+print 'Clover is the best'  # this won't work
+```
+
+теперь используется *функция* `print`:
+
+```python
+print('Clover is the best')
+```
+
+Оператор деления по умолчанию выполняет деление с плавающей точкой (вместо целочисленного). Python 2:
+
+```python
+>>> 10 / 4
+2
+```
+
+Python 3:
+
+```python
+>>> 10 / 4
+2.5
+```
+
+Для строк по умолчанию теперь используется тип `unicode` (вместо типа `str`).
+
+Указание кодировки файла (`# coding: utf8`) перестало быть необходимым.
+
+Полное описание всех изменений языка смотрите в [соответствующей статье](https://pythonworld.ru/osnovy/python2-vs-python3-razlichiya-sintaksisa.html).
+
+## Переход на ROS Noetic
+
+<img src="../assets/noetic.png" width=200>
+
+Версия ROS Melodic обновлена до ROS Noetic. Смотрите полный список изменений в [официальной документации ROS](http://wiki.ros.org/noetic/Migration).
+
+## Изменения в launch-файлах
+
+Упрощено конфигурирование навигации с использованием ArUco-маркеров. Подробнее в статьях по [навигации по маркерам](aruco_marker.md) и [навигации по картам маркеров](aruco_map.md).

docs/ru/models.md

@@ -96,6 +96,16 @@
         </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>Проставка для захвата</b>.<br>
+            Функция: Опорный элемент для механического захвата.<br>
+            Материал: Монолитный поликарбонат 2мм.<br>
+            Количество: 1 шт.
+        </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>
 
 ## Клевер 4

docs/ru/packages.md

@@ -0,0 +1,27 @@
+# Репозиторий пакетов COEX
+
+COEX предоставляет открытый [Debian-репозиторий](https://wiki.debian.org/ru/SourcesList) с предсобранными пакетами, относящимися к ROS Noetic, для архитектуры `armhf`.
+
+> **Info** Адрес репозитория: http://packages.coex.tech.
+
+Репозиторий подключен в [образе для RPi](image.md) и может быть использован для легкой установки дополнительных ROS-пакетов.
+
+## Публикация пакетов
+
+Вы можете прислать Pull Request в [git-репозиторий с пакетами](https://github.com/CopterExpress/packages), добавляющий или обновляющий ваш пакет (файл с расширением `.deb`), относящийся с Клеверу или ROS. После принятия ваш пакет будет доступен для установки с помощью утилиты `apt`:
+
+```bash
+sudo apt install ros-noetic-clover-some-feature
+```
+
+Пакеты, расширяющие функциональность Клевера, рекомендуется называть с префиксом `clover_`, например `clover_some_feature`.
+
+## Использование на обычной Raspberry Pi OS
+
+На обычной Raspberry Pi OS репозиторий может быть добавлен в список источников пакетов следующими командами:
+
+```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/ru/programming.md

@@ -32,12 +32,20 @@
 
 ## Автономный полет {#flight}
 
-> **Info** Для изучения языка программирования Python обращайтесь к [самоучителю](https://pythonworld.ru/samouchitel-python).
+> **Info** Для изучения языка программирования Python вы можете обратиться к [самоучителю](https://pythonworld.ru/samouchitel-python).
 
-После настройки системы позиционирования становится возможным написание скриптов для автономных полетов. Для выполнения скриптов [подключитесь в Raspberry Pi по SSH](ssh.md). Для того, чтобы запустить Python-скрипт, используйте команду `python`:
+После настройки системы позиционирования становится возможным написание скриптов для автономных полетов. Для выполнения скриптов [подключитесь в Raspberry Pi по SSH](ssh.md).
+
+Перед первым полетом рекомендуется проверить конфигурацию Клевера при помощи [утилиты selfcheck.py](selfcheck.md):
 
 ```bash
-python flight.py
+rosrun clover selfcheck.py
+```
+
+Для того, чтобы запустить Python-скрипт, используйте команду `python3`:
+
+```bash
+python3 flight.py
 ```
 
 Пример программы для полета (взлет, пролет вперед, посадка):

docs/ru/ros.md

@@ -63,7 +63,7 @@ foo_pub.publish(data='Hello, world!')  # публикуем сообщение
 
 ```python
 def foo_callback(msg):
-    print msg.data
+    print(msg.data)
 
 # Подписываемся. При получении сообщения в топик /foo будет вызвана функция foo_callback.
 rospy.Subscriber('/foo', String, foo_callback)

docs/ru/simple_offboard.md

@@ -75,14 +75,14 @@ land = rospy.ServiceProxy('land', Trigger)
 
 ```python
 telemetry = get_telemetry()
-print telemetry.x, telemetry.y, telemetry.z
+print(telemetry.x, telemetry.y, telemetry.z)
 ```
 
 Вывод высоты коптера относительно [карты ArUco-меток](aruco.md):
 
 ```python
 telemetry = get_telemetry(frame_id='aruco_map')
-print telemetry.z
+print(telemetry.z)
 ```
 
 Проверка доступности глобальной позиции:
@@ -90,9 +90,9 @@ print telemetry.z
 ```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')
 ```
 
 Вывод текущей телеметрии (командная строка):
@@ -309,7 +309,7 @@ set_velocity(vx=0.4, vy=0.0, vz=0, yaw=float('nan'), yaw_rate=0.4, frame_id='bod
 res = land()
 
 if res.success:
-    print 'Copter is landing'
+    print('Copter is landing')
 ```
 
 Посадка коптера (командная строка):

docs/ru/snippets.md

@@ -337,7 +337,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')
@@ -377,3 +377,28 @@ calibrate_gyro()
 ```
 
 > **Note** В процессе калибровки гироскопов дрон нельзя двигать.
+
+<!-- markdownlint-disable MD044 -->
+
+### # {#aruco-detect-enabled}
+
+<!-- markdownlint-enable MD044 -->
+
+Динамически включать и отключать [распознавание ArUco-маркеров](aruco_marker.md) (например, для экономии ресурсов процессора):
+
+```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/ru/sonar.md

@@ -83,7 +83,7 @@ pi.callback(ECHO, pigpio.FALLING_EDGE, fall)
 
 while True:
     # Читаем дистанцию:
-    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())
 ```
 
 Пример графиков исходных и отфильтрованных данных:

redirects.json

@@ -49,7 +49,8 @@
 
         { "from": "modes/", "to": "ru/modes.html" },
         { "from": "firmware/", "to": "en/firmware.html" },
-        { "from": "simple_offboard/", "to": "ru/simple_offboard.html" },
+        { "from": "simple_offboard/", "to": "en/simple_offboard.html" },
+        { "from": "offboard/", "to": "en/simple_offboard.html" },
         { "from": "camera/", "to": "ru/camera.html" },
         { "from": "snippets/", "to": "ru/snippets.html" },
         { "from": "optical_flow/", "to": "ru/optical_flow.html" },
@@ -61,6 +62,7 @@
         { "from": "camera_setup/", "to": "en/camera_setup.html" },
         { "from": "power/", "to": "en/power.html" },
         { "from": "connection/", "to": "en/connection.html" },
+        { "from": "clover_vm/", "to": "en/simulation_vm.html" },
 
         { "from": "ru/microsd_images.html", "to": "image.html" },
         { "from": "en/microsd_images.html", "to": "image.html" }

roswww_static/CMakeLists.txt

@@ -6,3 +6,7 @@ find_package(catkin REQUIRED)
 catkin_package()
 
 install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+
+catkin_install_python(PROGRAMS main.py
+    DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)