Add articles on new aruco navigation to gitbook

`cv::aruco::drawAxis` would attempt to draw detected frame origin even when it's behind the camera. This resulted in an invalid, flipped frame displayed in `/aruco_map/debug`.

This commit prevents drawing frame axis if the frame origin (projected to the screen space) is behind the screen plane.

.travis.yml

@@ -9,11 +9,12 @@ env:
     - if [[ -z ${TRAVIS_TAG} ]]; then IMAGE_VERSION="${TRAVIS_COMMIT}}"; else IMAGE_VERSION="${TRAVIS_TAG}"; fi
     - IMAGE_NAME="$(basename -s '.git' ${TARGET_REPO})_${IMAGE_VERSION}.img"
 git:
-  depth: 1
-matrix:
+  depth: 50
+jobs:
   fast_finish: true
   include:
-    - name: "Raspberry Pi Image Build"
+    - stage: Build
+      name: "Raspberry Pi Image Build"
       cache:
         directories:
         - imgcache
@@ -39,7 +40,9 @@ matrix:
         on:
           tags: true
         draft: true
-    - name: "Documentation"
+        name: ${TRAVIS_TAG}
+    - stage: Build
+      name: "Documentation"
       language: node_js
       node_js:
         - "10"
@@ -65,7 +68,17 @@ matrix:
 #        target-branch: gh-pages
 #        on:
 #          branch: WIP/gitbook-autobuild
-
+    - stage: Annotate
+      name: Auto-generate changelog
+      language: python
+      python: 3.6
+      install:
+        - pip install GitPython PyGithub
+      script:
+        - PYTHONUNBUFFERED=1 python ./gen_changelog.py
+stages:
+  - Build
+  - Annotate
 # More info there
 # https://github.com/travis-ci/travis-ci/issues/6893
 # https://docs.travis-ci.com/user/customizing-the-build/

apps/ios/README.md

@@ -1,5 +1,4 @@
-iOS-приложение для управления Клевером
---------------------------------------
+# iOS-приложение для управления Клевером
 
 Для установки зависимостей необходим [CocoaPods](https://cocoapods.org):
 
@@ -8,3 +7,11 @@ pod install
 ```
 
 Для разработки и сборки откройте в XCode файл `cleverrc.xcworkspace`.
+
+## Политика конфиденциальности
+
+App Store приложение CLEVER RC не собирает и не хранит каких-либо личных данных пользователя.
+
+## Privacy policy
+
+The App Store app CLEVER RC does not collect and store any personal user data.

apps/ios/cleverrc/Assets.xcassets/AppIcon.appiconset/Contents.json

@@ -145,8 +145,15 @@
       "size" : "44x44",
       "idiom" : "watch",
       "scale" : "2x",
-      "role" : "longLook",
-      "subtype" : "42mm"
+      "role" : "appLauncher",
+      "subtype" : "40mm"
+    },
+    {
+      "size" : "50x50",
+      "idiom" : "watch",
+      "scale" : "2x",
+      "role" : "appLauncher",
+      "subtype" : "44mm"
     },
     {
       "size" : "86x86",
@@ -162,10 +169,24 @@
       "role" : "quickLook",
       "subtype" : "42mm"
     },
+    {
+      "size" : "108x108",
+      "idiom" : "watch",
+      "scale" : "2x",
+      "role" : "quickLook",
+      "subtype" : "44mm"
+    },
     {
       "idiom" : "watch-marketing",
       "size" : "1024x1024",
       "scale" : "1x"
+    },
+    {
+      "size" : "44x44",
+      "idiom" : "watch",
+      "scale" : "2x",
+      "role" : "longLook",
+      "subtype" : "42mm"
     }
   ],
   "info" : {

apps/ios/cleverrc/Info.plist

@@ -17,9 +17,9 @@
 	<key>CFBundlePackageType</key>
 	<string>APPL</string>
 	<key>CFBundleShortVersionString</key>
-	<string>1.1</string>
+	<string>1.2</string>
 	<key>CFBundleVersion</key>
-	<string>6</string>
+	<string>7</string>
 	<key>LSRequiresIPhoneOS</key>
 	<true/>
 	<key>UILaunchStoryboardName</key>

aruco_pose/CMakeLists.txt

@@ -212,3 +212,8 @@ target_link_libraries(aruco_pose
 
 ## Add folders to be run by python nosetests
 # catkin_add_nosetests(test)
+
+if (CATKIN_ENABLE_TESTING)
+  find_package(rostest REQUIRED)
+  add_rostest(test/basic.test)
+endif()

aruco_pose/map/office_ceiling.txt

@@ -0,0 +1,31 @@
+14	0.365	0.000	0.0	0	0	0	0
+15	0.365	1.335	0.0	0	0	0	0
+30	0.365	2.865	0.0	0	0	0	0
+31	0.365	4.200	0.0	0	0	0	0
+12	0.365	0.000	1.8	0	0	0	0
+13	0.365	1.335	1.8	0	0	0	0
+28	0.365	2.865	1.8	0	0	0	0
+29	0.365	4.200	1.8	0	0	0	0
+10	0.365	0.000	3.6	0	0	0	0
+11	0.365	1.335	3.6	0	0	0	0
+26	0.365	2.865	3.6	0	0	0	0
+27	0.365	4.200	3.6	0	0	0	0
+8	0.365	0.000	5.4	0	0	0	0
+9	0.365	1.335	5.4	0	0	0	0
+24	0.365	2.865	5.4	0	0	0	0
+25	0.365	4.200	5.4	0	0	0	0
+6	0.365	0.000	7.2	0	0	0	0
+7	0.365	1.335	7.2	0	0	0	0
+22	0.365	2.865	7.2	0	0	0	0
+23	0.365	4.200	7.2	0	0	0	0
+4	0.365	0.000	9.0	0	0	0	0
+5	0.365	1.335	9.0	0	0	0	0
+20	0.365	2.865	9.0	0	0	0	0
+21	0.365	4.200	9.0	0	0	0	0
+2	0.365	0.000	10.8	0	0	0	0
+3	0.365	1.335	10.8	0	0	0	0
+18	0.365	2.865	10.8	0	0	0	0
+19	0.365	4.200	10.8	0	0	0	0
+1	0.365	0.000	12.6	0	0	0	0
+0	0.365	1.335	12.6	0	0	0	0
+16	0.365	2.865	12.6	0	0	0	0

aruco_pose/msg/Marker.msg

@@ -1,4 +1,5 @@
 uint32 id
+float32 length
 geometry_msgs/Pose pose
 Point2D c1
 Point2D c2

aruco_pose/package.xml

@@ -29,6 +29,8 @@
   <depend>sensor_msgs</depend>
   <depend>rostest</depend>
 
+  <test_depend>image_publisher</test_depend>
+
   <!-- The export tag contains other, unspecified, tags -->
   <export>
     <nodelet plugin="${prefix}/nodelet_plugins.xml" />

aruco_pose/src/aruco_detect.cpp

@@ -62,7 +62,7 @@ private:
 	image_transport::Publisher debug_pub_;
 	image_transport::CameraSubscriber img_sub_;
 	ros::Publisher markers_pub_, vis_markers_pub_;
-	bool estimate_poses_, send_tf_;
+	bool estimate_poses_, send_tf_, auto_flip_;
 	double length_;
 	std::unordered_map<int, double> length_override_;
 	std::string frame_id_prefix_, known_tilt_;
@@ -87,6 +87,8 @@ public:
 		readLengthOverride();
 
 		nh_priv_.param<std::string>("known_tilt", known_tilt_, "");
+		nh_priv_.param("auto_flip", auto_flip_, false);
+
 		nh_priv_.param<std::string>("frame_id_prefix", frame_id_prefix_, "aruco_");
 
 		camera_matrix_ = cv::Mat::zeros(3, 3, CV_64F);
@@ -169,6 +171,7 @@ private:
 
 			for (unsigned int i = 0; i < ids.size(); i++) {
 				marker.id = ids[i];
+				marker.length = getMarkerLength(marker.id);
 				fillCorners(marker, corners[i]);
 
 				if (estimate_poses_) {
@@ -176,7 +179,7 @@ private:
 
 					// snap orientation (if enabled and snap frame available)
 					if (!known_tilt_.empty() && !snap_to.header.frame_id.empty()) {
-						snapOrientation(marker.pose.orientation, snap_to.transform.rotation);
+						snapOrientation(marker.pose.orientation, snap_to.transform.rotation, auto_flip_);
 					}
 
 					// TODO: check IDs are unique

aruco_pose/src/aruco_map.cpp

@@ -73,6 +73,7 @@ private:
 	visualization_msgs::MarkerArray vis_array_;
 	std::string known_tilt_;
 	int image_width_, image_height_, image_margin_;
+	bool auto_flip_;
 
 public:
 	virtual void onInit()
@@ -95,6 +96,7 @@ public:
 		nh_priv_.param<std::string>("type", type, "map");
 		nh_priv_.param<std::string>("frame_id", transform_.child_frame_id, "aruco_map");
 		nh_priv_.param<std::string>("known_tilt", known_tilt_, "");
+		nh_priv_.param("auto_flip", auto_flip_, false);
 		nh_priv_.param("image_width", image_width_, 2000);
 		nh_priv_.param("image_height", image_height_, 2000);
 		nh_priv_.param("image_margin", image_margin_, 200);
@@ -183,7 +185,7 @@ public:
 			try {
 				geometry_msgs::TransformStamped snap_to = tf_buffer_.lookupTransform(markers->header.frame_id,
 				                                          known_tilt_, markers->header.stamp, ros::Duration(0.02));
-				snapOrientation(transform_.transform.rotation, snap_to.transform.rotation);
+				snapOrientation(transform_.transform.rotation, snap_to.transform.rotation, auto_flip_);
 			} catch (const tf2::TransformException& e) {
 				ROS_WARN_THROTTLE(1, "aruco_map: can't snap: %s", e.what());
 			}
@@ -217,7 +219,7 @@ publish_debug:
 			Mat mat = cv_bridge::toCvCopy(image, "bgr8")->image; // copy image as we're planning to modify it
 			cv::aruco::drawDetectedMarkers(mat, corners, ids); // draw detected markers
 			if (valid) {
-				cv::aruco::drawAxis(mat, camera_matrix_, dist_coeffs_, rvec, tvec, 1.0); // draw board axis
+				_drawAxis(mat, camera_matrix_, dist_coeffs_, rvec, tvec, 1.0); // draw board axis
 			}
 			cv_bridge::CvImage out_msg;
 			out_msg.header.frame_id = image->header.frame_id;
@@ -228,7 +230,6 @@ publish_debug:
 		}
 	}
 
-	// TODO consider z
 	void alignObjPointsToCenter(Mat &obj_points, double &center_x, double &center_y, double &center_z) const
 	{
 		// Align object points to the center of mass
@@ -399,12 +400,19 @@ publish_debug:
 
 	void publishMapImage()
 	{
+		cv::Size size(image_width_, image_height_);
 		cv::Mat image;
 		cv_bridge::CvImage msg;
-		drawPlanarBoard(board_, cv::Size(image_width_, image_height_), image, image_margin_, 1);
 
-		cv::cvtColor(image, image, CV_GRAY2BGR);
-		msg.encoding = sensor_msgs::image_encodings::BGR8;
+		if (!board_->ids.empty()) {
+			_drawPlanarBoard(board_, size, image, image_margin_, 1);
+		} else {
+			// empty map
+			image.create(size, CV_8UC1);
+			image.setTo(cv::Scalar::all(255));
+		}
+
+		msg.encoding = sensor_msgs::image_encodings::MONO8;
 		msg.image = image;
 		img_pub_.publish(msg.toImageMsg());
 	}

aruco_pose/src/draw.cpp

@@ -6,92 +6,772 @@
 using namespace cv;
 using namespace cv::aruco;
 
-void drawPlanarBoard(Board *_board, Size outSize, OutputArray _img, int marginSize,
-                     int borderBits) {
+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));
 
-    CV_Assert(outSize.area() > 0);
-    CV_Assert(marginSize >= 0);
+static void _projectPoints( InputArray objectPoints,
+                    InputArray rvec, InputArray tvec,
+                    InputArray cameraMatrix, InputArray distCoeffs,
+                    OutputArray imagePoints,
+                    OutputArray jacobian = noArray(),
+                    double aspectRatio = 0 );
 
-    _img.create(outSize, CV_8UC1);
-    Mat out = _img.getMat();
-    out.setTo(Scalar::all(255));
-    out.adjustROI(-marginSize, -marginSize, -marginSize, -marginSize);
 
-    // calculate max and min values in XY plane
-    CV_Assert(_board->objPoints.size() > 0);
-    float minX, maxX, minY, maxY;
-    minX = maxX = _board->objPoints[0][0].x;
-    minY = maxY = _board->objPoints[0][0].y;
+void _drawPlanarBoard(Board *_board, Size outSize, OutputArray _img, int marginSize,
+                      int borderBits) {
 
-    for(unsigned int i = 0; i < _board->objPoints.size(); i++) {
-        for(int j = 0; j < 4; j++) {
-            minX = min(minX, _board->objPoints[i][j].x);
-            maxX = max(maxX, _board->objPoints[i][j].x);
-            minY = min(minY, _board->objPoints[i][j].y);
-            maxY = max(maxY, _board->objPoints[i][j].y);
-        }
-    }
+	CV_Assert(outSize.area() > 0);
+	CV_Assert(marginSize >= 0);
 
-    float sizeX = maxX - minX;
-    float sizeY = maxY - minY;
+	_img.create(outSize, CV_8UC1);
+	Mat out = _img.getMat();
+	out.setTo(Scalar::all(255));
+	out.adjustROI(-marginSize, -marginSize, -marginSize, -marginSize);
 
-    // proportion transformations
-    float xReduction = sizeX / float(out.cols);
-    float yReduction = sizeY / float(out.rows);
+	// calculate max and min values in XY plane
+	CV_Assert(_board->objPoints.size() > 0);
+	float minX, maxX, minY, maxY;
+	minX = maxX = _board->objPoints[0][0].x;
+	minY = maxY = _board->objPoints[0][0].y;
 
-    // determine the zone where the markers are placed
-    if(xReduction > yReduction) {
-        int nRows = int(sizeY / xReduction);
-        int rowsMargins = (out.rows - nRows) / 2;
-        out.adjustROI(-rowsMargins, -rowsMargins, 0, 0);
-    } else {
-        int nCols = int(sizeX / yReduction);
-        int colsMargins = (out.cols - nCols) / 2;
-        out.adjustROI(0, 0, -colsMargins, -colsMargins);
-    }
+	for(unsigned int i = 0; i < _board->objPoints.size(); i++) {
+		for(int j = 0; j < 4; j++) {
+			minX = min(minX, _board->objPoints[i][j].x);
+			maxX = max(maxX, _board->objPoints[i][j].x);
+			minY = min(minY, _board->objPoints[i][j].y);
+			maxY = max(maxY, _board->objPoints[i][j].y);
+		}
+	}
 
-    // now paint each marker
-    Dictionary &dictionary = *(_board->dictionary);
-    Mat marker;
-    Point2f outCorners[3];
-    Point2f inCorners[3];
-    for(unsigned int m = 0; m < _board->objPoints.size(); m++) {
-        // transform corners to markerZone coordinates
-        for(int j = 0; j < 3; j++) {
-            Point2f pf = Point2f(_board->objPoints[m][j].x, _board->objPoints[m][j].y);
-            // move top left to 0, 0
-            pf -= Point2f(minX, minY);
-            pf.x = pf.x / sizeX * float(out.cols);
-            pf.y = (1.0f - pf.y / sizeY) * float(out.rows);
-            outCorners[j] = pf;
-        }
+	float sizeX = maxX - minX;
+	float sizeY = maxY - minY;
 
-        // get marker
-        Size dst_sz(outCorners[2] - outCorners[0]); // assuming CCW order
-        // dst_sz.width = dst_sz.height = std::min(dst_sz.width, dst_sz.height); //marker should be square
+	// proportion transformations
+	float xReduction = sizeX / float(out.cols);
+	float yReduction = sizeY / float(out.rows);
+
+	// determine the zone where the markers are placed
+	if(xReduction > yReduction) {
+		int nRows = int(sizeY / xReduction);
+		int rowsMargins = (out.rows - nRows) / 2;
+		out.adjustROI(-rowsMargins, -rowsMargins, 0, 0);
+	} else {
+		int nCols = int(sizeX / yReduction);
+		int colsMargins = (out.cols - nCols) / 2;
+		out.adjustROI(0, 0, -colsMargins, -colsMargins);
+	}
+
+	// now paint each marker
+	Dictionary &dictionary = *(_board->dictionary);
+	Mat marker;
+	Point2f outCorners[3];
+	Point2f inCorners[3];
+	for(unsigned int m = 0; m < _board->objPoints.size(); m++) {
+		// transform corners to markerZone coordinates
+		for(int j = 0; j < 3; j++) {
+			Point2f pf = Point2f(_board->objPoints[m][j].x, _board->objPoints[m][j].y);
+			// move top left to 0, 0
+			pf -= Point2f(minX, minY);
+			pf.x = pf.x / sizeX * float(out.cols);
+			pf.y = (1.0f - pf.y / sizeY) * float(out.rows);
+			outCorners[j] = pf;
+		}
+
+		// get marker
+		Size dst_sz(outCorners[2] - outCorners[0]); // assuming CCW order
+		// dst_sz.width = dst_sz.height = std::min(dst_sz.width, dst_sz.height); //marker should be square
 		double diag = std::round(std::hypot(dst_sz.width, dst_sz.height));
 		int side = std::round(diag / std::sqrt(2));
 
 		dictionary.drawMarker(_board->ids[m], side, marker, borderBits);
 
-		try {
-			if((outCorners[0].y == outCorners[1].y) && (outCorners[1].x == outCorners[2].x)) {
-				// marker is aligned to image axes
-				marker.copyTo(out(Rect(outCorners[0], dst_sz)));
-				continue;
-			}
+		// interpolate tiny marker to marker position in markerZone
+		inCorners[0] = Point2f(-0.5f, -0.5f);
+		inCorners[1] = Point2f(marker.cols - 0.5f, -0.5f);
+		inCorners[2] = Point2f(marker.cols - 0.5f, marker.rows - 0.5f);
 
-			// interpolate tiny marker to marker position in markerZone
-			inCorners[0] = Point2f(-0.5f, -0.5f);
-			inCorners[1] = Point2f(marker.cols - 0.5f, -0.5f);
-			inCorners[2] = Point2f(marker.cols - 0.5f, marker.rows - 0.5f);
-
-			// remove perspective
-			Mat transformation = getAffineTransform(inCorners, outCorners);
-			warpAffine(marker, out, transformation, out.size(), INTER_LINEAR,
-							BORDER_TRANSPARENT);
-		} catch(cv::Exception& e) {
-			ROS_INFO("Skip drawing marker %d", m);
-		}
+		// remove perspective
+		Mat transformation = getAffineTransform(inCorners, outCorners);
+		warpAffine(marker, out, transformation, out.size(), INTER_LINEAR,
+						BORDER_TRANSPARENT);
 	}
 }
+
+void _drawAxis(InputOutputArray _image, InputArray _cameraMatrix, InputArray _distCoeffs,
+              InputArray _rvec, InputArray _tvec, float length) {
+
+    CV_Assert(_image.getMat().total() != 0 &&
+              (_image.getMat().channels() == 1 || _image.getMat().channels() == 3));
+    CV_Assert(length > 0);
+
+    // project axis points
+    std::vector< Point3f > axisPoints;
+    axisPoints.push_back(Point3f(0, 0, 0));
+    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);
+
+    if (imagePointsZ[0].z < 0 ||
+        imagePointsZ[1].z < 0 ||
+        imagePointsZ[2].z < 0 ||
+        imagePointsZ[3].z < 0)
+    {
+        // Any axis point is behind screen plane -> don't draw anything
+        return;
+    }
+
+    // Intersect axis lines with screen plane (they may be outside)
+    std::vector<Point2f> imagePoints(4);
+    imagePoints[0] = Point2f{imagePointsZ[0].x, imagePointsZ[0].y};
+    imagePoints[1] = Point2f{imagePointsZ[1].x, imagePointsZ[1].y};
+    imagePoints[2] = Point2f{imagePointsZ[2].x, imagePointsZ[2].y};
+    imagePoints[3] = Point2f{imagePointsZ[3].x, imagePointsZ[3].y};
+
+    // draw axis lines
+    line(_image, imagePoints[0], imagePoints[1], Scalar(0, 0, 255), 3);
+    line(_image, imagePoints[0], imagePoints[2], Scalar(0, 255, 0), 3);
+    line(_image, imagePoints[0], imagePoints[3], Scalar(255, 0, 0), 3);
+}
+
+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() )
+    {
+        _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)));
+    }
+
+    _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/draw.h

@@ -3,4 +3,6 @@
 #include <opencv2/opencv.hpp>
 #include <opencv2/aruco.hpp>
 
-void drawPlanarBoard(cv::aruco::Board *_board, cv::Size outSize, cv::OutputArray _img, int marginSize, int borderBits);
+void _drawPlanarBoard(cv::aruco::Board *_board, cv::Size outSize, cv::OutputArray _img, int marginSize, int borderBits);
+void _drawAxis(cv::InputOutputArray image, cv::InputArray cameraMatrix, cv::InputArray distCoeffs,
+              cv::InputArray rvec, cv::InputArray tvec, float length);

aruco_pose/src/genmap.py

@@ -0,0 +1,45 @@
+#!/usr/bin/env python
+
+"""Markers map generator
+
+Generate map file for aruco_map nodelet.
+
+Usage:
+  genmap.py <length> <x> <y> <dist_x> <dist_y> <first> [--top-left]
+  genmap.py (-h | --help)
+
+Options:
+  <length>    Marker side length
+  <x>         Marker count along X axis
+  <y>         Marker count along Y axis
+  <dist_x>    Distance between markers along X axis
+  <dist_y>    Distance between markers along Y axis
+  <first>     First marker ID
+  --top-left  First marker is on top-left (not bottom-left)
+"""
+
+from __future__ import print_function
+
+from docopt import docopt
+
+
+arguments = docopt(__doc__)
+
+length = float(arguments['<length>'])
+first = int(arguments['<first>'])
+markers_x = int(arguments['<x>'])
+markers_y = int(arguments['<y>'])
+dist_x = float(arguments['<dist_x>'])
+dist_y = float(arguments['<dist_y>'])
+top_left = arguments['--top-left']
+
+max_y = (markers_y - 1) * dist_y
+
+for y in range(markers_y):
+    for x in range(markers_x):
+        pos_x = x * dist_x
+        pos_y = y * dist_y
+        if top_left:
+            pos_y = max_y - pos_y
+        print('{}\t{}\t{}\t{}\t{}\t{}\t{}\t{}\t'.format(first, length, pos_x, pos_y, 0, 0, 0, 0))
+        first += 1

aruco_pose/src/utils.h

@@ -1,5 +1,6 @@
 #pragma once
 
+#include <cmath>
 #include <ros/ros.h>
 #include <tf/transform_datatypes.h>
 #include <geometry_msgs/Quaternion.h>
@@ -90,14 +91,33 @@ inline void fillTranslation(geometry_msgs::Vector3& translation, const cv::Vec3d
 	translation.z = tvec[2];
 }
 
-inline void snapOrientation(geometry_msgs::Quaternion& to, const geometry_msgs::Quaternion& from)
+inline bool isFlipped(tf::Quaternion& q)
 {
-	tf::Quaternion q;
-	q.setRPY(0, 0, -tf::getYaw(to) + tf::getYaw(from));
-	tf::Quaternion pq;
-	tf::quaternionMsgToTF(from, pq);
-	pq = pq * q;
-	tf::quaternionTFToMsg(pq, to);
+	double yaw, pitch, roll;
+	tf::Matrix3x3(q).getEulerYPR(yaw, pitch, roll);
+	return (abs(pitch) > M_PI / 2) || (abs(roll) > M_PI / 2);
+}
+
+/* Set roll and pitch from "from" to "to", keeping yaw */
+inline void snapOrientation(geometry_msgs::Quaternion& to, const geometry_msgs::Quaternion& from, bool auto_flip = false)
+{
+	tf::Quaternion _from, _to;
+	tf::quaternionMsgToTF(from, _from);
+	tf::quaternionMsgToTF(to, _to);
+
+	if (auto_flip) {
+		if (!isFlipped(_from)) {
+			static const tf::Quaternion flip = tf::createQuaternionFromRPY(M_PI, 0, 0);
+			_from *= flip; // flip "from"
+		}
+	}
+
+	auto diff = tf::Matrix3x3(_to).transposeTimes(tf::Matrix3x3(_from));
+	double _, yaw;
+	diff.getRPY(_, _, yaw);
+	auto q = tf::createQuaternionFromRPY(0, 0, -yaw);
+	_from = _from * q; // set yaw from "to" to "from"
+	tf::quaternionTFToMsg(_from, to); // set "from" to "to"
 }
 
 inline void transformToPose(const geometry_msgs::Transform& transform, geometry_msgs::Pose& pose)

aruco_pose/test/basic.py

@@ -2,7 +2,6 @@
 import sys
 import unittest
 import json
-from pytest import approx
 import rospy
 import rostest
 
@@ -18,59 +17,82 @@ class TestArucoPose(unittest.TestCase):
 
     def test_markers(self):
         markers = rospy.wait_for_message('aruco_detect/markers', MarkerArray, timeout=5)
-        assert len(markers.markers) == 4
-        assert markers.header.frame_id == 'main_camera_optical'
+        self.assertEqual(len(markers.markers), 4)
+        self.assertEqual(markers.header.frame_id, 'main_camera_optical')
 
-        assert markers.markers[0].id == 2
-        assert markers.markers[0].pose.position.x == approx(0.36706567854)
-        assert markers.markers[0].pose.position.y == approx(0.290484516644)
-        assert markers.markers[0].pose.position.z == approx(2.18787602301)
-        assert markers.markers[0].pose.orientation.x == approx(0.993997406299)
-        assert markers.markers[0].pose.orientation.y == approx(-0.00532003481626)
-        assert markers.markers[0].pose.orientation.z == approx(-0.107390951553)
-        assert markers.markers[0].pose.orientation.w == approx(0.0201999263402)
-        assert markers.markers[0].c1.x == approx(415.557739258)
-        assert markers.markers[0].c1.y == approx(335.557739258)
-        assert markers.markers[0].c2.x == approx(509.442260742)
-        assert markers.markers[0].c2.y == approx(335.557739258)
-        assert markers.markers[0].c3.x == approx(509.442260742)
-        assert markers.markers[0].c3.y == approx(429.442260742)
-        assert markers.markers[0].c4.x == approx(415.557739258)
-        assert markers.markers[0].c4.y == approx(429.442260742)
+        self.assertEqual(markers.markers[0].id, 2)
+        self.assertAlmostEqual(markers.markers[0].length, 0.33, places=4)
+        self.assertAlmostEqual(markers.markers[0].pose.position.x, 0.36706567854, places=4)
+        self.assertAlmostEqual(markers.markers[0].pose.position.y, 0.290484516644, places=4)
+        self.assertAlmostEqual(markers.markers[0].pose.position.z, 2.18787602301, places=4)
+        self.assertAlmostEqual(markers.markers[0].pose.orientation.x, 0.993997406299, places=4)
+        self.assertAlmostEqual(markers.markers[0].pose.orientation.y, -0.00532003481626, places=4)
+        self.assertAlmostEqual(markers.markers[0].pose.orientation.z, -0.107390951553, places=4)
+        self.assertAlmostEqual(markers.markers[0].pose.orientation.w, 0.0201999263402, places=4)
+        self.assertAlmostEqual(markers.markers[0].c1.x, 415.557739258, places=4)
+        self.assertAlmostEqual(markers.markers[0].c1.y, 335.557739258, places=4)
+        self.assertAlmostEqual(markers.markers[0].c2.x, 509.442260742, places=4)
+        self.assertAlmostEqual(markers.markers[0].c2.y, 335.557739258, places=4)
+        self.assertAlmostEqual(markers.markers[0].c3.x, 509.442260742, places=4)
+        self.assertAlmostEqual(markers.markers[0].c3.y, 429.442260742, places=4)
+        self.assertAlmostEqual(markers.markers[0].c4.x, 415.557739258, places=4)
+        self.assertAlmostEqual(markers.markers[0].c4.y, 429.442260742, places=4)
 
-        assert markers.markers[3].id == 3
-        assert markers.markers[3].pose.position.x == approx(-0.1805169666)
-        assert markers.markers[3].pose.position.y == approx(-0.200697302327)
-        assert markers.markers[3].pose.position.z == approx(0.585767514823)
-        assert markers.markers[3].pose.orientation.x == approx(-0.961738074009)
-        assert markers.markers[3].pose.orientation.y == approx(-0.0375180244707)
-        assert markers.markers[3].pose.orientation.z == approx(-0.0115387773672)
-        assert markers.markers[3].pose.orientation.w == approx(0.271144115664)
-        assert markers.markers[3].c1.x == approx(129.557723999)
-        assert markers.markers[3].c1.y == approx(49.557723999)
-        assert markers.markers[3].c2.x == approx(223.442276001)
-        assert markers.markers[3].c2.y == approx(49.557723999)
-        assert markers.markers[3].c3.x == approx(223.442276001)
-        assert markers.markers[3].c3.y == approx(143.442276001)
-        assert markers.markers[3].c4.x == approx(129.557723999)
-        assert markers.markers[3].c4.y == approx(143.442276001)
+        self.assertEqual(markers.markers[3].id, 3)
+        self.assertAlmostEqual(markers.markers[3].length, 0.1, places=4)
+        self.assertAlmostEqual(markers.markers[3].pose.position.x, -0.1805169666, places=4)
+        self.assertAlmostEqual(markers.markers[3].pose.position.y, -0.200697302327, places=4)
+        self.assertAlmostEqual(markers.markers[3].pose.position.z, 0.585767514823, places=4)
+        self.assertAlmostEqual(markers.markers[3].pose.orientation.x, -0.961738074009, places=4)
+        self.assertAlmostEqual(markers.markers[3].pose.orientation.y, -0.0375180244707, places=4)
+        self.assertAlmostEqual(markers.markers[3].pose.orientation.z, -0.0115387773672, places=4)
+        self.assertAlmostEqual(markers.markers[3].pose.orientation.w, 0.271144115664, places=4)
+        self.assertAlmostEqual(markers.markers[3].c1.x, 129.557723999, places=4)
+        self.assertAlmostEqual(markers.markers[3].c1.y, 49.557723999, places=4)
+        self.assertAlmostEqual(markers.markers[3].c2.x, 223.442276001, places=4)
+        self.assertAlmostEqual(markers.markers[3].c2.y, 49.557723999, places=4)
+        self.assertAlmostEqual(markers.markers[3].c3.x, 223.442276001, places=4)
+        self.assertAlmostEqual(markers.markers[3].c3.y, 143.442276001, places=4)
+        self.assertAlmostEqual(markers.markers[3].c4.x, 129.557723999, places=4)
+        self.assertAlmostEqual(markers.markers[3].c4.y, 143.442276001, places=4)
 
-        assert markers.markers[1].id == 1
-        assert markers.markers[2].id == 4
+        self.assertEqual(markers.markers[1].id, 1)
+        self.assertAlmostEqual(markers.markers[1].length, 0.33, places=4)
+        self.assertEqual(markers.markers[2].id, 4)
+        self.assertAlmostEqual(markers.markers[2].length, 0.33, places=4)
 
     def test_visualization(self):
         vis = rospy.wait_for_message('aruco_detect/visualization', VisMarkerArray, timeout=5)
+        self.assertEqual(len(vis.markers), 9)
+
+    def test_debug(self):
+        img = rospy.wait_for_message('aruco_detect/debug', Image, timeout=5)
+        self.assertEqual(img.width, 640)
+        self.assertEqual(img.height, 480)
+        self.assertEqual(img.header.frame_id, 'main_camera_optical')
 
     def test_map(self):
         pose = rospy.wait_for_message('aruco_map/pose', PoseWithCovarianceStamped, timeout=5)
-        assert pose.header.frame_id == 'main_camera_optical'
-        assert pose.pose.pose.position.x == approx(-0.629167753342)
-        assert pose.pose.pose.position.y == approx(0.293822650809)
-        assert pose.pose.pose.position.z == approx(2.12641343155)
-        assert pose.pose.pose.orientation.x == approx(-0.998383794799)
-        assert pose.pose.pose.orientation.y == approx(-5.20919098575e-06)
-        assert pose.pose.pose.orientation.z == approx(-0.0300861070302)
-        assert pose.pose.pose.orientation.w == approx(0.0482143590507)
+        self.assertEqual(pose.header.frame_id, 'main_camera_optical')
+        self.assertAlmostEqual(pose.pose.pose.position.x, -0.629167753342, places=4)
+        self.assertAlmostEqual(pose.pose.pose.position.y, 0.293822650809, places=4)
+        self.assertAlmostEqual(pose.pose.pose.position.z, 2.12641343155, places=4)
+        self.assertAlmostEqual(pose.pose.pose.orientation.x, -0.998383794799, places=4)
+        self.assertAlmostEqual(pose.pose.pose.orientation.y, -5.20919098575e-06, places=4)
+        self.assertAlmostEqual(pose.pose.pose.orientation.z, -0.0300861070302, places=4)
+        self.assertAlmostEqual(pose.pose.pose.orientation.w, 0.0482143590507, places=4)
+
+    def test_map_image(self):
+        img = rospy.wait_for_message('aruco_map/image', Image, timeout=5)
+        self.assertEqual(img.width, 2000)
+        self.assertEqual(img.height, 2000)
+        self.assertEqual(img.encoding, 'mono8')
+
+    def test_map_visualization(self):
+        vis = rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5)
+
+    def test_map_debug(self):
+        img = rospy.wait_for_message('aruco_map/debug', Image, timeout=5)
 
     # def test_transforms(self):
     #     pass

aruco_pose/test/basic.test

@@ -20,7 +20,7 @@
         <remap from="camera_info" to="main_camera/camera_info"/>
         <remap from="markers" to="aruco_detect/markers"/>
         <param name="type" value="map"/>
-        <param name="map" value="$(find aruco_pose)/map/map.txt"/>
+        <param name="map" value="$(find aruco_pose)/test/basic.txt"/>
     </node>
 
     <test test-name="test_aruco_pose" pkg="aruco_pose" type="basic.py"/>

aruco_pose/test/basic.txt

@@ -0,0 +1,7 @@
+1	0.33	0	0	0	0	0	0
+2	0.33	1	0	0	0	0	0
+3	0.33	0	1	0	0	0	0
+4	0.33	1	1	0	0	0	0
+10	0.5	0.5	2	0	1.2	0	0
+11	0.2	0.5	0.5	0	0.0	-1	1
+12	0.4	0.2	0.5	0	0.1	-1.2	-0.5

builder/assets/99-px4fmu.rules

@@ -1,15 +1,15 @@
 # PixHawk (px4fmu-v2), px4fmu-v3
-SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0011", SYMLINK+="px4fmu"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0011", ATTRS{product}=="PX4 FMU v2.x", SYMLINK+="px4fmu"
 # PixRacer (px4fmu-v4)
-SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0012", SYMLINK+="px4fmu"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0012", ATTRS{product}=="PX4 FMU v4.x", SYMLINK+="px4fmu"
 # px4fmu-v5
-SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0012", SYMLINK+="px4fmu"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0032", ATTRS{product}=="PX4 FMU v5.x", SYMLINK+="px4fmu"
 # auav
-SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0021", SYMLINK+="px4fmu"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0021", ATTRS{product}=="PX4 AUAV x2.1", SYMLINK+="px4fmu"
 # crazyflie
-SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0016", SYMLINK+="px4fmu"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0016", ATTRS{product}=="PX4 Crazyflie v2.0", SYMLINK+="px4fmu"
 # px4fmu-v4pro
-SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0013", SYMLINK+="px4fmu"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0013", ATTRS{product}=="PX4 FMU v4.x PRO", SYMLINK+="px4fmu"
 # Omnibus
-SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0001", SYMLINK+="px4fmu"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0001", ATTRS{product}=="PX4 OmnibusF4SD", SYMLINK+="px4fmu"
 

builder/assets/clever.service

@@ -7,7 +7,8 @@ After=roscore.service
 User=pi
 EnvironmentFile=/lib/systemd/system/roscore.env
 ExecStart=/opt/ros/kinetic/bin/roslaunch clever clever.launch --wait --screen
-Restart=on-abort
+Restart=on-failure
+RestartSec=3
 
 [Install]
 WantedBy=multi-user.target

builder/assets/kinetic-rosdep-clever.yaml

@@ -537,4 +537,7 @@ interactive_marker_proxy:
     stretch: [ros-kinetic-interactive-marker-proxy]
 tf2_web_republisher:
   debian:
-    stretch: [ros-kinetic-tf2-web-republisher]
+    stretch: [ros-kinetic-tf2-web-republisher]
+image_publisher:
+  debian:
+    stretch: [ros-kinetic-image-publisher]

builder/assets/ros_python_paths

@@ -0,0 +1,8 @@
+Defaults        env_keep += "PYTHONPATH"
+Defaults        env_keep += "PATH"
+Defaults        env_keep += "ROS_ROOT"
+Defaults        env_keep += "ROS_MASTER_URI"
+Defaults        env_keep += "ROS_PACKAGE_PATH"
+Defaults        env_keep += "ROS_LOCATIONS"
+Defaults        env_keep += "ROS_HOME"
+Defaults        env_keep += "ROS_LOG_DIR"

builder/assets/roscore.service

@@ -6,7 +6,8 @@ After=network.target
 User=pi
 EnvironmentFile=/lib/systemd/system/roscore.env
 ExecStart=/opt/ros/kinetic/bin/roscore
-Restart=on-abort
+Restart=on-failure
+RestartSec=3
 
 [Install]
 WantedBy=multi-user.target

builder/image-build.sh

@@ -108,6 +108,7 @@ ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/clever.
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/roscore.env' '/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/kinetic-rosdep-clever.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/kinetic-ros-clever.rosinstall' '/home/pi/ros_catkin_ws/'
 # Add PX4 udev rules
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/99-px4fmu.rules' '/lib/udev/rules.d/'

builder/image-ros.sh

@@ -78,7 +78,7 @@ resolve_rosdep() {
 
   echo_stamp "Installing dependencies apps with rosdep in ${CATKIN_PATH}"
   cd ${CATKIN_PATH}
-  my_travis_retry rosdep install -y --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} -r --os=${OS_DISTRO}:${OS_VERSION}
+  my_travis_retry rosdep install -y --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} --os=${OS_DISTRO}:${OS_VERSION}
 }
 
 INSTALL_ROS_PACK_SOURCES=${INSTALL_ROS_PACK_SOURCES:='false'}
@@ -135,6 +135,8 @@ if [ "${INSTALL_ROS_PACK_SOURCES}" = "true" ]; then
   chown -Rf pi:pi /home/pi/ros_catkin_ws
 fi
 
+export ROS_IP='127.0.0.1' # needed for running tests
+
 echo_stamp "Installing CLEVER" \
 && cd /home/pi/catkin_ws/src/clever \
 && git status \
@@ -144,6 +146,8 @@ echo_stamp "Installing CLEVER" \
 && my_travis_retry pip install -r /home/pi/catkin_ws/src/clever/clever/requirements.txt \
 && source /opt/ros/kinetic/setup.bash \
 && catkin_make -j2 -DCMAKE_BUILD_TYPE=Release \
+&& catkin_make run_tests \
+&& catkin_test_results \
 && systemctl enable roscore \
 && systemctl enable clever \
 && echo_stamp "All CLEVER was installed!" "SUCCESS" \

builder/image-software.sh

@@ -91,7 +91,7 @@ tcpdump \
 ltrace \
 libpoco-dev=1.7.6+dfsg1-5+deb9u1 \
 python-rosdep \
-python-rosinstall-generator=0.1.14-1 \
+python-rosinstall-generator \
 python-wstool=0.1.17-1 \
 python-rosinstall=0.7.8-1 \
 build-essential=12.3 \
@@ -113,6 +113,7 @@ echo_stamp "Installing pip"
 curl https://bootstrap.pypa.io/get-pip.py -o get-pip.py
 python3 get-pip.py
 python get-pip.py
+rm get-pip.py
 #my_travis_retry pip install --upgrade pip
 #my_travis_retry pip3 install --upgrade pip
 
@@ -121,8 +122,10 @@ pip --version
 pip3 --version
 
 echo_stamp "Install and enable Butterfly (web terminal)"
-my_travis_retry pip3 install --prefer-binary butterfly
-my_travis_retry pip3 install --prefer-binary butterfly[systemd]
+echo_stamp "Workaround for tornado >= 6.0 breaking butterfly"
+my_travis_retry pip3 install tornado==5.1.1
+my_travis_retry pip3 install butterfly
+my_travis_retry pip3 install butterfly[systemd]
 systemctl enable butterfly.socket
 
 echo_stamp "Install ws281x library"
@@ -139,6 +142,7 @@ wget https://nodejs.org/dist/v10.15.0/node-v10.15.0-linux-armv6l.tar.gz
 tar -xzf node-v10.15.0-linux-armv6l.tar.gz
 cp -R node-v10.15.0-linux-armv6l/* /usr/local/
 rm -rf node-v10.15.0-linux-armv6l/
+rm node-v10.15.0-linux-armv6l.tar.gz
 
 echo_stamp "Add .vimrc"
 cat << EOF > /home/pi/.vimrc

builder/test/tests.sh

@@ -44,3 +44,5 @@ rosversion compressed_image_transport
 rosversion rosbridge_suite
 rosversion rosserial
 rosversion usb_cam
+rosversion cv_camera
+rosversion web_video_server

clever/launch/aruco.launch

@@ -3,6 +3,8 @@
     <arg name="aruco_map" default="false"/>
     <arg name="aruco_vpe" default="false"/>
 
+    <!-- For additional help go to https://clever.copterexpress.com/aruco.html -->
+
     <!-- 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">
         <remap from="image_raw" to="main_camera/image_raw"/>

clever/launch/main_camera.launch

@@ -21,11 +21,11 @@
         <param name="frame_id" value="main_camera_optical"/>
         <param name="camera_info_url" value="file://$(find clever)/camera_info/fisheye_cam_320.yaml"/>
 
-        <!-- setting camera FPS -->
-        <param name="rate" value="100"/>
-        <param name="cv_cap_prop_fps" value="40"/>
-        <param name="capture_delay" value="0.02"/>
+        <param name="rate" value="100"/> <!-- poll rate -->
+        <param name="cv_cap_prop_fps" value="40"/> <!-- camera FPS -->
+        <param name="capture_delay" value="0.02"/> <!-- approximate delay on frame retrieving -->
 
+        <!-- camera resolution, NOTE: camera_info file should match it -->
         <param name="image_width" value="320"/>
         <param name="image_height" value="240"/>
     </node>

clever/launch/mavros.launch

@@ -27,26 +27,28 @@
         <!-- basic params -->
         <rosparam command="load" file="$(find clever)/launch/mavros_config.yaml"/>
 
-        <rosparam param="plugin_blacklist">
-            - safety_area
-            - image_pub
-            - vibration
-            - rangefinder
-            - 3dr_radio
-            - actuator_control
-            - hil_controls
-            - vfr_hud
-            - vision_speed_estimate
-            - fake_gps
-            - cam_imu_sync
-            - hil
-            - adsb
-            - waypoint
-            - obstacle_distance
-            - setpoint_accel
-            - trajectory
-            - wind_estimation
-            - home_position
+        <rosparam param="plugin_whitelist">
+            - altitude
+            - command
+            - distance_sensor
+            - ftp
+            - global_position
+            - imu
+            - local_position
+            - manual_control
+            # - mocap_pose_estimate
+            - param
+            - px4flow
+            - rc_io
+            - setpoint_attitude
+            - setpoint_position
+            - setpoint_raw
+            - setpoint_velocity
+            - sys_status
+            - sys_time
+            - vision_pose_estimate
+            # - vision_speed_estimate
+            # - waypoint
         </rosparam>
     </node>
 

clever/requirements.txt

@@ -1,4 +1,5 @@
 flask==0.12.3
+docopt==0.6.2
 geopy==1.11.0
 pymavlink==2.2.10
 smbus2==0.2.1

clever/src/optical_flow.cpp

@@ -161,7 +161,12 @@ private:
 			flow_camera.header.stamp = msg->header.stamp;
 			flow_camera.vector.x = flow_y; // +y means counter-clockwise rotation around Y axis
 			flow_camera.vector.y = -flow_x; // +x means clockwise rotation around X axis
-			tf_buffer_.transform(flow_camera, flow_fcu, fcu_frame_id_);
+			try {
+				tf_buffer_.transform(flow_camera, flow_fcu, fcu_frame_id_);
+			} catch (const tf2::TransformException& e) {
+				// transform is not available yet
+				return;
+			}
 
 			// Calculate integration time
 			ros::Duration integration_time = msg->header.stamp - prev_stamp_;

clever/src/selfcheck.py

@@ -8,8 +8,10 @@ import rospy
 from std_srvs.srv import Trigger
 from sensor_msgs.msg import Image, CameraInfo, NavSatFix, Imu, Range
 from mavros_msgs.msg import State, OpticalFlowRad
-from geometry_msgs.msg import PoseStamped, TwistStamped
+from mavros_msgs.srv import ParamGet
+from geometry_msgs.msg import PoseStamped, TwistStamped, PoseWithCovarianceStamped
 import tf.transformations as t
+from aruco_pose.msg import MarkerArray
 
 
 # TODO: roscore is running
@@ -42,8 +44,10 @@ def check(name):
                 for f in failures:
                     rospy.logwarn('%s: %s', name, f)
             except Exception as e:
+                for f in failures:
+                    rospy.logwarn('%s: %s', name, f)
                 traceback.print_exc()
-                rospy.logwarn('%s: exception occured', name)
+                rospy.logwarn('%s: exception occurred', name)
                 return
             if not failures:
                 rospy.loginfo('%s: OK', name)
@@ -51,12 +55,44 @@ def check(name):
     return inner
 
 
+param_get = rospy.ServiceProxy('mavros/param/get', ParamGet)
+
+
+def get_param(name):
+    res = param_get(param_id=name)
+    if not res.success:
+        failure('Unable to retrieve PX4 parameter %s', name)
+    else:
+        if res.value.integer != 0:
+            return res.value.integer
+        return res.value.real
+
+
 @check('FCU')
 def check_fcu():
     try:
         state = rospy.wait_for_message('mavros/state', State, timeout=3)
         if not state.connected:
             failure('no connection to the FCU (check wiring)')
+
+        est = get_param('SYS_MC_EST_GROUP')
+        if est == 1:
+            rospy.loginfo('Selected estimator: LPE')
+            fuse = get_param('LPE_FUSION')
+            if fuse & (1 << 4):
+                rospy.loginfo('LPE_FUSION: land detector fusion is enabled')
+            else:
+                rospy.loginfo('LPE_FUSION: land detector fusion is disabled')
+            if fuse & (1 << 7):
+                rospy.loginfo('LPE_FUSION: barometer fusion is enabled')
+            else:
+                rospy.loginfo('LPE_FUSION: barometer fusion is disabled')
+
+        elif est == 2:
+            rospy.loginfo('Selected estimator: EKF2')
+        else:
+            failure('Unknown selected estimator: %s', est)
+
     except rospy.ROSException:
         failure('no MAVROS state (check wiring)')
 
@@ -80,12 +116,17 @@ def check_camera(name):
         failure('%s: calibration height doesn\'t match image height (%d != %d))', name, info.height, img.height)
 
 
-@check('Aruco detector')
+@check('ArUco detector')
 def check_aruco():
     try:
-        rospy.wait_for_message('aruco_pose/debug', Image, timeout=1)
+        rospy.wait_for_message('aruco_detect/markers', MarkerArray, timeout=1)
     except rospy.ROSException:
-        failure('no aruco_pose/debug messages')
+        failure('no markers detection')
+        return
+    try:
+        rospy.wait_for_message('aruco_map/pose', PoseWithCovarianceStamped, timeout=1)
+    except rospy.ROSException:
+        failure('no map detection')
 
 
 @check('Vision position estimate')
@@ -99,6 +140,37 @@ def check_vpe():
             failure('no VPE or MoCap messages')
             return
 
+    # check PX4 settings
+    est = get_param('SYS_MC_EST_GROUP')
+    if est == 1:
+        ext_yaw = get_param('ATT_EXT_HDG_M')
+        if ext_yaw != 1:
+            failure('vision yaw is disabled, change ATT_EXT_HDG_M parameter')
+        vision_yaw_w = get_param('ATT_W_EXT_HDG')
+        if vision_yaw_w == 0:
+            failure('vision yaw weight is zero, change ATT_W_EXT_HDG parameter')
+        else:
+            rospy.loginfo('Vision yaw weight: %.2f', vision_yaw_w)
+        fuse = get_param('LPE_FUSION')
+        if not fuse & (1 << 2):
+            failure('vision position fusing is disabled, change LPE_FUSION parameter')
+        delay = get_param('LPE_VIS_DELAY')
+        if delay != 0:
+            failure('LPE_VIS_DELAY parameter is %s, but it should be zero', delay)
+        rospy.loginfo('LPE_VIS_XY is %.2f m, LPE_VIS_Z is %.2f m', get_param('LPE_VIS_XY'), get_param('LPE_VIS_Z'))
+    elif est == 2:
+        fuse = get_param('EKF2_AID_MASK')
+        if not fuse & (1 << 3):
+            failure('vision position fusing is disabled, change EKF2_AID_MASK parameter')
+        if not fuse & (1 << 4):
+            failure('vision yaw fusing is disabled, change EKF2_AID_MASK parameter')
+        delay = get_param('EKF2_EV_DELAY')
+        if delay != 0:
+            failure('EKF2_EV_DELAY is %.2f, but it should be zero', delay)
+        rospy.loginfo('EKF2_EVA_NOISE is %.3f, EKF2_EVP_NOISE is %.3f',
+            get_param('EKF2_EVA_NOISE'),
+            get_param('EKF2_EVP_NOISE'))
+
     # check vision pose and estimated pose inconsistency
     try:
         pose = rospy.wait_for_message('mavros/local_position/pose', PoseStamped, timeout=1)
@@ -195,6 +267,42 @@ def check_optical_flow():
     # TODO:check FPS!
     try:
         rospy.wait_for_message('mavros/px4flow/raw/send', OpticalFlowRad, timeout=0.5)
+
+        # check PX4 settings
+        rot = get_param('SENS_FLOW_ROT')
+        if rot != 0:
+            failure('SENS_FLOW_ROT parameter is %s, but it should be zero', rot)
+        est = get_param('SYS_MC_EST_GROUP')
+        if est == 1:
+            fuse = get_param('LPE_FUSION')
+            if not fuse & (1 << 1):
+                failure('optical flow fusing is disabled, change LPE_FUSION parameter')
+            if not fuse & (1 << 1):
+                failure('flow gyro compensation is disabled, change LPE_FUSION parameter')
+            scale = get_param('LPE_FLW_SCALE')
+            if scale != 0:
+                failure('LPE_FLW_SCALE parameter is %.2f, but it should be 1.0', scale)
+
+            rospy.loginfo('LPE_FLW_QMIN is %s, LPE_FLW_R is %.4f, LPE_FLW_RR is %.4f, SENS_FLOW_MINHGT is %.3f, SENS_FLOW_MAXHGT is %.3f',
+                get_param('LPE_FLW_QMIN'),
+                get_param('LPE_FLW_R'),
+                get_param('LPE_FLW_RR'),
+                get_param('SENS_FLOW_MINHGT'),
+                get_param('SENS_FLOW_MAXHGT'))
+        elif est == 2:
+            fuse = get_param('EKF2_AID_MASK')
+            if not fuse & (1 << 1):
+                failure('optical flow fusing is disabled, change EKF2_AID_MASK parameter')
+            delay = get_param('EKF2_OF_DELAY')
+            if delay != 0:
+                failure('EKF2_OF_DELAY is %.2f, but it should be zero', delay)
+            rospy.loginfo('EKF2_OF_QMIN is %s, EKF2_OF_N_MIN is %.4f, EKF2_OF_N_MAX is %.4f, SENS_FLOW_MINHGT is %.3f, SENS_FLOW_MAXHGT is %.3f',
+                get_param('EKF2_OF_QMIN'),
+                get_param('EKF2_OF_N_MIN'),
+                get_param('EKF2_OF_N_MAX'),
+                get_param('SENS_FLOW_MINHGT'),
+                get_param('SENS_FLOW_MAXHGT'))
+
     except rospy.ROSException:
         failure('no optical flow data (from Raspberry)')
 
@@ -202,15 +310,42 @@ def check_optical_flow():
 @check('Rangefinder')
 def check_rangefinder():
     # TODO: check FPS!
+    rng = False
     try:
-        rospy.wait_for_message('mavros/distance_sensor/rangefinder_3_sub', Range, timeout=0.5)
+        rospy.wait_for_message('mavros/distance_sensor/rangefinder_sub', Range, timeout=0.5)
+        rng = True
     except rospy.ROSException:
-        failure('no randefinder data from Raspberry')
+        failure('no rangefinder data from Raspberry')
+
     try:
-        rospy.wait_for_message('mavros/distance_sensor/rangefinder_0', Range, timeout=0.5)
+        rospy.wait_for_message('mavros/distance_sensor/rangefinder', Range, timeout=0.5)
+        rng = True
     except rospy.ROSException:
         failure('no rangefinder data from PX4')
 
+    if not rng:
+        return
+
+    est = get_param('SYS_MC_EST_GROUP')
+    if est == 1:
+        fuse = get_param('LPE_FUSION')
+        if not fuse & (1 << 5):
+            rospy.loginfo('"pub agl as lpos down" in LPE_FUSION is disabled, NOT operating over flat surface')
+        else:
+            rospy.loginfo('"pub agl as lpos down" in LPE_FUSION is enabled, operating over flat surface')
+
+    elif est == 2:
+        hgt = get_param('EKF2_HGT_MODE')
+        if hgt != 2:
+            rospy.loginfo('EKF2_HGT_MODE != Range sensor, NOT operating over flat surface')
+        else:
+            rospy.loginfo('EKF2_HGT_MODE = Range sensor, operating over flat surface')
+        aid = get_param('EKF2_RNG_AID')
+        if aid != 1:
+            rospy.loginfo('EKF2_RNG_AID != 1, range sensor aiding disabled')
+        else:
+            rospy.loginfo('EKF2_RNG_AID = 1, range sensor aiding enabled')
+
 
 @check('Boot duration')
 def check_boot_duration():

clever/src/simple_offboard.cpp

@@ -454,10 +454,10 @@ inline void checkState()
 		throw std::runtime_error("No connection to FCU, https://clever.copterexpress.com/connection.html");
 }
 
-inline void serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, float vy, float vz,
-                  float pitch, float roll, float yaw, float pitch_rate, float roll_rate, float yaw_rate,
-                  float lat, float lon, float thrust, float speed, string frame_id, bool auto_arm,
-                  uint8_t& success, string& message)
+bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, float vy, float vz,
+           float pitch, float roll, float yaw, float pitch_rate, float roll_rate, float yaw_rate,
+           float lat, float lon, float thrust, float speed, string frame_id, bool auto_arm,
+           uint8_t& success, string& message)
 {
 	auto stamp = ros::Time::now();
 
@@ -528,12 +528,13 @@ inline void serve(enum setpoint_type_t sp_type, float x, float y, float z, float
 			nav_speed = speed;
 		}
 
-		if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == POSITION || sp_type == VELOCITY) {
-			if (std::isnan(yaw) && yaw_rate == 0) {
-				// keep yaw unchanged
-				yaw = tf2::getYaw(local_position.pose.orientation);
-			}
-		}
+		// if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == POSITION || sp_type == VELOCITY) {
+		// 	if (std::isnan(yaw) && yaw_rate == 0) {
+		// 		// keep yaw unchanged
+		//		// TODO: this is incorrect, because we need yaw in desired frame
+		// 		yaw = tf2::getYaw(local_position.pose.orientation);
+		// 	}
+		// }
 
 		if (sp_type == POSITION || sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == VELOCITY || sp_type == ATTITUDE) {
 			// destination point and/or yaw
@@ -601,42 +602,36 @@ inline void serve(enum setpoint_type_t sp_type, float x, float y, float z, float
 		message = e.what();
 		ROS_INFO("simple_offboard: %s", message.c_str());
 		busy = false;
-		return;
+		return true;
 	}
 
 	success = true;
 	busy = false;
-	return;
+	return true;
 }
 
 bool navigate(Navigate::Request& req, Navigate::Response& res) {
-	serve(NAVIGATE, req.x, req.y, req.z, 0, 0, 0, 0, 0, req.yaw, 0, 0, req.yaw_rate, 0, 0, 0, req.speed, req.frame_id, req.auto_arm, res.success, res.message);
-	return true;
+	return serve(NAVIGATE, req.x, req.y, req.z, 0, 0, 0, 0, 0, req.yaw, 0, 0, req.yaw_rate, 0, 0, 0, req.speed, req.frame_id, req.auto_arm, res.success, res.message);
 }
 
 bool navigateGlobal(NavigateGlobal::Request& req, NavigateGlobal::Response& res) {
-	serve(NAVIGATE_GLOBAL, 0, 0, req.z, 0, 0, 0, 0, 0, req.yaw, 0, 0, req.yaw_rate, req.lat, req.lon, 0, req.speed, req.frame_id, req.auto_arm, res.success, res.message);
-	return true;
+	return serve(NAVIGATE_GLOBAL, 0, 0, req.z, 0, 0, 0, 0, 0, req.yaw, 0, 0, req.yaw_rate, req.lat, req.lon, 0, req.speed, req.frame_id, req.auto_arm, res.success, res.message);
 }
 
 bool setPosition(SetPosition::Request& req, SetPosition::Response& res) {
-	serve(POSITION, req.x, req.y, req.z, 0, 0, 0, 0, 0, req.yaw, 0, 0, req.yaw_rate, 0, 0, 0, 0, req.frame_id, req.auto_arm, res.success, res.message);
-	return true;
+	return serve(POSITION, req.x, req.y, req.z, 0, 0, 0, 0, 0, req.yaw, 0, 0, req.yaw_rate, 0, 0, 0, 0, req.frame_id, req.auto_arm, res.success, res.message);
 }
 
 bool setVelocity(SetVelocity::Request& req, SetVelocity::Response& res) {
-	serve(VELOCITY, 0, 0, 0, req.vx, req.vy, req.vz, 0, 0, req.yaw, 0, 0, req.yaw_rate, 0, 0, 0, 0, req.frame_id, req.auto_arm, res.success, res.message);
-	return true;
+	return serve(VELOCITY, 0, 0, 0, req.vx, req.vy, req.vz, 0, 0, req.yaw, 0, 0, req.yaw_rate, 0, 0, 0, 0, req.frame_id, req.auto_arm, res.success, res.message);
 }
 
 bool setAttitude(SetAttitude::Request& req, SetAttitude::Response& res) {
-	serve(ATTITUDE, 0, 0, 0, 0, 0, 0, req.pitch, req.roll, req.yaw, 0, 0, 0, 0, 0, req.thrust, 0, req.frame_id, req.auto_arm, res.success, res.message);
-	return true;
+	return serve(ATTITUDE, 0, 0, 0, 0, 0, 0, req.pitch, req.roll, req.yaw, 0, 0, 0, 0, 0, req.thrust, 0, req.frame_id, req.auto_arm, res.success, res.message);
 }
 
 bool setRates(SetRates::Request& req, SetRates::Response& res) {
-	serve(RATES, 0, 0, 0, 0, 0, 0, 0, 0, 0, req.pitch_rate, req.roll_rate, req.yaw_rate, 0, 0, req.thrust, 0, "", req.auto_arm, res.success, res.message);
-	return true;
+	return serve(RATES, 0, 0, 0, 0, 0, 0, 0, 0, 0, req.pitch_rate, req.roll_rate, req.yaw_rate, 0, 0, req.thrust, 0, "", req.auto_arm, res.success, res.message);
 }
 
 bool land(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)

clever/src/vpe_publisher.cpp

@@ -116,7 +116,7 @@ int main(int argc, char **argv) {
 	nh_priv.param<string>("offset_frame_id", offset_frame_id, "");
 	nh_priv.param<string>("mavros/local_position/frame_id", local_frame_id, "map");
 	nh_priv.param<string>("mavros/local_position/tf/child_frame_id", child_frame_id, "base_link");
-	offset_timeout = ros::Duration(nh_priv.param("offset_timeout", 5.0));
+	offset_timeout = ros::Duration(nh_priv.param("offset_timeout", 3.0));
 
 	if (!frame_id.empty()) {
 		ROS_INFO("vpe_publisher: using data from TF");

clever/www/aruco_map.html

@@ -0,0 +1,19 @@
+<!DOCTYPE html>
+<html>
+	<head>
+		<script type="text/javascript" src="js/three.min.js"></script>
+		<script type="text/javascript" src="js/eventemitter2.js"></script>
+		<script type="text/javascript" src="js/roslib.js"></script>
+		<script type="text/javascript" src="js/ros3d.js"></script>
+		<title>Aruco Map visualization</title>
+	</head>
+	<body>
+		<div id="viz"></div>
+		<script type="text/javascript" src="js/viz.js"></script>
+		<script>
+			setScene('aruco_map');
+			addArucoMap();
+			addAxes();
+		</script>
+	</body>
+</html>

clever/www/index.html

@@ -5,6 +5,7 @@
 	<li><a href="" id="wvs">View image topics</a> (<code>web_video_server</code>)</li>
 	<li><a href="" id="butterfly">Open web terminal</a> (<code>Butterfly</code>)</li>
 	<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>
 </ul>
 
 <script type="text/javascript">

clever/www/js/viz.js

@@ -20,50 +20,75 @@ ros.on('close', function() {
 	titleEl.innerText = 'Disconnected';
 });
 
-var viewer = new ROS3D.Viewer({
-	divID: 'viz',
-	width: 1000,
-	height: 600,
-	antialias: true
-});
+var viewer, tfClient;
 
-var tfClient = new ROSLIB.TFClient({
-	ros: ros,
-	angularThres: 0.01,
-	transThres: 0.01,
-	rate: 10.0,
-	fixedFrame : 'map'
-});
+function setScene(fixedFrame) {
+	viewer = new ROS3D.Viewer({
+		divID: 'viz',
+		width: 1000,
+		height: 600,
+		antialias: true
+	});
 
-// vehicle markers
-var vehicleMarkers = new ROS3D.MarkerArrayClient({
-	ros: ros,
-	tfClient: tfClient,
-	topic: '/vehicle_marker',
-	rootObject: viewer.scene
-});
+	tfClient = new ROSLIB.TFClient({
+		ros: ros,
+		angularThres: 0.01,
+		transThres: 0.01,
+		rate: 10.0,
+		fixedFrame : fixedFrame
+	});
 
-// camera markers
-var cameraMarkers = new ROS3D.MarkerArrayClient({
-	ros: ros,
-	tfClient: tfClient,
-	topic: '/main_camera/camera_markers',
-	rootObject: viewer.scene
-});
+	var map = new ROS3D.Grid({
+		ros: ros,
+		tfClient: tfClient,
+		rootObject: viewer.scene
+	});
 
-// detected aruco markers
-var cameraMarkers = new ROS3D.MarkerArrayClient({
-	ros: ros,
-	tfClient: tfClient,
-	topic: '/aruco_detect/visualization',
-	rootObject: viewer.scene
-});
+	viewer.scene.add(map);
+}
 
-var map = new ROS3D.Grid({
-	ros: ros,
-	tfClient: tfClient,
-	// frameID: 'map',
-	rootObject: viewer.scene
-});
+function addAxes() {
+	var axes = new ROS3D.Axes({
+		ros: ros,
+		tfClient: tfClient,
+		rootObject: viewer.scene
+	});
+	viewer.scene.add(axes);
+}
 
-viewer.scene.add(map);
+function addVehicle() {
+	new ROS3D.MarkerArrayClient({
+		ros: ros,
+		tfClient: tfClient,
+		topic: '/vehicle_marker',
+		rootObject: viewer.scene
+	});
+}
+
+
+function addCamera() {
+	new ROS3D.MarkerArrayClient({
+		ros: ros,
+		tfClient: tfClient,
+		topic: '/main_camera/camera_markers',
+		rootObject: viewer.scene
+	});
+}
+
+function addAruco() {
+	new ROS3D.MarkerArrayClient({
+		ros: ros,
+		tfClient: tfClient,
+		topic: '/aruco_detect/visualization',
+		rootObject: viewer.scene
+	});
+}
+
+function addArucoMap() {
+	new ROS3D.MarkerArrayClient({
+		ros: ros,
+		tfClient: tfClient,
+		topic: '/aruco_map/visualization',
+		rootObject: viewer.scene
+	});
+}

clever/www/viz.html

@@ -10,5 +10,11 @@
 	<body>
 		<div id="viz"></div>
 		<script type="text/javascript" src="js/viz.js"></script>
+		<script>
+			setScene('map');
+			addVehicle();
+			addCamera();
+			addAruco();
+		</script>
 	</body>
 </html>

docs/ru/README.md

@@ -5,7 +5,7 @@
 
 «Клевер» — это учебный конструктор программируемого квадрокоптера, состоящего из популярных открытых компонентов, а также набор необходимой документации и библиотек для работы с ним.
 
-Набор включает в себя полетный контроллер Pixhawk/Pixracer с полетным стеком PX4, Raspberry Pi 3 в качестве управлящего бортового компьютера, модуль камеры для реализации полетов с использованием компьютерного зрения, а также набор различных датчиков и другой периферии.
+Набор включает в себя полетный контроллер Pixhawk/Pixracer с полетным стеком PX4, Raspberry Pi 3 в качестве управляющего бортового компьютера, модуль камеры для реализации полетов с использованием компьютерного зрения, а также набор различных датчиков и другой периферии.
 
 На базе точно такой же платформы были созданы многие «большие» проекты компании Copter Express, например, дроны для [пиар-акций по автономной доставке пиццы](https://www.youtube.com/watch?v=hmkAoZOtF58) (Самара, Казань); дрон-доставщик кофе в Сколково, мониторинговый дрон с зарядной станцией, дроны-победители на полевых испытаниях «[Робокросс-2016](https://www.youtube.com/watch?v=dGbDaz_VmYU)», «[Робокросс-2017](https://youtu.be/AQnd2CRczbQ)» и многие другие.
 
@@ -15,6 +15,8 @@
 
 Также у нас есть чат для программистов, которые разрабатывают под PX4, автономную навигацию в помещениях и рои дронов https://t.me/DroneCode.
 
+The English version of this documentation [is available](../en/).
+
 Образ для Raspberry Pi
 ----------------------
 

docs/ru/SUMMARY.md

@@ -36,7 +36,10 @@
   * [ROS](ros.md)
   * [MAVROS](mavros.md)
   * [Автономный полет в OFFBOARD](simple_offboard.md)
-  * [Навигация по ArUco-маркерам](aruco.md)
+  * Визуальные маркеры (ArUco)
+    * [Общая информация](aruco.md)
+    * [Распознавание маркеров](aruco_marker.md)
+    * [Распознавание карт маркеров](aruco_map.md)
   * [Навигация по Optical Flow](optical_flow.md)
   * [Автоматическая проверка](selfcheck.md)
   * [Примеры кода](snippets.md)
@@ -47,6 +50,7 @@
   * [Ультразвуковой дальномер](sonar.md)
   * [Лазерный дальномер](laser.md)
   * [Работа с SITL](sitl.md)
+  * [Контейнер с симулятором](sitl_docker.md)
   * [Автозапуск ПО](autolaunch.md)
   * [Взаимодействие с Arduino](arduino.md)
   * [3G-модем](3g.md)
@@ -54,6 +58,7 @@
   * [Шаровая защита коптера](shield.md)
   * [Распознавание лиц](face_recognition.md)
   * [Пульт на Андроид](android.md)
+  * [Блочный конструктор полета](clever_blocks.md)
   * [CopterHack-2018](copterhack2018.md)
   * [CopterHack-2017](copterhack2017.md)
 * Дополнительные материалы

docs/ru/aruco.md

@@ -1,172 +1,23 @@
-# Навигация с использованием ArUco-маркеров
+# ArUco-маркеры
 
-> **Note** Документация для версий [образа](microsd_images.md), начиная с **0.15**. Для более ранних версий см. [документацию для версии **0.14**](https://github.com/CopterExpress/clever/blob/v0.14/docs/ru/aruco.md).
+> **Note** Документация для версий [образа](microsd_images.md), начиная с версии **0.16**. Для более ранних версий см. [документацию для версии **0.15.1**](https://github.com/CopterExpress/clever/blob/v0.15.1/docs/ru/aruco.md).
 
 [ArUco-маркеры](https://docs.opencv.org/3.2.0/d5/dae/tutorial_aruco_detection.html) — это популярная технология для позиционирования
 робототехнических систем с использованием компьютерного зрения.
 
-Пример ArUco-маркеров:
-
 ![ArUco-маркеры](../assets/markers.jpg)
 
 > **Hint** При печати визуальных маркеров необходимо использовать максимально матовую бумагу. Глянцевая бумага будет бликовать на свету, сильно ухудшая качество распознавания.
 
 Для быстрого генерирования маркеров для печати можно использовать онлайн-инструмент: http://chev.me/arucogen/.
 
-## aruco\_pose
+На [образе Клевера для RPi](microsd_images.md) предустановлен пакет `aruco_pose`, предназначенный для работы с ArUco-маркерами.
 
-Модуль `aruco_pose` позволяет восстанавливать позицию коптера относительно карты ArUco-маркеров и сообщать ее полетному контролеру, используя механизм [Vision Position Estimation](https://dev.px4.io/en/ros/external_position_estimation.html).
+## Режимы работы
 
-При наличии источника положения коптера по маркерам появляется возможность производить точную автономную indoor-навигацию по позициям при помощи модуля [simple\_offboard](simple_offboard.md).
+Клевер имеет несколько преднастроенных режимов работы с ArUco-маркерами:
 
-### Включение
+* [распознавание и навигация по отдельным маркерам](aruco_marker.md);
+* [распознавание и навигация по картам маркеров](aruco_map.md).
 
-Необходимо убедиться, что в launch-файле Клевера \(`~/catkin_ws/src/clever/clever/launch/clever.launch`\) включен запуск aruco\_pose и [камеры для компьютерного зрения](camera.md):
-
-```xml
-<arg name="main_camera" default="true"/>
-```
-
-```xml
-<arg name="aruco" default="true"/>
-```
-
-При изменении launch-файла необходимо перезапустить пакет `clever`:
-
-```bash
-sudo systemctl restart clever
-```
-
-### Настройка карты ArUco-меток
-
-В качестве карты меток можно использовать автоматически сгенерированный [ArUco-board](https://docs.opencv.org/trunk/db/da9/tutorial_aruco_board_detection.html).
-
-Настройка карты меток производится с помощью файла `~/catkin_ws/src/clever/clever/launch/aruco.launch`. Для использования ArUco-board введите его параметры:
-
-```xml
-<node pkg="nodelet" type="nodelet" name="aruco_pose" args="load aruco_pose/aruco_pose nodelet_manager">
-    <param name="frame_id" value="aruco_map_raw"/>
-    <!-- тип маркерного поля -->
-    <param name="type" value="gridboard"/>
-
-    <!-- количество маркеров по x -->
-    <param name="markers_x" value="1"/>
-
-    <!-- количество маркеров по y -->
-    <param name="markers_y" value="6"/>
-
-    <!-- ID маркера первого маркера (левого верхнего) -->
-    <param name="first_marker" value="240"/>
-
-    <!-- длина стороны маркера в метрах -->
-    <param name="markers_side" value="0.3362"/>
-
-    <!-- расстояние между маркерами -->
-    <param name="markers_sep" value="0.46"/>
-</node>
-```
-
-Можно задать отдельно расстояние между маркерами по горизонтали и вертикали:
-
-```xml
-<!-- расстояние между маркерами по горизонтали -->
-<param name="markers_sep_x" value="0.97"/>
-
-<!-- расстояние между маркерами по вертикали -->
-<param name="markers_sep_y" value="1.435"/>
-```
-
-Если используется карта с нестандартным порядком ID меток, то можно использовать параметр `marker_ids`:
-
-```xml
-<rosparam param="marker_ids">[5, 7, 9, 11, 13, 15]</rosparam>
-```
-
-Нумерация маркеров ведется с левого верхнего угла поля.
-
-Для контроля карты, по которой в данный момент коптер осуществляет навигацию, можно просмотреть содержимое топика `aruco_pose/map_image`. Через браузер его можно просмотреть при помощи [web\_video\_server](web_video_server.md) по ссылке [http://192.168.11.1:8080/snapshot?topic=/aruco\_pose/map\_image](http://192.168.11.1:8080/snapshot?topic=/aruco_pose/map_image):
-
-![](../../assets/Снимок экрана 2017-11-27 в 23.20.49.png)
-
-При полетах необходимо убедиться, что наклеенные на пол метки соответствуют карте.
-
-В топике `aruco_pose/debug` \([http://192.168.11.1:8080/snapshot?topic=/aruco\_pose/debug](http://192.168.11.1:8080/snapshot?topic=/aruco_pose/debug)\) доступен текущий результат распознавания меток:
-
-TODO
-
-### Система координат
-
-По [соглашению](http://www.ros.org/reps/rep-0103.html), в маркерном поле используется стандартная система координат ENU:
-
-* x — вправо \(условный "восток"\);
-* y — вперед \(условный "север"\);
-* z — вверх.
-
-_Примечание_: указанное выше определение приведено для ситуации, когда поле маркеров лежит на полу.
-
-Таким образом, нулевой является левая нижняя точка маркерного поля. Угол по рысканью считается равным 0, когда коптер смотрит направо \(по оси x\).
-
-![Система координат маркеров](../assets/aruco-frame.png)
-
-### Настройка полетного контролера
-
-Для правильной работы Vision Position Estimation необходимо \(через [QGroundControl](gcs_bridge.md)\) убедиться, что:
-
-* **Для Pixhawk**: Установлена прошивка с LPE \(local position estimator\). Для Pixhawk необходимо [скачать прошивку `px4fmu-v2_lpe.px4`](https://github.com/PX4/Firmware/releases).
-
-  **Для Pixracer**: параметр `SYS_MC_EST_GROUP` должен быть установлен в `local_position_estimator, attitude_estimator_q`.
-
-  > **Note** После изменения значения параметра `SYS_MC_EST_GROUP` необходимо перезагрузить полетный контроллер.
-* В параметре `LPE_FUSION` включены **только** флажки `vision position`, `land detector`. Итоговое значение _20_.
-* Выключен компас: `ATT_W_MAG` = 0
-* Вес угла по рысканью по зрению: `ATT_W_EXT_HDG` = 0.5
-* Включена ориентация по Yaw по зрению: `ATT_EXT_HDG_M` = 2 `MOCAP`.
-* Настройки VPE: `LPE_VIS_DELAY` = 0 sec, `LPE_VIS_XY` = 0.1 m, `LPE_VIS_Z` = 0.15 m.
-* Рекомендуемые настройки land detector'а:
-  * `COM_DISARM_LAND` = 1 s
-  * `LNDMC_ROT_MAX` = 45 deg
-  * `LNDMC_THR_RANGE` = 0.5
-  * `LNDMC_Z_VEL_MAX` = 1 m/s
-
-<!--
-Для простоты настройки можно воспользоваться готовым файлом настроек для [Clever 2](https://github.com/CopterExpress/clever/blob/master/docs/assets/Clever2LPE_160118.params) или для [Clever 3](https://github.com/CopterExpress/clever/blob/master/docs/assets/Clever3_LPE_020218.params) и вгрузить его в контроллер с помощью меню Tools - Load from file из раздела Parameters в QGroundControl.
-
-![](../assets/Screenshot from 2018-02-27 22-30-50.png)
--->
-
-### Полет
-
-При правильной настройке коптер начнет удерживать позицию по VPE \(в [режимах](modes.md) `POSCTL` или `OFFBOARD`\) автоматически.
-
-Для [автономных полетов](simple_offboard.md) можно будет использовать функции `navigate`, `set_position`, `set_velocity`. Для полета в определенные координаты маркерного поля необходимо использовать фрейм `aruco_map`:
-
-```python
-# Вначале необходимо взлететь, чтобы коптер увидел карту меток
-# и появился фрейм aruco_map:
-navigate(0, 0, 2, frame_id='body', speed=0.5, auto_arm=True) #  взлет на 2 метра
-
-time.sleep(5)
-
-# Полет в координату 2:2 маркерного поля, высота 2 метра
-navigate(2, 2, 2, speed=1, frame_id='aruco_map')  #  полет в координату 2:2, высота 3 метра
-```
-
-См. [другие функции](simple_offboard.md) simple_offboard.
-
-### Расположение маркеров на потолке
-
-![Маркеры на потолке](../assets/IMG_4175.JPG)
-
-Для навигации по маркерам, расположенным на потолке, необходимо поставить основную камеру так, чтобы она смотрела вверх и [установить соответствующий фрейм камеры](camera_frame.md).
-
-Чтобы задавать карту маркеров в "перевернутой" системе координат, необходимо изменить параметр `aruco_orientation` в файле `~/catkin_ws/src/clever/clever/aruco.launch`:
-
-```xml
-<param name="aruco_orientation" value="map_upside_down"/>
-```
-
-При задании вышеуказанного параметра фрейм aruco\_map также окажется "перевернутым". Таким образом, для полета на высоту 2 метра ниже потолка, аргумент `z` нужно устанавливать в 2:
-
-```python
-navigate(x=1, y=2, z=1.1, speed=0.5, frame_id='aruco_map')
-```
+> **Info** Исчерпывающую документацию по пакету `aruco_pose` на английском языке можно посмотреть [на GitHub](https://github.com/CopterExpress/clever/blob/master/aruco_pose/README.md).

docs/ru/aruco_map.md

@@ -6,9 +6,10 @@
 
 ## Конфигурирование
 
-Для включения распознавания карт маркеров аргумент `aruco_map` в файле `~/catkin_ws/src/clever/clever/launch/aruco.launch` должен быть в значении `true`:
+Для включения распознавания карт маркеров аргументы `aruco_map` и `aruco_detect` в файле `~/catkin_ws/src/clever/clever/launch/aruco.launch` должны быть в значении `true`:
 
 ```xml
+<arg name="aruco_detect" default="true"/>
 <arg name="aruco_map" default="true"/>
 ```
 
@@ -36,6 +37,20 @@ id_маркера размер_маркера x y z угол_z угол_y уго
 
 Смотрите примеры карт маркеров в каталоге [`~/catkin_ws/src/clever/aruco_pose/map`](https://github.com/CopterExpress/clever/tree/master/aruco_pose/map).
 
+Файл карты может быть сгенерирован с помощью инструмента `genmap.py`:
+
+```bash
+rosrun aruco_pose genmap.py length x y dist_x dist_y first > ~/catkin_ws/src/clever/aruco_pose/map/test_map.txt
+```
+
+Где `length` – размер маркера, `x` – количество маркеров по оси *x*, `y` - количество маркеров по оси *y*, `dist_x` – расстояние между центрами маркеров по оси *x*, `y` – расстояние между центрами маркеров по оси *y*, `first` – ID первого (левого нижнего) маркера, `test_map.txt` – название файла с картой. Дополнительный ключ `--top-left` позволяет нумеровать маркеры с левого верхнего угла.
+
+Пример:
+
+```bash
+rosrun aruco_pose genmap.py 0.33 2 4 1 1 0 > ~/catkin_ws/src/clever/aruco_pose/map/test_map.txt
+```
+
 ### Проверка
 
 Для контроля карты, по которой в данный момент коптер осуществляет навигацию, можно просмотреть содержимое топика `/aruco_map/image`. Через браузер его можно просмотреть при помощи [web_video_server](web_video_server.md) по ссылке http://192.168.11.1:8080/snapshot?topic=/aruco_map/image:
@@ -66,7 +81,7 @@ id_маркера размер_маркера x y z угол_z угол_y уго
 
 * В параметре `EKF2_AID_MASK` включены флажки `vision position fusion`, `vision yaw fusion`.
 * Шум угла по зрению: `EKF2_EVA_NOISE` = 0.1 rad
-* Шум позиции по зрению: `EKF2_EVP_NOISE` = 0.05 m
+* Шум позиции по зрению: `EKF2_EVP_NOISE` = 0.1 m
 * `EKF2_EV_DELAY` = 0
 
 При использовании **LPE** (параметр `SYS_MC_EST_GROUP` = `local_position_estimator, attitude_estimator_q`):
@@ -74,11 +89,13 @@ id_маркера размер_маркера x y z угол_z угол_y уго
 * В параметре `LPE_FUSION` включены флажки `vision position`, `land detector`.
 * Вес угла по рысканью по зрению: `ATT_W_EXT_HDG` = 0.5
 * Включена ориентация по Yaw по зрению: `ATT_EXT_HDG_M` = 1 `Vision`.
-* Шумы позиции по зрению: `LPE_VIS_XY` = 0.05 m, `LPE_VIS_Z` = 0.05 m.
+* Шумы позиции по зрению: `LPE_VIS_XY` = 0.1 m, `LPE_VIS_Z` = 0.1 m.
 * `LPE_VIS_DELAY` = 0 sec
 
 <!-- * Выключен компас: `ATT_W_MAG` = 0 -->
 
+Для проверки правильности всех настроек можно [воспользоваться утилитой `selfcheck.py`](selfcheck.md).
+
 > **Info** Для использования LPE в Pixhawk необходимо [скачать прошивку с названием `px4fmu-v2_lpe.px4`](https://github.com/PX4/Firmware/releases).
 
 ## Полет
@@ -99,8 +116,12 @@ navigate(2, 2, 2, speed=1, frame_id='aruco_map')  # полет в координ
 
 ## Дополнительные настройки
 
+<!-- TODO: статья по пидам -->
+
 Если коптер нестабильно удерживает позицию по VPE, попробуйте увеличить коэффициенты *P* PID-регулятора по скорости – параметры `MPC_XY_VEL_P` и `MPC_Z_VEL_P`.
 
+Если коптер нестабильно удерживает высоту, попробуйте увеличить коэффициент `MPC_Z_VEL_P` или лучше подобрать газ висения – `MPC_THR_HOVER`.
+
 ## Расположение маркеров на потолке
 
 ![Маркеры на потолке](../assets/IMG_4175.JPG)

docs/ru/aruco_new.md

@@ -1,23 +0,0 @@
-# ArUco-маркеры
-
-> **Note** Документация для версий [образа](microsd_images.md), начиная с версии **0.16**. Для более ранних версий см. [документацию для версии **0.15.1**](https://github.com/CopterExpress/clever/blob/v0.15.1/docs/ru/aruco.md).
-
-[ArUco-маркеры](https://docs.opencv.org/3.2.0/d5/dae/tutorial_aruco_detection.html) — это популярная технология для позиционирования
-робототехнических систем с использованием компьютерного зрения.
-
-![ArUco-маркеры](../assets/markers.jpg)
-
-> **Hint** При печати визуальных маркеров необходимо использовать максимально матовую бумагу. Глянцевая бумага будет бликовать на свету, сильно ухудшая качество распознавания.
-
-Для быстрого генерирования маркеров для печати можно использовать онлайн-инструмент: http://chev.me/arucogen/.
-
-На [образе Клевера для RPi](microsd_images.md) предустановлен пакет `aruco_pose`, предназначенный для работы с ArUco-маркерами.
-
-## Режимы работы
-
-Клевер имеет несколько преднастроенных режимов работы с ArUco-маркерами:
-
-* [распознавание и навигация по отдельным маркерам](aruco_marker.md);
-* [распознавание и навигация по картам маркеров](aruco_map.md).
-
-> **Info** Исчерпывающую документацию по пакету `aruco_pose` на английском языке можно посмотреть [на GitHub](https://github.com/CopterExpress/clever/blob/master/aruco_pose/README.md).

docs/ru/assemble_3.md

@@ -95,7 +95,30 @@ TODO
 
 ![Монтаж преобразователя напряжения BEC ](../assets/cl3_mountBEC.JPG)
 
-## Монтаж платы регуляторов 4в1 и платы питания PDB
+## Монтаж регуляторов
+
+### Монтаж 4 отдельных регуляторов
+
+#### Залудить три контактные площадки регулятора
+
+* Нанести флюс
+* Нанести припой
+
+Чтобы припой аккуратно заполнил всю площадку, необходимо прогреть площадку регулятора. Для этого нужно удерживать жало паяльника на контактной площадке в течение 2 сек (или больше, если потребуется)
+
+![Лужение контактных площадок регуляторов](../assets/escDYSzap.png)
+
+* Повторить данную операцию для оставшихся трех регуляторов
+
+#### Припаять провода моторов к регуляторам
+
+Припаять ранее приготовленные провода моторов к контактным площадкам регуляторов.
+
+![Припаять провода моторов к регуляторам](../assets/solderingBrrc2205ondeckTOescDYSzap.png)
+
+* Повторить данную операцию для оставшихся трех регуляторов
+
+### Монтаж платы регуляторов 4в1 и платы питания PDB
 
 1. Установить плату регуляторов 4в1, как показано на картинке.
 

docs/ru/clever_blocks.md

@@ -0,0 +1,43 @@
+# Блочное программирование Клевера
+
+<img src="../assets/clever_blocks.jpg">
+
+В этой статье я опишу процесс скачивания, установки и запуска блочного конструктора программ для квадрокоптера Клевер.
+
+## Скачивание
+
+Есть два варианта скачивания кода проекта на RPi:
+
+### Вариант 1
+
+Подключить плату к интернету (вставив в нее ethernet-кабель или [перенастроив Wi-Fi](network.md)) и в командной строке RPi выполнить:
+
+```
+cd
+git clone https://github.com/garinegor/clever-blocks
+```
+
+### Вариант 2
+
+Исходный код проекта можно скачать на компьютер с [GitHub](https://github.com/garinegor/clever-blocks), а затем скопировать его в домашнюю директорию RPi посредством SFTP или SCP.
+
+## Установка
+
+Если Вы хотите, чтобы блочный конструктор запускался автоматически, необходимо создать соответствующий сервис. Для этого следует ввести в командную строку RPi следующие команды:
+
+```
+sudo systemctl enable /home/pi/clever-blocks/service/
+sudo systemctl start clever-blocks.service
+```
+
+Все готово! Теперь можно переходить к использованию.
+
+## Использование
+
+Если Вы не стали добавлять сервис для автоматического старта сервера, для его запуска Вам нужно ввести следующую команду, находясь в папке проекта:
+
+```
+python main.py
+```
+
+После запуска Вы можете открыть веб-интерфейс для блочного программирования по адресу [192.168.11.1:5000](192.168.11.1:5000).

docs/ru/frames.md

@@ -14,3 +14,14 @@
 > **Hint** В соответствии с [соглашением](http://www.ros.org/reps/rep-0103.html), для фреймов, связанных с коптером, ось X направлена вперед, Y – налево и Z – вверх.
 
 Более наглядно 3D визуализацию систем координат можно наблюдать, используя [rviz](rviz.md).
+
+tf2
+--
+
+Основная документация: http://wiki.ros.org/tf2
+
+Для работы с системами координат в Клевере используется ROS-пакет tf2. tf2 – это набор библиотек для языков программирования C++, Python и других, которые помогают работать с системами координат. ROS-ноды публикуют в топик `/tf` сообщения формата `TransformStamped`, которые содержат в себе трансформации между заданными системами координат в определенные моменты времени.
+
+С помощью [`simple_offboard`](simple_offboard.md) можно запросить расположение коптера в любой системе координат, используя аргумент `frame_id` сервиса `get_telemetry`.
+
+Из Python можно использовать библиотеку tf2 для преобразования геометрических объектов (например, PoseStamped, PointStamped) из одной системы координат в другую.

docs/ru/laser.md

@@ -8,6 +8,19 @@
 
 ### Подключение к Raspberry Pi
 
+> **Note** Для корректной работы лазерного дальномера с полетным контроллером необходима <a id="download-firmware" href="https://github.com/CopterExpress/Firmware/releases">кастомная прошивка PX4</a>. Подробнее про прошивку см. [соответствующую статью](firmware.md).
+
+<script type="text/javascript">
+    fetch('https://api.github.com/repos/CopterExpress/Firmware/releases').then(res => res.json()).then(function(data) {
+        for (let release of data) {
+            if (!release.prerelease && !release.draft && release.tag_name.includes('-clever.')) {
+                document.querySelector('#download-firmware').href = release.html_url;
+                return;
+            }
+        }
+    });
+</script>
+
 Подключите дальномер по интерфейсу I²C к пинам 3V, GND, SCL и SDA:
 
 <img src="../assets/raspberry-vl53l1x.png" alt="Подключение VL53L1X" height=600>

docs/ru/optical_flow.md

@@ -1,12 +1,21 @@
 # Использование Optical Flow
 
-> **Warning** Данная функция является **экспериментальной** и включена в образ с версии v0.11.4.
-
 При использовании технологии Optical Flow возможен полет в режиме POSCTL и автономные полеты по камере, направленной вниз, за счет измерения сдвигов текстуры поверхности пола.
 
 ## Включение
 
-На данный момент для использования Optical Flow необходима [кастомная прошивка PX4](https://github.com/CopterExpress/Firmware/releases/tag/v1.8.2-clever.1).
+> **Note** Для использования Optical Flow необходима <a id="download-firmware" href="https://github.com/CopterExpress/Firmware/releases">кастомная прошивка PX4</a>. Подробнее про прошивку см. [соответствующую статью](firmware.md).
+
+<script type="text/javascript">
+    fetch('https://api.github.com/repos/CopterExpress/Firmware/releases').then(res => res.json()).then(function(data) {
+        for (let release of data) {
+            if (!release.prerelease && !release.draft && release.tag_name.includes('-clever.')) {
+                document.querySelector('#download-firmware').href = release.html_url;
+                return;
+            }
+        }
+    });
+</script>
 
 Необходимо использование дальномера. [Подключите и настройте дальномер VL53L1X](laser.md), используя инструкцию.
 
@@ -16,18 +25,37 @@
 <arg name="optical_flow" default="true"/>
 ```
 
+Optical Flow публикует данные в топик `mavros/px4flow/raw/send`. Кроме того, в топик `optical_flow/debug` публикуется визуализация, которую можно просмотреть с помощью [web_video_server](web_video_server.md).
+
 > **Info** Для правильной работы модуль камеры должен быть корректно подключен и [сконфигурирован](camera.md).
 
 ## Настройка полетного контроллера
 
-Рекомендуемые параметры PX4:
+При использовании **EKF2** (параметр `SYS_MC_EST_GROUP` = `ekf2`):
 
-* `SYS_MC_EST_GROUP` – 2 (EKF2).
-* `EKF2_AID_MASK` – use optical flow.
+* `EKF2_AID_MASK` – включен флажок use optical flow.
 * `EKF2_OF_DELAY` – 0.
-* `EKF2_OF_QMIN` – 20.
+* `EKF2_OF_QMIN` – 10.
+* `EKF2_OF_N_MIN` – 0.05.
+* `EKF2_OF_N_MAX` - 0.2.
 * `SENS_FLOW_ROT` – No rotation (отсутствие поворота).
-* `EKF2_HGT_MODE` – range sensor (см. [конфигурирование дальномера](laser.md)).
+* `SENS_FLOW_MAXHGT` – 4.0 (для дальномера VL53L1X)
+* `SENS_FLOW_MINHGT` – 0.01 (для дальномера VL53L1X)
+* Опционально: `EKF2_HGT_MODE` – range sensor (см. [конфигурирование дальномера](laser.md)).
+
+При использовании **LPE** (параметр `SYS_MC_EST_GROUP` = `local_position_estimator, attitude_estimator_q`):
+
+* `LPE_FUSION` – включены флажки fuse optical flow и flow gyro compensation.
+* `LPE_FLW_QMIN` – 10.
+* `LPE_FLW_SCALE` – 1.0.
+* `LPE_FLW_R` – 0.1.
+* `LPE_FLW_RR` – 0.0.
+* `SENS_FLOW_ROT` – No rotation (отсутствие поворота).
+* `SENS_FLOW_MAXHGT` – 4.0 (для дальномера VL53L1X)
+* `SENS_FLOW_MINHGT` – 0.01 (для дальномера VL53L1X)
+* Опционально: `LPE_FUSION` – включен флажок pub agl as lpos down (см. [конфигурирование дальномера](laser.md).
+
+Для проверки правильности всех настроек можно [воспользоваться утилитой `selfcheck.py`](selfcheck.md).
 
 ## Полет в POSCTL
 
@@ -49,14 +77,15 @@ navigate(z=1.5, frame_id='body', auto_arm=True)
 navigate(x=1.5, frame_id='body')
 ```
 
-## Неисправности
+При использовании Optical Flow возможна также [навигация по ArUco-маркерам](aruco_marker.md), в том числе [используя VPE](aruco_map.md).
 
-При появлении в QGC ошибок типа `EKF INTERNAL CHECKS` попробуйте перезагрузить EKF2. Для этого наберите в MAVLink-консоли:
+## Дополнительные настройки
 
-```nsh
-ekf2 stop
-ekf2 start
-```
+<!-- TODO: статья по пидам -->
+
+Если коптер нестабильно удерживает позицию по VPE, попробуйте увеличить коэффициенты *P* PID-регулятора по скорости – параметры `MPC_XY_VEL_P` и `MPC_Z_VEL_P`.
+
+Если коптер нестабильно удерживает высоту, попробуйте увеличить коэффициент `MPC_Z_VEL_P` или лучше подобрать газ висения – `MPC_THR_HOVER`.
 
 Если коптер сильно уплывает по рысканью, попробуйте:
 
@@ -67,5 +96,17 @@ ekf2 start
 
 Если коптер уплывает по высоте, попробуйте:
 
-* Изменить значение параметра `MPC_THR_HOVER`;
+* повысить значение коэффициента `MPC_Z_VEL_P`;
+* изменить значение параметра `MPC_THR_HOVER`;
 * выставить `MPC_ALT_MODE` = 2 (Terrain following).
+
+При использовании Optical Flow максимальная горизонтальная скорость дополнительно ограничивается. За это косвенно отвечает параметр `SENS_FLOW_MAXR` (максимальная достоверная "угловая скорость" оптического потока). При нормальном полёте горизонтальная скорость будет регулироваться так, чтобы показания Optical Flow не превышали 50% значения данного параметра.
+
+## Неисправности
+
+При появлении в QGC ошибок типа `EKF INTERNAL CHECKS` попробуйте перезагрузить EKF2. Для этого наберите в MAVLink-консоли:
+
+```nsh
+ekf2 stop
+ekf2 start
+```

docs/ru/selfcheck.md

@@ -22,6 +22,9 @@ rosrun clever selfcheck.py
 * Velocity estimation – оценка скоростей дрона (**запрещено выполнять автономный взлет при ошибках в этой проверке!**);
 * Global position (GPS) – наличие глобальной позиции (требуется GPS);
 * Camera – корректная работа камеры Raspberry.
+* ArUco – проверка работы [распознавания ArUco-маркеров](aruco.md).
+* VPE – проверка правильности работы VPE.
+* Rangefinder – проверка работы [дальномера](laser.md).
 
 ## commander check
 

docs/ru/sitl.md

@@ -1,6 +1,8 @@
 Симуляция PX4
 ===
 
+> **Hint** Мы также предоставляем [предварительно настроенный](sitl_docker.md) симулятор, поставляемый в виде Docker-контейнера!
+
 Основная статья: https://dev.px4.io/en/simulation/
 
 Симуляция PX4 возможна в ОС Linux и macOS с использованием систем симуляции физической среды [jMavSim](https://pixhawk.org/dev/hil/jmavsim) и [Gazebo](http://gazebosim.org).

docs/ru/sitl_docker.md

@@ -0,0 +1,142 @@
+# Docker-контейнер с преднастроенным SITL
+
+![SITL demo](../assets/sitl_docker_demo.png)
+
+Для упрощения запуска симулятора предлагается использовать предварительно настроенный [Docker-контейнер](https://hub.docker.com/r/sfalexrog/clever-sitl) с симулятором [Gazebo](http://gazebosim.org/), автопилотом [PX4](https://px4.io/) и предустановленными пакетами Клевера.
+
+## Состав контейнера
+
+В [Docker](https://docker.com/)-контейнере с симулятором установлены и настроены:
+
+* Симулятор Gazebo с плагинами для симуляции камеры, дальномера и связью с ROS
+* Пакеты [ROS](http://www.ros.org/), требуемые для запуска нод Клевера
+* Собранный для симулятора PX4
+* Легковесный web-интерфейс для Gazebo [Gzweb](http://gazebosim.org/gzweb.html)
+* Web-терминал [Butterfly](http://paradoxxxzero.github.io/2014/02/28/butterfly.html)
+
+## Предварительная настройка
+
+Запуск контейнера рекомендуется производить на ОС Ubuntu версии не ниже 16.04 с использованием Docker версии не ниже 18.09. Для комфортной работы с симулятором следует использовать компьютер с не менее чем 4 ядрами CPU (Intel Core i5/i7, не ниже Haswell) и не менее чем 8 ГБ ОЗУ. Работа с симулятором может происходить как на компьютере с запущенным контейнером, так и на другом компьютере в той же локальной сети.
+
+> **Hint** Не забудьте [установить](https://docs.docker.com/install/linux/docker-ce/ubuntu/) и [настроить](https://docs.docker.com/install/linux/linux-postinstall/) Docker на своей системе!
+
+## Запуск симулятора
+
+Для запуска симулятора можно использовать следующую команду:
+
+```bash
+docker pull sfalexrog/clever-sitl:slim
+
+docker run \
+    -it \
+    --rm \
+    -p 14556:14556 \
+    -p 14557:14557 \
+    -p 8080:8080 \
+    -p 8081:8081 \
+    -p 57575:57575 \
+    -p 9090:9090 \
+    -p 35602:35602 \
+    -p 2222:22 \
+    --name clever_sitl \
+    sfalexrog/clever-sitl:slim
+```
+
+> **Note** Здесь и далее предполагается, что при настройке Docker на своей системе Вы [настроили запуск Docker от обычного пользователя](https://docs.docker.com/install/linux/linux-postinstall/) (раздел "Manage Docker as a non-root user")
+
+Первая команда загружает последнюю версию контейнера с симулятором ```sfalexrog/clever-sitl:slim```, вторая его запускает. Ключ ```-p``` позволяет задать соответствие между портом компьютера, на котором запущен контейнер, и портом "внутри" контейнера. Порты 14556 и 14557 нужны для подключения к симулятору с помощью [QGroundControl](http://qgroundcontrol.com/), порт 8080 - для просмотра топиков ROS с изображениями и видеопотоками, порт 8081 - для подключения к визуализации симуляции Gazebo, порт 57575 - для доступа к web-терминалу Butterfly.
+
+После запуска контейнера можно перейти по следующим ссылкам в браузере для доступа к сервисам симулятора:
+
+* [http://localhost:8080](http://localhost:8080) - просмотр топиков с камеры (аналогично тому, как это сделано в Клевере)
+* [http://localhost:8081](http://localhost:8081) - визуализация текущего состояния симулятора через Gzweb
+* [http://localhost:57575](http://localhost:57575) - Web-терминал Butterfly с запущенным сеансом [tmux](https://github.com/tmux/tmux/wiki)
+
+Доступ к этим сервисам также есть с других компьютеров, расположенных в той же локальной сети; для этого в ссылках, указанных выше, следует ```localhost``` поменять на IP-адрес компьютера с запущеным контейнером.
+
+## Работа с симулятором
+
+Основное взаимодействие с симулятором происходит через Web-терминал Butterfly. По умолчанию в нём открывается сессия tmux, так что происходящее в терминале можно демонстрировать сразу на нескольких компьютерах.
+
+> **Hint** Можно также создавать дополнительные сеансы средствами Docker. Для этого воспользуйтесь командой ```docker exec -it clever_sitl /bin/bash```
+
+В web-терминале работают команды ROS, доступны редакторы [vim](https://www.vim.org/) и [nano](https://www.nano-editor.org/), поддерживается работа с интерпретатором [Python](https://www.python.org/). В симуляторе можно запускать программы, написанные для Клевера и не использующие специфический функционал бортового компьютера или периферии (например, LED-ленты).
+
+Визуализация текущего состояния симулированного мира доступна в Gzweb. Камеру можно перемещать, передвигая мышь с зажатой левой кнопкой. Поворот камеры производится при зажатой средней кнопке мыши, приближение/удаление камеры - при повороте колёсика. При выполнении этих манипуляций появляется небольшая жёлтая сфера, означающая центр поворота/приближения.
+
+В web-терминале также можно просмотреть текущее состояние PX4, отладочный вывод нод Клевера, лог Gazebo и Gzweb. Для этого надо переключиться на нулевой экран tmux; это делается комбинацией клавиш ```ctrl+B``` и последующим нажатием клавиши ```0```. Появится примерно следующее:
+
+![SITL debug pane](../assets/sitl_debug_pane.png)
+
+Верхняя панель - отладочная консоль PX4; на нижней панели: слева находится отладочный вывод симулятора Gazebo, справа сверху - лог нод Клевера, справа снизу - лог Gzweb.
+
+Для переключения между панелями следует использовать комбинацию клавиш ```ctrl+B``` и последующее нажатие кнопки ```Q``` и номера панели. Номера панелей будут кратковременно выведены поверх самих панелей при нажатии ```ctrl+B``` и ```Q```.
+
+## Подключение локальных директорий к контейнеру
+
+Для подключения директории, доступной как на основной системе, так и в контейнере, при запуске следует указать ключ ```-v``` с указанием директории в основной системе и в контейнере.
+
+Так, для того, чтобы текущая директория стала доступна в контейнере по пути ```/home/user/data```, запустите контейнер со следующими параметрами:
+
+```bash
+docker run \
+    -it \
+    --rm \
+    -p 14556:14556 \
+    -p 14557:14557 \
+    -p 8080:8080 \
+    -p 8081:8081 \
+    -p 57575:57575 \
+    -p 9090:9090 \
+    -p 35602:35602 \
+    -p 2222:22 \
+    -v $(pwd):/home/user/data:rw \
+    --name clever_sitl \
+    sfalexrog/clever-sitl:slim
+```
+
+> **Note** В команде для запуска контейнера ключ ```-v``` может встречаться многократно. Это позволяет указать несколько общих директорий.
+
+Разумно использовать механизм подключения локальных директорий для добавления своих моделей в симулятор, сохранения и загрузки параметров PX4, а также своих программ для Клевера на Python.
+
+> **Warning** Программы, скомпилированные в контейнере, могут не запускаться на основной системе. Аналогично, программы, скомпилированные на основной системе, могут не запускаться в контейнере. Это следует учитывать при написании нод, использующих компилируемые языки.
+
+## Завершение работы с симулятором
+
+Для завершения работы с симулятором достаточно завершить работу соответствующего контейнера. Это можно сделать с помощью инструментов управления Docker-контейнерами или нажатием комбинации клавиш ```ctrl+C``` в терминале, в котором был запущен контейнер.
+
+## Увеличение скорости работы симулятора
+
+> **Warning** Предложенный в этом режиме метод является экспериментальным; вполне возможно, что он не заработает, и в этом случае некоторые элементы симулятора также перестанут работать
+
+По умолчанию симулятор будет создавать изображения в симулируемых камерах, используя программную растеризацию. Это создаёт повышенную нагрузку на CPU компьютера с запущенным симулятором, а также не позволяет получить приемлемую частоту кадров для большинства задач компьютерного зрения. Как правило, Docker не используют для графически интенсивных задач, поэтому возможности по увеличению производительности ограничены.
+
+При использовании достаточно современного графического оборудования с открытыми драйверами (например, Intel HD Graphics 520+ и mesa на Linux) можно попробовать "пробросить" сеанс X Server и видеокарту в контейнер; в этом случае будет использована гораздо более быстрая аппаратная растеризация.
+
+Для проброса видеокарты в контейнер следует выполнить следующие команды на компьютере, на котором будет запущен контейнер:
+
+```
+touch /tmp/.docker.xauth
+
+xauth nlist $DISPLAY | sed -e 's/^..../ffff/' | xauth -f /tmp/.docker/xauth nmerge -
+
+docker run \
+    -it \
+    --rm \
+    -v /tmp/.X11-unix:/tmp/.X11-unix:rw \
+    -v /tmp/.docker.xauth:/tmp/.docker.xauth:rw \
+    -e DISPLAY=$DISPLAY \
+    -e XAUTHORITY=/tmp/.docker.xauth \
+    --device /dev/dri/card0:/dev/dri/card0 \
+    -p 14556:14556 \
+    -p 14557:14557 \
+    -p 8080:8080 \
+    -p 8081:8081 \
+    -p 57575:57575 \
+    -p 9090:9090 \
+    -p 35602:35602 \
+    -p 2222:22 \
+    sfalexrog/clever-sitl:slim
+```
+
+При этом на компьютере с контейнером должна быть запущена графическая среда, использующая X Server.

docs/ru/snippets.md

@@ -240,7 +240,7 @@ from mavros_msgs.msg import RCIn
 # Вызывается при получении новых данных с пульта
 def rc_callback(data):
     # Произвольная реакция на переключение тумблера на пульте
-        if data.channels[5] < 1100:
+    if data.channels[5] < 1100:
         # ...
         pass
     elif data.channels[5] > 1900:

gen_changelog.py

@@ -0,0 +1,75 @@
+#!/usr/bin/env python3
+
+# Generate and upload changelog
+
+from git import Repo, exc
+from github import Github
+import os
+import sys
+
+upload_changelog = True
+
+try:
+    current_tag = os.environ['TRAVIS_TAG']
+    if current_tag == '':
+        current_tag = 'HEAD'
+        upload_changelog = False
+    print('TRAVIS_TAG is set to {}'.format(current_tag))
+except KeyError:
+    print('TRAVIS_TAG not set - not uploading changelog')
+    current_tag = 'HEAD'
+    upload_changelog = False
+
+try:
+    api_key = os.environ['GITHUB_OAUTH_TOKEN']
+except KeyError:
+    print('GITHUB_OAUTH_TOKEN not set - not uploading changelog')
+    api_key = None
+    upload_changelog = False
+
+try:
+    repo_slug = os.environ['TRAVIS_REPO_SLUG']
+except KeyError:
+    print('TRAVIS_REPO_SLUG not set - cannot determine remote repository')
+    repo_slug = ''
+    exit(1)
+
+if len(sys.argv) > 1:
+    repo_path = sys.argv[1]
+else:
+    repo_path = '.'
+
+print('Opening repository at {}'.format(repo_path))
+repo = Repo(repo_path)
+git = repo.git()
+try:
+    print('Unshallowing repository')
+    git.fetch('--unshallow', '--tags')
+except exc.GitCommandError:
+    print('Repository already unshallowed')
+print('Attempting to get previous tag')
+base_tag = git.describe('--tags', '--abbrev=0', '{}^'.format(current_tag))
+print('Base tag set to {}'.format(base_tag))
+
+changelog = git.log('{}...{}'.format(base_tag, current_tag), '--pretty=format:* %H %s *(%an)*')
+print('Current changelog: \n{}'.format(changelog))
+
+# Only interact with Github if uploading is enabled
+if upload_changelog:
+    gh = Github(api_key)
+    gh_repo = gh.get_repo(repo_slug)
+    # Get all releases and find ours by its tag name
+    gh_release = None
+    for release in gh_repo.get_releases():
+        if release.tag_name == current_tag:
+            gh_release = release
+    if gh_release is None:
+        # We could not find the correct release, so here's our last resort. It will most likely fail.
+        gh_release = gh_repo.get_release(current_tag)
+    gh_body = gh_release.body
+    if gh_body is None:
+        gh_body = ''
+    gh_body = '{}\nChanges between `{}` and `{}`:\n\n{}'.format(gh_body, base_tag, current_tag, changelog)
+    print('New release body: {}'.format(gh_body))
+    gh_release.update_release(gh_release.tag_name, gh_body, draft=True, prerelease=True,
+                              tag_name=gh_release.tag_name, target_commitish=gh_release.target_commitish)

redirects.json

@@ -27,6 +27,7 @@
         { "from": "ros.html", "to": "ru/ros.html" },
         { "from": "mavros.html", "to": "ru/mavros.html" },
         { "from": "simple_offboard.html", "to": "ru/simple_offboard.html" },
+        { "from": "aruco/", "to": "ru/aruco.html" },
         { "from": "aruco.html", "to": "ru/aruco.html" },
         { "from": "selfcheck.html", "to": "ru/selfcheck.html" },
         { "from": "snippets.html", "to": "ru/snippets.html" },
@@ -40,6 +41,14 @@
         { "from": "copterhack2018.html", "to": "ru/copterhack2018.html" },
         { "from": "copterhack2018.html", "to": "ru/copterhack2017.html" },
         { "from": "mavlink.html", "to": "ru/mavlink.html" },
-        { "from": "flight_logs.html", "to": "ru/flight_logs.html" }
+        { "from": "flight_logs.html", "to": "ru/flight_logs.html" },
+
+        { "from": "modes/", "to": "ru/modes.html" },
+        { "from": "firmware/", "to": "ru/firmware.html" },
+        { "from": "simple_offboard/", "to": "ru/simple_offboard.html" },
+        { "from": "camera/", "to": "ru/camera.html" },
+        { "from": "snippets/", "to": "ru/snippets.html" },
+        { "from": "optical_flow/", "to": "ru/optical_flow.html" },
+        { "from": "laser/", "to": "ru/laser.html" }
     ]
 }