aruco_map: start to work on mapping

* Add files via upload

* Add files via upload

* Добавила статью по настройке  режима тренера.

В разделе "Настройки", после статьи Полётные режимы.

* Update Trainer_mode.md

* Update Trainer_mode.md

* Update Trainer_mode.md

.markdownlint.json

@@ -24,6 +24,8 @@
             "Python",
             "C++",
             "PX4",
+            "px4.io",
+            "logs.px4.io",
             "QGroundControl",
             "QGC",
             "WireShark",
@@ -38,6 +40,10 @@
             "RPi",
             "Linux",
             "Windows",
+            "Docker",
+            "Travis",
+            "travis-ci.org",
+            "travis-ci.com",
             "macOS",
             "iOS",
             "Android",

.travis.yml

@@ -23,7 +23,16 @@ jobs:
 # Check if there are any cached images, copy them to our "images" directory
         - if [ -n "$(ls -A imgcache/*.zip)" ]; then mkdir -p images && cp imgcache/*.zip images; fi
       script:
-        - docker run --privileged --rm -v /dev:/dev -v $(pwd):/builder/repo -e TRAVIS_TAG="${TRAVIS_TAG}" ${DOCKER}
+        - if [[ -z ${TRAVIS_TAG} && "${TRAVIS_PULL_REQUEST}" != "false" ]]; then
+            echo "Commit range is ${TRAVIS_COMMIT_RANGE}" &&
+            if [ $(git diff --name-only ${TRAVIS_COMMIT_RANGE} | grep -v ^"docs/" | wc -l) -eq 0 ]; then
+              echo " === Docs-only change; skipping build ===" &&
+              export SKIP_BUILD=true;
+            fi;
+          fi
+        - if [ -z ${SKIP_BUILD} ]; then
+            docker run --privileged --rm -v /dev:/dev -v $(pwd):/builder/repo -e TRAVIS_TAG="${TRAVIS_TAG}" ${DOCKER};
+          fi
       before_cache:
         - cp images/*.zip imgcache
       before_deploy:
@@ -55,19 +64,18 @@ jobs:
         - markdownlint docs
         - gitbook install
         - gitbook build
-# ***
-# Disable deployments for now, revisit this later
-#   --sfalexrog, 06.02.2019
-# ***
-#      deploy:
-#        provider: pages
-#        local-dir: _book
-#        skip-cleanup: true
-#        github-token: ${GITHUB_OAUTH_TOKEN}
-#        keep-history: true
-#        target-branch: gh-pages
-#        on:
-#          branch: WIP/gitbook-autobuild
+      deploy:
+        provider: pages
+        local-dir: _book
+        skip-cleanup: true
+        github-token: ${GITHUB_OAUTH_TOKEN}
+        keep-history: true
+        target-branch: master
+        repo: CopterExpress/clever-gitbook
+        fqdn: clever.copterexpress.com
+        verbose: true
+        on:
+          branch: master
     - stage: Annotate
       name: Auto-generate changelog
       language: python

aruco_pose/CMakeLists.txt

@@ -216,4 +216,7 @@ target_link_libraries(aruco_pose
 if (CATKIN_ENABLE_TESTING)
   find_package(rostest REQUIRED)
   add_rostest(test/basic.test)
+  add_rostest(test/test_parser_pass.test)
+  add_rostest(test/test_parser_empty_map.test)
+  add_rostest(test/test_node_failure.test)
 endif()

aruco_pose/README.md

@@ -74,6 +74,7 @@ It's recommended to run it within the same nodelet manager with the camera nodel
 * `~image_width` – debug image width (default: 2000)
 * `~image_height` – debug image height (default: 2000)
 * `~image_margin` – debug image margin (default: 200)
+* `~dictionary` (*int*) – ArUco dictionary (default: 2) - should be the same as `dictionary` parameter of `aruco_detect` nodelet
 
 Map file has one marker per line with the following line format:
 

aruco_pose/map/cmit.txt

@@ -0,0 +1,100 @@
+0	0.33	0.0	9.0	0	0	0	0	
+1	0.33	1.0	9.0	0	0	0	0	
+2	0.33	2.0	9.0	0	0	0	0	
+3	0.33	3.0	9.0	0	0	0	0	
+4	0.33	4.0	9.0	0	0	0	0	
+5	0.33	5.0	9.0	0	0	0	0	
+6	0.33	6.0	9.0	0	0	0	0	
+7	0.33	7.0	9.0	0	0	0	0	
+8	0.33	8.0	9.0	0	0	0	0	
+9	0.33	9.0	9.0	0	0	0	0	
+10	0.33	0.0	8.0	0	0	0	0	
+11	0.33	1.0	8.0	0	0	0	0	
+12	0.33	2.0	8.0	0	0	0	0	
+13	0.33	3.0	8.0	0	0	0	0	
+14	0.33	4.0	8.0	0	0	0	0	
+15	0.33	5.0	8.0	0	0	0	0	
+16	0.33	6.0	8.0	0	0	0	0	
+#17	0.33	7.0	8.0	0	0	0	0	
+18	0.33	8.0	8.0	0	0	0	0	
+19	0.33	9.0	8.0	0	0	0	0	
+20	0.33	0.0	7.0	0	0	0	0	
+21	0.33	1.0	7.0	0	0	0	0	
+22	0.33	2.0	7.0	0	0	0	0	
+23	0.33	3.0	7.0	0	0	0	0	
+24	0.33	4.0	7.0	0	0	0	0	
+25	0.33	5.0	7.0	0	0	0	0	
+26	0.33	6.0	7.0	0	0	0	0	
+27	0.33	7.0	7.0	0	0	0	0	
+28	0.33	8.0	7.0	0	0	0	0	
+29	0.33	9.0	7.0	0	0	0	0	
+30	0.33	0.0	6.0	0	0	0	0	
+31	0.33	1.0	6.0	0	0	0	0	
+32	0.33	2.0	6.0	0	0	0	0	
+33	0.33	3.0	6.0	0	0	0	0	
+34	0.33	4.0	6.0	0	0	0	0	
+35	0.33	5.0	6.0	0	0	0	0	
+36	0.33	6.0	6.0	0	0	0	0	
+37	0.33	7.0	6.0	0	0	0	0	
+38	0.33	8.0	6.0	0	0	0	0	
+39	0.33	9.0	6.0	0	0	0	0	
+40	0.33	0.0	5.0	0	0	0	0	
+41	0.33	1.0	5.0	0	0	0	0	
+42	0.33	2.0	5.0	0	0	0	0	
+43	0.33	3.0	5.0	0	0	0	0	
+44	0.33	4.0	5.0	0	0	0	0	
+45	0.33	5.0	5.0	0	0	0	0	
+46	0.33	6.0	5.0	0	0	0	0	
+47	0.33	7.0	5.0	0	0	0	0	
+48	0.33	8.0	5.0	0	0	0	0	
+49	0.33	9.0	5.0	0	0	0	0	
+50	0.33	0.0	4.0	0	0	0	0	
+51	0.33	1.0	4.0	0	0	0	0	
+52	0.33	2.0	4.0	0	0	0	0	
+53	0.33	3.0	4.0	0	0	0	0	
+54	0.33	4.0	4.0	0	0	0	0	
+55	0.33	5.0	4.0	0	0	0	0	
+56	0.33	6.0	4.0	0	0	0	0	
+57	0.33	7.0	4.0	0	0	0	0	
+58	0.33	8.0	4.0	0	0	0	0	
+59	0.33	9.0	4.0	0	0	0	0	
+60	0.33	0.0	3.0	0	0	0	0	
+61	0.33	1.0	3.0	0	0	0	0	
+62	0.33	2.0	3.0	0	0	0	0	
+63	0.33	3.0	3.0	0	0	0	0	
+64	0.33	4.0	3.0	0	0	0	0	
+65	0.33	5.0	3.0	0	0	0	0	
+66	0.33	6.0	3.0	0	0	0	0	
+67	0.33	7.0	3.0	0	0	0	0	
+68	0.33	8.0	3.0	0	0	0	0	
+69	0.33	9.0	3.0	0	0	0	0	
+70	0.33	0.0	2.0	0	0	0	0	
+71	0.33	1.0	2.0	0	0	0	0	
+72	0.33	2.0	2.0	0	0	0	0	
+73	0.33	3.0	2.0	0	0	0	0	
+74	0.33	4.0	2.0	0	0	0	0	
+75	0.33	5.0	2.0	0	0	0	0	
+76	0.33	6.0	2.0	0	0	0	0	
+77	0.33	7.0	2.0	0	0	0	0	
+78	0.33	8.0	2.0	0	0	0	0	
+79	0.33	9.0	2.0	0	0	0	0	
+80	0.33	0.0	1.0	0	0	0	0	
+81	0.33	1.0	1.0	0	0	0	0	
+82	0.33	2.0	1.0	0	0	0	0	
+83	0.33	3.0	1.0	0	0	0	0	
+84	0.33	4.0	1.0	0	0	0	0	
+85	0.33	5.0	1.0	0	0	0	0	
+86	0.33	6.0	1.0	0	0	0	0	
+87	0.33	7.0	1.0	0	0	0	0	
+88	0.33	8.0	1.0	0	0	0	0	
+89	0.33	9.0	1.0	0	0	0	0	
+90	0.33	0.0	0.0	0	0	0	0	
+91	0.33	1.0	0.0	0	0	0	0	
+92	0.33	2.0	0.0	0	0	0	0	
+93	0.33	3.0	0.0	0	0	0	0	
+94	0.33	4.0	0.0	0	0	0	0	
+95	0.33	5.0	0.0	0	0	0	0	
+96	0.33	6.0	0.0	0	0	0	0	
+97	0.33	7.0	0.0	0	0	0	0	
+98	0.33	8.0	0.0	0	0	0	0	
+99	0.33	9.0	0.0	0	0	0	0	

aruco_pose/src/aruco_map.cpp

@@ -36,6 +36,7 @@
 #include <sensor_msgs/Image.h>
 #include <visualization_msgs/Marker.h>
 #include <visualization_msgs/MarkerArray.h>
+#include <algorithm>
 
 #include <aruco_pose/MarkerArray.h>
 #include <aruco_pose/Marker.h>
@@ -74,6 +75,9 @@ private:
 	std::string known_tilt_;
 	int image_width_, image_height_, image_margin_;
 	bool auto_flip_;
+	bool mapping_;
+	vector<int> mapping_exclude_;
+	std::fstream map_file_;
 
 public:
 	virtual void onInit()
@@ -97,6 +101,8 @@ public:
 		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("mapping", mapping_, false);
+		nh_priv_.getParam("mapping_exclude", mapping_exclude_);
 		nh_priv_.param("image_width", image_width_, 2000);
 		nh_priv_.param("image_height", image_height_, 2000);
 		nh_priv_.param("image_margin", image_margin_, 200);
@@ -155,6 +161,13 @@ public:
 				cv::Point2f(marker.c4.x, marker.c4.y)
 			};
 			corners.push_back(marker_corners);
+
+			if (mapping_) {
+				if(std::find(board_->ids.begin(), board_->ids.end(), marker.id) == board_->ids.end()) {
+					// no such marker in map
+					mappingAddMarker(marker);
+				}
+			}
 		}
 
 		if (known_tilt_.empty()) {
@@ -256,30 +269,99 @@ publish_debug:
 
 	void loadMap(std::string filename)
 	{
-		std::ifstream f(filename);
+		map_file_.open(filename, std::fstream::in | std::fstream::out | std::fstream::app);
 		std::string line;
 
-		if (!f.good()) {
+		if (!map_file_.good()) {
 			ROS_FATAL("aruco_map: %s - %s", strerror(errno), filename.c_str());
 			ros::shutdown();
 		}
 
-		while (std::getline(f, line)) {
+		while (std::getline(map_file_, line)) {
 			int id;
 			double length, x, y, z, yaw, pitch, roll;
 
 			std::istringstream s(line);
 
-			if (!(s >> id >> length >> x >> y >> z >> yaw >> pitch >> roll)) {
-				ROS_ERROR("aruco_map: cannot parse line: %s", line.c_str());
+			// Read first character to see whether it's a comment
+			char first = 0;
+			if (!(s >> first)) {
+				// No non-whitespace characters, must be a blank line
 				continue;
 			}
+
+			if (first == '#') {
+				ROS_DEBUG("aruco_map: Skipping line as a comment: %s", line.c_str());
+				continue;
+			} else if (isdigit(first)) {
+				// Put the digit back into the stream
+				// Note that this is a non-modifying putback, so this should work with istreams
+				// (see https://en.cppreference.com/w/cpp/io/basic_istream/putback)
+				s.putback(first);
+			} else {
+				// Probably garbage data; inform user and throw an exception, possibly killing nodelet
+				ROS_FATAL("aruco_map: Malformed input: %s", line.c_str());
+				ros::shutdown();
+				throw std::runtime_error("Malformed input");
+			}
+
+			if (!(s >> id >> length >> x >> y)) {
+				ROS_ERROR("aruco_map: Not enough data in line: %s; "
+				          "Each marker must have at least id, length, x, y fields", line.c_str());
+				continue;
+			}
+			// Be less strict about z, yaw, pitch roll
+			if (!(s >> z)) {
+				ROS_DEBUG("aruco_map: No z coordinate provided for marker %d, assuming 0", id);
+				z = 0;
+			}
+			if (!(s >> yaw)) {
+				ROS_DEBUG("aruco_map: No yaw provided for marker %d, assuming 0", id);
+				yaw = 0;
+			}
+			if (!(s >> pitch)) {
+				ROS_DEBUG("aruco_map: No pitch provided for marker %d, assuming 0", id);
+				pitch = 0;
+			}
+			if (!(s >> roll)) {
+				ROS_DEBUG("aruco_map: No roll provided for marker %d, assuming 0", id);
+				roll = 0;
+			}
 			addMarker(id, length, x, y, z, yaw, pitch, roll);
 		}
 
+		map_file_.clear(); // clear EOF state
 		ROS_INFO("aruco_map: loading %s complete (%d markers)", filename.c_str(), static_cast<int>(board_->ids.size()));
 	}
 
+	void mappingAddMarker(const aruco_pose::Marker& marker)
+	{
+		ROS_INFO("aruco_map: add marker %d to map", marker.id);
+
+		double roll, pitch, yaw;
+		tf::Quaternion q;
+		tf::quaternionMsgToTF(marker.pose.orientation, q);
+		tf::Matrix3x3(q).getRPY(roll, pitch, yaw);
+		const geometry_msgs::Point& p = marker.pose.position;
+		addMarker(marker.id, marker.length, p.x, p.y, p.z, yaw, pitch, roll);
+		writeToMap(marker.id, marker.length, p.x, p.y, p.z, yaw, pitch, roll);
+		publishMapImage();
+		vis_markers_pub_.publish(vis_array_);
+	}
+
+	inline void writeToMap(int id, double length, double x, double y, double z, double yaw, double pitch, double roll)
+	{
+		map_file_ << std::setprecision(3) // round numbers
+		          << id << '\t'
+		          << length << '\t'
+		          << x << '\t'
+		          << y << '\t'
+		          << z << '\t'
+		          << yaw << '\t'
+		          << pitch << '\t'
+		          << roll << std::endl;
+	}
+
 	void createGridBoard()
 	{
 		ROS_INFO("aruco_map: generate gridboard");
@@ -339,6 +421,19 @@ publish_debug:
 	void addMarker(int id, double length, double x, double y, double z,
 				   double yaw, double pitch, double roll)
 	{
+		// Check whether the id is in range for current dictionary
+		int num_markers = board_->dictionary->bytesList.rows;
+		if (num_markers <= id) {
+			ROS_ERROR("aruco_map: Marker id %d is not in dictionary; current dictionary contains %d markers. "
+			          "Please see https://github.com/CopterExpress/clever/blob/master/aruco_pose/README.md#parameters for details",
+					  id, num_markers);
+			return;
+		}
+		// Check if marker is already in the board
+		if (std::count(board_->ids.begin(), board_->ids.end(), id) > 0) {
+			ROS_ERROR("aruco_map: Marker id %d is already in the map", id);
+			return;
+		}
 		// Create transform
 		tf::Quaternion q;
 		q.setRPY(roll, pitch, yaw);

aruco_pose/src/genmap.py

@@ -1,5 +1,13 @@
 #!/usr/bin/env python
 
+# Copyright (C) 2018 Copter Express Technologies
+#
+# Author: Oleg Kalachev <okalachev@gmail.com>
+#
+# Distributed under MIT License (available at https://opensource.org/licenses/MIT).
+# The above copyright notice and this permission notice shall be included in all
+# copies or substantial portions of the Software.
+
 """Markers map generator
 
 Generate map file for aruco_map nodelet.

aruco_pose/src/utils.h

@@ -1,3 +1,14 @@
+/*
+ * Utility functions
+ * Copyright (C) 2018 Copter Express Technologies
+ *
+ * Author: Oleg Kalachev <okalachev@gmail.com>
+ *
+ * Distributed under MIT License (available at https://opensource.org/licenses/MIT).
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ */
+
 #pragma once
 
 #include <cmath>

aruco_pose/test/basic.txt

@@ -1,7 +1,11 @@
+# Default markers
 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
+# Marker with non-zero yaw rotation
 10	0.5	0.5	2	0	1.2	0	0
+# Marker with non-zero pitch and roll rotation
 11	0.2	0.5	0.5	0	0.0	-1	1
+# Marker with yaw, pitch and roll rotation
 12	0.4	0.2	0.5	0	0.1	-1.2	-0.5

aruco_pose/test/test_node_failure.py

@@ -0,0 +1,27 @@
+#!/usr/bin/env python
+import sys
+import unittest
+import json
+import rospy
+import rostest
+
+from geometry_msgs.msg import PoseWithCovarianceStamped
+from sensor_msgs.msg import Image
+from aruco_pose.msg import MarkerArray
+from visualization_msgs.msg import MarkerArray as VisMarkerArray
+
+
+class TestArucoMapPass(unittest.TestCase):
+    def setUp(self):
+        rospy.init_node('test_parser_fail', anonymous=True)
+
+    def test_node_failure(self):
+        try:
+            markers = rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5)
+            did_post_message = True
+        except rospy.exceptions.ROSException:
+            did_post_message = False
+        self.assertFalse(did_post_message)
+
+
+rostest.rosrun('aruco_pose', 'test_aruco_map', TestArucoMapPass, sys.argv)

aruco_pose/test/test_node_failure.test

@@ -0,0 +1,13 @@
+<launch>
+    <node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager"/>
+
+    <node name="aruco_map" pkg="nodelet" type="nodelet" args="load aruco_pose/aruco_map nodelet_manager" clear_params="true">
+        <remap from="image_raw" to="main_camera/image_raw"/>
+        <remap from="camera_info" to="main_camera/camera_info"/>
+        <remap from="markers" to="aruco_detect/markers"/>
+        <param name="type" value="map"/>
+        <param name="map" value="$(find aruco_pose)/test/test_node_failure.txt"/>
+    </node>
+
+    <test test-name="test_aruco_map_fail_dict" pkg="aruco_pose" type="test_node_failure.py"/>
+</launch>

aruco_pose/test/test_node_failure.txt

@@ -0,0 +1,4 @@
+# Any garbage data (pretty much anything apart from a comment starting with a hash starting
+# with a hash sign or a number) will be interpreted as garbage data; the node should fail
+# after reading it.
+// Don't try to put your comments this way, it will kill your node!

aruco_pose/test/test_parser_empty_map.py

@@ -0,0 +1,24 @@
+#!/usr/bin/env python
+import sys
+import unittest
+import json
+import rospy
+import rostest
+
+from geometry_msgs.msg import PoseWithCovarianceStamped
+from sensor_msgs.msg import Image
+from aruco_pose.msg import MarkerArray
+from visualization_msgs.msg import MarkerArray as VisMarkerArray
+
+
+class TestArucoMapPass(unittest.TestCase):
+    def setUp(self):
+        rospy.init_node('test_parser_fail', anonymous=True)
+
+
+    def test_node_failure(self):
+        markers = rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5)
+        self.assertEquals(len(markers.markers), 0)
+
+
+rostest.rosrun('aruco_pose', 'test_aruco_map', TestArucoMapPass, sys.argv)

aruco_pose/test/test_parser_empty_map.test

@@ -0,0 +1,13 @@
+<launch>
+    <node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager"/>
+
+    <node name="aruco_map" pkg="nodelet" type="nodelet" args="load aruco_pose/aruco_map nodelet_manager" clear_params="true">
+        <remap from="image_raw" to="main_camera/image_raw"/>
+        <remap from="camera_info" to="main_camera/camera_info"/>
+        <remap from="markers" to="aruco_detect/markers"/>
+        <param name="type" value="map"/>
+        <param name="map" value="$(find aruco_pose)/test/test_parser_empty_map.txt"/>
+    </node>
+
+    <test test-name="test_aruco_map_incomplete" pkg="aruco_pose" type="test_parser_empty_map.py"/>
+</launch>

aruco_pose/test/test_parser_empty_map.txt

@@ -0,0 +1,6 @@
+# Failing markers: Not enough parameters to add a marker
+1
+2 1
+3 0.5 1
+# Failing markers: Marker IDs outside of dictionary range
+1001 1 1 0

aruco_pose/test/test_parser_pass.py

@@ -0,0 +1,75 @@
+#!/usr/bin/env python
+import sys
+import unittest
+import json
+import rospy
+import rostest
+
+from geometry_msgs.msg import PoseWithCovarianceStamped
+from sensor_msgs.msg import Image
+from aruco_pose.msg import MarkerArray
+from visualization_msgs.msg import MarkerArray as VisMarkerArray
+
+
+class TestArucoMapPass(unittest.TestCase):
+    def setUp(self):
+        rospy.init_node('test_parser_pass', anonymous=True)
+
+    def test_markers(self):
+        markers = rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5)
+        
+        self.assertEqual(len(markers.markers), 6)
+# FIXME: visual marker id is not ArUco marker id
+#        self.assertEqual(markers.markers[0].id, 1)
+#        self.assertEqual(markers.markers[1].id, 2)
+#        self.assertEqual(markers.markers[2].id, 3)
+#        self.assertEqual(markers.markers[3].id, 4)
+
+        self.assertAlmostEqual(markers.markers[0].pose.position.x, 0, places=7)
+        self.assertAlmostEqual(markers.markers[0].pose.position.y, 0, places=7)
+        self.assertAlmostEqual(markers.markers[0].pose.position.z, 0, places=7)
+        
+        self.assertAlmostEqual(markers.markers[1].pose.position.x, 1, places=7)
+        self.assertAlmostEqual(markers.markers[1].pose.position.y, 1, places=7)
+        self.assertAlmostEqual(markers.markers[1].pose.position.z, 1, places=7)
+        
+        self.assertAlmostEqual(markers.markers[2].pose.position.x, 1, places=7)
+        self.assertAlmostEqual(markers.markers[2].pose.position.y, 0, places=7)
+        self.assertAlmostEqual(markers.markers[2].pose.position.z, 0.5, places=7)
+        
+        self.assertAlmostEqual(markers.markers[3].pose.position.x, 0, places=7)
+        self.assertAlmostEqual(markers.markers[3].pose.position.y, 1, places=7)
+        self.assertAlmostEqual(markers.markers[3].pose.position.z, 0, places=7)
+        
+        self.assertAlmostEqual(markers.markers[4].pose.position.x, 1, places=7)
+        self.assertAlmostEqual(markers.markers[4].pose.position.y, 0.5, places=7)
+        self.assertAlmostEqual(markers.markers[4].pose.position.z, 0, places=7)
+        
+        self.assertAlmostEqual(markers.markers[5].pose.position.x, 2.2, places=7)
+        self.assertAlmostEqual(markers.markers[5].pose.position.y, 0.2, places=7)
+        self.assertAlmostEqual(markers.markers[5].pose.position.z, 0, places=7)
+
+        self.assertAlmostEqual(markers.markers[0].scale.x, 0.33, places=7)
+        self.assertAlmostEqual(markers.markers[0].scale.y, 0.33, places=7)
+        self.assertAlmostEqual(markers.markers[1].scale.x, 0.225, places=7)
+        self.assertAlmostEqual(markers.markers[1].scale.y, 0.225, places=7)
+        self.assertAlmostEqual(markers.markers[2].scale.x, 0.45, places=7)
+        self.assertAlmostEqual(markers.markers[2].scale.y, 0.45, places=7)
+        self.assertAlmostEqual(markers.markers[3].scale.x, 0.15, places=7)
+        self.assertAlmostEqual(markers.markers[3].scale.y, 0.15, places=7)
+        self.assertAlmostEqual(markers.markers[4].scale.x, 0.25, places=7)
+        self.assertAlmostEqual(markers.markers[4].scale.y, 0.25, places=7)
+        self.assertAlmostEqual(markers.markers[5].scale.x, 0.35, places=7)
+        self.assertAlmostEqual(markers.markers[5].scale.y, 0.35, places=7)
+
+    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_transforms(self):
+    #     pass
+
+
+rostest.rosrun('aruco_pose', 'test_aruco_map', TestArucoMapPass, sys.argv)

aruco_pose/test/test_parser_pass.test

@@ -0,0 +1,13 @@
+<launch>
+    <node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager"/>
+
+    <node name="aruco_map" pkg="nodelet" type="nodelet" args="load aruco_pose/aruco_map nodelet_manager" clear_params="true">
+        <remap from="image_raw" to="main_camera/image_raw"/>
+        <remap from="camera_info" to="main_camera/camera_info"/>
+        <remap from="markers" to="aruco_detect/markers"/>
+        <param name="type" value="map"/>
+        <param name="map" value="$(find aruco_pose)/test/test_parser_pass.txt"/>
+    </node>
+
+    <test test-name="test_aruco_map" pkg="aruco_pose" type="test_parser_pass.py"/>
+</launch>

aruco_pose/test/test_parser_pass.txt

@@ -0,0 +1,23 @@
+# Parser test #1 - correct file
+# 1. Commentary test
+#Commentary text without space after pound sign
+# Commentary text with space after pound sign
+    # Commentary text with spaces before pound sign
+	# Commentary text with tab before pound sign
+			# Text with tabs before pound sign
+# Empty line test:
+
+# All-whitespace line test:
+		    
+# 2. Default coordinates test
+# Fully filled marker description, tab-delimited:
+1	0.33	0	0	0	0	0	0
+# Fully filled marker description, space-delimited:
+2 0.225 1 1 1 0 0 0
+# Default roll, pitch, yaw angles
+3 0.45 1 0 0.5
+# Default roll, pitch, yaw, z
+4 0.15 0 1
+# Inline commentary
+5 0.25 1 0.5# Comment straight after digit
+6 0.35 2.2 0.2 # Comment with a space after digit

book.json

@@ -4,6 +4,7 @@
     "author": "Copter Express",
     "language": "ru",
     "root": "docs/",
+    "gitbook": "^3.2.2",
     "plugins": [
         "youtube",
         "richquotes@https://github.com/okalachev/gitbook-plugin-richquotes.git",

builder/assets/hardware_setup.sh

@@ -8,6 +8,10 @@
 #
 # Author: Artem Smirnov <urpylka@gmail.com>
 #
+# Distributed under MIT License (available at https://opensource.org/licenses/MIT).
+# The above copyright notice and this permission notice shall be included in all
+# copies or substantial portions of the Software.
+#
 
 set -e # Exit immidiately on non-zero result
 

builder/assets/init_rpi.sh

@@ -8,6 +8,10 @@
 #
 # Author: Artem Smirnov <urpylka@gmail.com>
 #
+# Distributed under MIT License (available at https://opensource.org/licenses/MIT).
+# The above copyright notice and this permission notice shall be included in all
+# copies or substantial portions of the Software.
+#
 
 set -e # Exit immidiately on non-zero result
 

builder/assets/kinetic-rosdep-clever.yaml

@@ -541,3 +541,12 @@ tf2_web_republisher:
 image_publisher:
   debian:
     stretch: [ros-kinetic-image-publisher]
+raspberry-kernel-headers:
+  debian:
+    stretch: [raspberry-kernel-headers]
+ddynamic_reconfigure:
+  debian:
+    stretch: [ros-kinetic-ddynamic-reconfigure]
+realsense2_camera:
+  debian:
+    stretch: [ros-kinetic-realsense2-camera]

builder/assets/pigpiod.service

@@ -0,0 +1,8 @@
+[Unit]
+Description=Daemon required to control GPIO pins via pigpio
+[Service]
+ExecStart=/usr/bin/pigpiod -l -t 0 -x 0x0FFF3FF0
+ExecStop=/bin/systemctl kill pigpiod
+Type=forking
+[Install]
+WantedBy=multi-user.target

builder/image-build.sh

@@ -1,17 +1,21 @@
 #! /usr/bin/env bash
 
 #
-# Script for build the image. Used builder script of the target repo
+# Script for build the image. Used builder script of the target repo.
 # For build: docker run --privileged -it --rm -v /dev:/dev -v $(pwd):/builder/repo smirart/builder
 #
 # Copyright (C) 2018 Copter Express Technologies
 #
 # Author: Artem Smirnov <urpylka@gmail.com>
 #
+# Distributed under MIT License (available at https://opensource.org/licenses/MIT).
+# The above copyright notice and this permission notice shall be included in all
+# copies or substantial portions of the Software.
+#
 
 set -e # Exit immidiately on non-zero result
 
-SOURCE_IMAGE="https://downloads.raspberrypi.org/raspbian_lite/images/raspbian_lite-2018-11-15/2018-11-13-raspbian-stretch-lite.zip"
+SOURCE_IMAGE="https://downloads.raspberrypi.org/raspbian_lite/images/raspbian_lite-2019-04-09/2019-04-08-raspbian-stretch-lite.zip"
 
 export DEBIAN_FRONTEND=${DEBIAN_FRONTEND:='noninteractive'}
 export LANG=${LANG:='C.UTF-8'}
@@ -109,6 +113,7 @@ ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/roscore
 ${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/pigpiod.service' '/lib/systemd/system/'
 # ${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-init.sh

@@ -1,12 +1,16 @@
 #! /usr/bin/env bash
 
 #
-# Script for initialisation image
+# Script for image initialisation
 #
 # Copyright (C) 2018 Copter Express Technologies
 #
 # Author: Artem Smirnov <urpylka@gmail.com>
 #
+# Distributed under MIT License (available at https://opensource.org/licenses/MIT).
+# The above copyright notice and this permission notice shall be included in all
+# copies or substantial portions of the Software.
+#
 
 set -e # Exit immidiately on non-zero result
 

builder/image-network.sh

@@ -1,12 +1,16 @@
 #! /usr/bin/env bash
 
 #
-# Script for network configure
+# Script for network configuration
 #
 # Copyright (C) 2018 Copter Express Technologies
 #
 # Author: Artem Smirnov <urpylka@gmail.com>
 #
+# Distributed under MIT License (available at https://opensource.org/licenses/MIT).
+# The above copyright notice and this permission notice shall be included in all
+# copies or substantial portions of the Software.
+#
 
 set -e # Exit immidiately on non-zero result
 

builder/image-ros.sh

@@ -8,6 +8,10 @@
 #
 # Author: Artem Smirnov <urpylka@gmail.com>
 #
+# Distributed under MIT License (available at https://opensource.org/licenses/MIT).
+# The above copyright notice and this permission notice shall be included in all
+# copies or substantial portions of the Software.
+#
 
 set -e # Exit immidiately on non-zero result
 
@@ -42,9 +46,10 @@ echo_stamp() {
 my_travis_retry() {
   local result=0
   local count=1
-  while [ $count -le 3 ]; do
+  local max_count=50
+  while [ $count -le $max_count ]; do
     [ $result -ne 0 ] && {
-      echo -e "\n${ANSI_RED}The command \"$@\" failed. Retrying, $count of 3.${ANSI_RESET}\n" >&2
+      echo -e "\nThe command \"$@\" failed. Retrying, $count of $max_count.\n" >&2
     }
     # ! { } ignores set -e, see https://stackoverflow.com/a/4073372
     ! { "$@"; result=$?; }
@@ -53,21 +58,21 @@ my_travis_retry() {
     sleep 1
   done
 
-  [ $count -gt 3 ] && {
-    echo -e "\n${ANSI_RED}The command \"$@\" failed 3 times.${ANSI_RESET}\n" >&2
+  [ $count -gt $max_count ] && {
+    echo -e "\nThe command \"$@\" failed $max_count times.\n" >&2
   }
 
   return $result
 }
 
 # TODO: 'kinetic-rosdep-clever.yaml' should add only if we use our repo?
-echo_stamp "Init rosdep" \
-&& rosdep init \
-&& echo "yaml file:///etc/ros/rosdep/kinetic-rosdep-clever.yaml" >> /etc/ros/rosdep/sources.list.d/20-default.list \
-&& rosdep update
+echo_stamp "Init rosdep"
+my_travis_retry rosdep init
+echo "yaml file:///etc/ros/rosdep/kinetic-rosdep-clever.yaml" >> /etc/ros/rosdep/sources.list.d/20-default.list
+my_travis_retry rosdep update
 
 echo_stamp "Populate rosdep for ROS user"
-sudo -u pi rosdep update
+my_travis_retry sudo -u pi rosdep update
 
 resolve_rosdep() {
   # TEMPLATE: resolve_rosdep <CATKIN_PATH> <ROS_DISTRO> <OS_DISTRO> <OS_VERSION>
@@ -137,6 +142,10 @@ fi
 
 export ROS_IP='127.0.0.1' # needed for running tests
 
+echo_stamp "Reconfiguring Clever repository for simplier unshallowing"
+cd /home/pi/catkin_ws/src/clever
+git config remote.origin.fetch "+refs/heads/*:refs/remotes/origin/*"
+
 echo_stamp "Installing CLEVER" \
 && cd /home/pi/catkin_ws/src/clever \
 && git status \
@@ -168,7 +177,8 @@ apt-get install -y --no-install-recommends \
     ros-kinetic-rosserial \
     ros-kinetic-usb-cam \
     ros-kinetic-vl53l1x \
-    ros-kinetic-opencv3=3.3.19-0stretch
+    ros-kinetic-opencv3=3.3.19-0stretch \
+    ros-kinetic-rosshow
 
 # TODO move GeographicLib datasets to Mavros debian package
 echo_stamp "Install GeographicLib datasets (needs for mavros)" \

builder/image-software.sh

@@ -1,12 +1,16 @@
 #! /usr/bin/env bash
 
 #
-# Script for install software to the image.
+# Script for installing software to the image.
 #
 # Copyright (C) 2018 Copter Express Technologies
 #
 # Author: Artem Smirnov <urpylka@gmail.com>
 #
+# Distributed under MIT License (available at https://opensource.org/licenses/MIT).
+# The above copyright notice and this permission notice shall be included in all
+# copies or substantial portions of the Software.
+#
 
 set -e # Exit immidiately on non-zero result
 
@@ -59,7 +63,7 @@ echo_stamp "Install apt keys & repos"
 curl http://repo.coex.space/aptly_repo_signing.key 2> /dev/null | apt-key add -
 apt-get update \
 && apt-get install --no-install-recommends -y -qq dirmngr=2.1.18-8~deb9u4 > /dev/null \
-&& apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key 421C365BD9FF1F717815A3895523BAEEB01FA116
+&& apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654
 
 echo "deb http://packages.ros.org/ros/ubuntu stretch main" > /etc/apt/sources.list.d/ros-latest.list
 echo "deb http://repo.coex.space/rpi-ros-kinetic stretch main" > /etc/apt/sources.list.d/rpi-ros-kinetic.list
@@ -84,9 +88,9 @@ lsof=4.89+dfsg-0.1 \
 git \
 dnsmasq=2.76-5+rpt1+deb9u1  \
 tmux=2.3-4 \
-vim=2:8.0.0197-4+deb9u1 \
+vim \
 cmake=3.7.2-1 \
-libjpeg8-dev=8d1-2 \
+libjpeg8=8d1-2 \
 tcpdump \
 ltrace \
 libpoco-dev=1.7.6+dfsg1-5+deb9u1 \
@@ -99,14 +103,23 @@ libffi-dev \
 monkey=1.6.9-1 \
 pigpio python-pigpio python3-pigpio \
 i2c-tools \
+espeak espeak-data python-espeak \
 ntpdate \
 python-dev \
 python3-dev \
+python-systemd \
 && echo_stamp "Everything was installed!" "SUCCESS" \
 || (echo_stamp "Some packages wasn't installed!" "ERROR"; exit 1)
 
 echo_stamp "Updating kernel to fix camera bug"
-apt-get install --no-install-recommends -y raspberrypi-kernel=1.20190215-1
+apt-get install --no-install-recommends -y \
+	raspberrypi-kernel=1.20190517-1 \
+	raspberrypi-bootloader=1.20190517-1 \
+	libraspberrypi-bin=1.20190517-1 \
+	libraspberrypi-dev=1.20190517-1 \
+	libraspberrypi0=1.20190517-1 \
+	wireless-regdb=2018.05.09-0~rpt1 \
+	wpasupplicant=2:2.6-21~bpo9~rpt1
 
 # Deny byobu to check available updates
 sed -i "s/updates_available//" /usr/share/byobu/status/status
@@ -154,6 +167,9 @@ syntax on
 autocmd BufNewFile,BufRead *.launch set syntax=xml
 EOF
 
+echo_stamp "Change default keyboard layout to US"
+sed -i 's/XKBLAYOUT="gb"/XKBLAYOUT="us"/g' /etc/default/keyboard
+
 echo_stamp "Attempting to kill dirmngr"
 gpgconf --kill dirmngr
 # dirmngr is only used by apt-key, so we can safely kill it.

builder/image-validate.sh

@@ -7,6 +7,10 @@
 #
 # Author: Oleg Kalachev <okalachev@gmail.com>
 #
+# Distributed under MIT License (available at https://opensource.org/licenses/MIT).
+# The above copyright notice and this permission notice shall be included in all
+# copies or substantial portions of the Software.
+#
 
 set -ex
 

builder/test/tests.py

@@ -25,6 +25,6 @@ import pymavlink
 from pymavlink import mavutil
 import rpi_ws281x
 import pigpio
+from espeak import espeak
 
 print cv2.getBuildInformation()
-

builder/test/tests.sh

@@ -28,6 +28,7 @@ monkey --version
 pigpiod -v
 i2cdetect -V
 butterfly -h
+espeak --version
 
 # ros stuff
 
@@ -46,3 +47,4 @@ rosversion rosserial
 rosversion usb_cam
 rosversion cv_camera
 rosversion web_video_server
+rosversion rosshow

clever/CMakeLists.txt

@@ -162,8 +162,6 @@ add_executable(rc src/rc.cpp)
 
 add_executable(camera_markers src/camera_markers.cpp)
 
-add_executable(frames src/frames.cpp)
-
 add_executable(vpe_publisher src/vpe_publisher.cpp)
 
 target_link_libraries(simple_offboard
@@ -175,8 +173,6 @@ target_link_libraries(rc ${catkin_LIBRARIES})
 
 target_link_libraries(camera_markers ${catkin_LIBRARIES})
 
-target_link_libraries(frames ${catkin_LIBRARIES})
-
 target_link_libraries(vpe_publisher ${catkin_LIBRARIES})
 
 add_dependencies(simple_offboard clever_generate_messages_cpp)

clever/launch/clever.launch

@@ -9,7 +9,6 @@
     <arg name="aruco" default="false"/>
     <arg name="rc" default="true"/>
     <arg name="rangefinder_vl53l1x" default="false"/>
-    <arg name="arduino" default="false"/>
 
     <!-- mavros -->
     <include file="$(find clever)/launch/mavros.launch">
@@ -21,6 +20,7 @@
     <!-- web video server -->
     <node name="web_video_server" pkg="web_video_server" type="web_video_server" if="$(arg web_video_server)" required="false" respawn="true" respawn_delay="5">
         <param name="default_stream_type" value="ros_compressed"/>
+        <param name="publish_rate" value="1.0"/>
     </node>
 
     <!-- aruco markers -->
@@ -46,11 +46,6 @@
         <param name="reference_frames/base_link" value="map"/>
     </node>
 
-    <!-- Auxiliary frames -->
-    <node name="frames" pkg="clever" type="frames" output="screen">
-        <param name="body/frame_id" value="body"/>
-    </node>
-
     <!-- main camera -->
     <include file="$(find clever)/launch/main_camera.launch" if="$(arg main_camera)"/>
 
@@ -63,15 +58,9 @@
     <!-- vl53l1x ToF rangefinder -->
     <node name="vl53l1x" pkg="vl53l1x" type="vl53l1x_node" output="screen" if="$(arg rangefinder_vl53l1x)">
         <param name="frame_id" value="rangefinder"/>
-        <param name="offset" value="-0.05"/>
         <remap from="~range" to="mavros/distance_sensor/rangefinder_sub"/> <!-- redirect data to FCU -->
     </node>
 
     <!-- rc backend -->
     <node name="rc" pkg="clever" type="rc" output="screen" if="$(arg rc)"/>
-
-    <!-- Arduino bridge -->
-    <node pkg="rosserial_python" type="serial_node.py" name="serial_node" output="screen" if="$(arg arduino)">
-        <param name="port" value="/dev/serial/by-id/usb-1a86_USB2.0-Serial-if00-port0"/>
-    </node>
 </launch>

clever/launch/main_camera.launch

@@ -24,6 +24,7 @@
         <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 -->
+        <param name="rescale_camera_info" value="true"/> <!-- automatically rescale camera calibration info -->
 
         <!-- camera resolution, NOTE: camera_info file should match it -->
         <param name="image_width" value="320"/>

clever/package.xml

@@ -37,6 +37,7 @@
   <depend>mjpg-streamer</depend>
   <depend>rosbridge_server</depend>
   <depend>web_video_server</depend>
+  <exec_depend>python-pymavlink</exec_depend>
   <!-- Use test_depend for packages you need only for testing: -->
   <!--   <test_depend>gtest</test_depend> -->
 

clever/requirements.txt

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

clever/src/frames.cpp

@@ -1,63 +0,0 @@
-/*
- * Auxiliary TF frames for CLEVER drone kit:
- * - Body frame (drone body with zero pitch and roll).
- * - TODO: REP-0105 `odom` frame emulation: continuous frame without discrete jumps.
- * - TODO: Terrain frame (base on ALTITUDE message).
- * - TODO: map_upside_down frame
- * - TODO: home frame?
- *
- * Copyright (C) 2018 Copter Express Technologies
- *
- * Author: Oleg Kalachev <okalachev@gmail.com>
- *
- * Distributed under MIT License (available at https://opensource.org/licenses/MIT).
- * The above copyright notice and this permission notice shall be included in all
- * copies or substantial portions of the Software.
- */
-
-// TODO: consider implementing as a mavros plugin
-
-#include <string>
-#include <memory>
-#include <ros/ros.h>
-#include <tf/transform_datatypes.h>
-#include <tf2_ros/transform_broadcaster.h>
-#include <geometry_msgs/TransformStamped.h>
-#include <geometry_msgs/PoseStamped.h>
-
-using std::string;
-
-static std::shared_ptr<tf2_ros::TransformBroadcaster> br;
-static geometry_msgs::TransformStamped body;
-
-inline void publishBody(const geometry_msgs::PoseStamped& pose)
-{
-	// Get only yaw from pose
-	tf::Quaternion q;
-	q.setRPY(0, 0, tf::getYaw(pose.pose.orientation));
-	tf::quaternionTFToMsg(q, body.transform.rotation);
-
-	body.transform.translation.x = pose.pose.position.x;
-	body.transform.translation.y = pose.pose.position.y;
-	body.transform.translation.z = pose.pose.position.z;
-	body.header.frame_id = pose.header.frame_id;
-	body.header.stamp = pose.header.stamp;
-	br->sendTransform(body);
-}
-
-void poseCallback(const geometry_msgs::PoseStamped& pose)
-{
-	publishBody(pose);
-}
-
-int main(int argc, char **argv) {
-	ros::init(argc, argv, "frames");
-	ros::NodeHandle nh, nh_priv("~");
-
-	nh_priv.param<string>("body/frame_id", body.child_frame_id, "body");
-
-	br = std::make_shared<tf2_ros::TransformBroadcaster>();
-	ros::Subscriber pose_sub = nh.subscribe("mavros/local_position/pose", 1, &poseCallback);
-	ROS_INFO("frames: ready");
-	ros::spin();
-}

clever/src/selfcheck.py

@@ -1,21 +1,35 @@
 #!/usr/bin/env python
 
+# Copyright (C) 2018 Copter Express Technologies
+#
+# Author: Oleg Kalachev <okalachev@gmail.com>
+#
+# Distributed under MIT License (available at https://opensource.org/licenses/MIT).
+# The above copyright notice and this permission notice shall be included in all
+# copies or substantial portions of the Software.
+
 import math
-from subprocess import Popen, PIPE
+import subprocess
 import re
 import traceback
+from threading import Event
+import numpy
 import rospy
+from systemd import journal
+import tf2_ros
+import tf2_geometry_msgs
+from pymavlink import mavutil
 from std_srvs.srv import Trigger
 from sensor_msgs.msg import Image, CameraInfo, NavSatFix, Imu, Range
-from mavros_msgs.msg import State, OpticalFlowRad
+from mavros_msgs.msg import State, OpticalFlowRad, Mavlink
 from mavros_msgs.srv import ParamGet
-from geometry_msgs.msg import PoseStamped, TwistStamped, PoseWithCovarianceStamped
+from geometry_msgs.msg import PoseStamped, TwistStamped, PoseWithCovarianceStamped, Vector3Stamped
+from visualization_msgs.msg import MarkerArray as VisualizationMarkerArray
 import tf.transformations as t
 from aruco_pose.msg import MarkerArray
+from mavros import mavlink
 
 
-# TODO: roscore is running
-# TODO: clever.service is running
 # TODO: check attitude is present
 # TODO: disk free space
 # TODO: map, base_link, body
@@ -28,28 +42,41 @@ from aruco_pose.msg import MarkerArray
 rospy.init_node('selfcheck')
 
 
+tf_buffer = tf2_ros.Buffer()
+tf_listener = tf2_ros.TransformListener(tf_buffer)
+
+
 failures = []
+infos = []
+current_check = None
 
 
 def failure(text, *args):
-    failures.append(text % args)
+    msg = text % args
+    rospy.logwarn('%s: %s', current_check, msg)
+    failures.append(msg)
+
+
+def info(text, *args):
+    msg = text % args
+    rospy.loginfo('%s: %s', current_check, msg)
+    infos.append(msg)
 
 
 def check(name):
     def inner(fn):
         def wrapper(*args, **kwargs):
             failures[:] = []
+            infos[:] = []
+            global current_check
+            current_check = name
             try:
                 fn(*args, **kwargs)
-                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 occurred', name)
+                rospy.logerr('%s: exception occurred', name)
                 return
-            if not failures:
+            if not failures and not infos:
                 rospy.loginfo('%s: OK', name)
         return wrapper
     return inner
@@ -59,7 +86,12 @@ param_get = rospy.ServiceProxy('mavros/param/get', ParamGet)
 
 
 def get_param(name):
-    res = param_get(param_id=name)
+    try:
+        res = param_get(param_id=name)
+    except rospy.ServiceException as e:
+        failure('%s: %s', name, str(e))
+        return None
+
     if not res.success:
         failure('Unable to retrieve PX4 parameter %s', name)
     else:
@@ -68,36 +100,116 @@ def get_param(name):
         return res.value.real
 
 
+recv_event = Event()
+link = mavutil.mavlink.MAVLink('', 255, 1)
+mavlink_pub = rospy.Publisher('mavlink/to', Mavlink, queue_size=1)
+mavlink_recv = ''
+
+
+def mavlink_message_handler(msg):
+    global mavlink_recv
+    if msg.msgid == 126:
+        mav_bytes_msg = mavlink.convert_to_bytes(msg)
+        mav_msg = link.decode(mav_bytes_msg)
+        mavlink_recv += ''.join(chr(x) for x in mav_msg.data[:mav_msg.count])
+        if 'nsh>' in mavlink_recv:
+            # Remove the last line, including newline before prompt
+            mavlink_recv = mavlink_recv[:mavlink_recv.find('nsh>') - 1]
+            recv_event.set()
+
+
+mavlink_sub = rospy.Subscriber('mavlink/from', Mavlink, mavlink_message_handler)
+# FIXME: not sleeping here still breaks things
+rospy.sleep(0.5)
+
+
+def mavlink_exec(cmd, timeout=3.0):
+    global mavlink_recv
+    mavlink_recv = ''
+    recv_event.clear()
+    if not cmd.endswith('\n'):
+        cmd += '\n'
+    msg = mavutil.mavlink.MAVLink_serial_control_message(
+        device=mavutil.mavlink.SERIAL_CONTROL_DEV_SHELL,
+        flags=mavutil.mavlink.SERIAL_CONTROL_FLAG_RESPOND | mavutil.mavlink.SERIAL_CONTROL_FLAG_EXCLUSIVE |
+              mavutil.mavlink.SERIAL_CONTROL_FLAG_MULTI,
+        timeout=3,
+        baudrate=0,
+        count=len(cmd),
+        data=map(ord, cmd.ljust(70, '\0')))
+    msg.pack(link)
+    ros_msg = mavlink.convert_to_rosmsg(msg)
+    mavlink_pub.publish(ros_msg)
+    recv_event.wait(timeout)
+    return mavlink_recv
+
+
 @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)')
+            return
+
+        # Make sure the console is available to us
+        mavlink_exec('\n')
+        version_str = mavlink_exec('ver all')
+        if version_str == '':
+            info('no version data available from SITL')
+
+        r = re.compile(r'^FW (git tag|version): (v?\d\.\d\.\d.*)$')
+        is_clever_firmware = False
+        for ver_line in version_str.split('\n'):
+            match = r.search(ver_line)
+            if match is not None:
+                field, version = match.groups()
+                info('firmware %s: %s' % (field, version))
+                if 'clever' in version:
+                    is_clever_firmware = True
+
+        if not is_clever_firmware:
+            failure('not running Clever PX4 firmware, check http://clever.copterexpress.com/firmware.html')
 
         est = get_param('SYS_MC_EST_GROUP')
         if est == 1:
-            rospy.loginfo('Selected estimator: LPE')
+            info('selected estimator: LPE')
             fuse = get_param('LPE_FUSION')
             if fuse & (1 << 4):
-                rospy.loginfo('LPE_FUSION: land detector fusion is enabled')
+                info('LPE_FUSION: land detector fusion is enabled')
             else:
-                rospy.loginfo('LPE_FUSION: land detector fusion is disabled')
+                info('LPE_FUSION: land detector fusion is disabled')
             if fuse & (1 << 7):
-                rospy.loginfo('LPE_FUSION: barometer fusion is enabled')
+                info('LPE_FUSION: barometer fusion is enabled')
             else:
-                rospy.loginfo('LPE_FUSION: barometer fusion is disabled')
+                info('LPE_FUSION: barometer fusion is disabled')
 
         elif est == 2:
-            rospy.loginfo('Selected estimator: EKF2')
+            info('selected estimator: EKF2')
         else:
-            failure('Unknown selected estimator: %s', est)
+            failure('unknown selected estimator: %s', est)
 
     except rospy.ROSException:
         failure('no MAVROS state (check wiring)')
 
 
-@check('Camera')
+def describe_direction(v):
+    if v.x > 0.9:
+        return 'forward'
+    elif v.x < - 0.9:
+        return 'backward'
+    elif v.y > 0.9:
+        return 'left'
+    elif v.y < -0.9:
+        return 'right'
+    elif v.z > 0.9:
+        return 'upward'
+    elif v.z < -0.9:
+        return 'downward'
+    else:
+        return None
+
+
 def check_camera(name):
     try:
         img = rospy.wait_for_message(name + '/image_raw', Image, timeout=1)
@@ -105,32 +217,98 @@ def check_camera(name):
         failure('%s: no images (is the camera connected properly?)', name)
         return
     try:
-        info = rospy.wait_for_message(name + '/camera_info', CameraInfo, timeout=1)
+        camera_info = rospy.wait_for_message(name + '/camera_info', CameraInfo, timeout=1)
     except rospy.ROSException:
         failure('%s: no calibration info', name)
         return
 
-    if img.width != info.width:
-        failure('%s: calibration width doesn\'t match image width (%d != %d)', name, info.width, img.width)
-    if img.height != info.height:
-        failure('%s: calibration height doesn\'t match image height (%d != %d))', name, info.height, img.height)
+    if img.width != camera_info.width:
+        failure('%s: calibration width doesn\'t match image width (%d != %d)', name, camera_info.width, img.width)
+    if img.height != camera_info.height:
+        failure('%s: calibration height doesn\'t match image height (%d != %d))', name, camera_info.height, img.height)
+
+    try:
+        optical = Vector3Stamped()
+        optical.header.frame_id = img.header.frame_id
+        optical.vector.z = 1
+        cable = Vector3Stamped()
+        cable.header.frame_id = img.header.frame_id
+        cable.vector.y = 1
+
+        optical = describe_direction(tf_buffer.transform(optical, 'base_link').vector)
+        cable = describe_direction(tf_buffer.transform(cable, 'base_link').vector)
+        if not optical or not cable:
+            info('%s: custom camera orientation detected', name)
+        else:
+           info('camera is oriented %s, camera cable goes %s', optical, cable)
+
+    except tf2_ros.TransformException:
+        failure('cannot transform from base_link to camera frame')
 
 
-@check('ArUco detector')
+@check('Main camera')
+def check_main_camera():
+    check_camera('main_camera')
+
+
+def is_process_running(binary, exact=False, full=False):
+    try:
+        args = ['pgrep']
+        if exact:
+            args.append('-x')  # match exactly with the command name
+        if full:
+            args.append('-f')  # use full process name to match
+        args.append(binary)
+        subprocess.check_output(args)
+        return True
+    except subprocess.CalledProcessError:
+        return False
+
+
+@check('ArUco markers')
 def check_aruco():
-    try:
-        rospy.wait_for_message('aruco_detect/markers', MarkerArray, timeout=1)
-    except rospy.ROSException:
-        failure('no markers detection')
+    if is_process_running('aruco_detect', full=True):
+        info('aruco_detect/length = %g m', rospy.get_param('aruco_detect/length'))
+        known_tilt = rospy.get_param('aruco_detect/known_tilt')
+        if known_tilt == 'map':
+            known_tilt += ' (ALL markers are on the floor)'
+        elif known_tilt == 'map_flipped':
+            known_tilt += ' (ALL markers are on the ceiling)'
+        info('aruco_detector/known_tilt = %s', known_tilt)
+        try:
+            rospy.wait_for_message('aruco_detect/markers', MarkerArray, timeout=1)
+        except rospy.ROSException:
+            failure('no markers detection')
+            return
+    else:
+        info('aruco_detect is not running')
         return
-    try:
-        rospy.wait_for_message('aruco_map/pose', PoseWithCovarianceStamped, timeout=1)
-    except rospy.ROSException:
-        failure('no map detection')
+
+    if is_process_running('aruco_map', full=True):
+        known_tilt = rospy.get_param('aruco_map/known_tilt')
+        if known_tilt == 'map':
+            known_tilt += ' (marker\'s map is on the floor)'
+        elif known_tilt == 'map_flipped':
+            known_tilt += ' (marker\'s map is on the ceiling)'
+        info('aruco_map/known_tilt = %s', known_tilt)
+
+        try:
+            visualization = rospy.wait_for_message('aruco_map/visualization', VisualizationMarkerArray, timeout=1)
+            info('map has %s markers', len(visualization.markers))
+        except:
+            failure('cannot read aruco_map/visualization topic')
+
+        try:
+            rospy.wait_for_message('aruco_map/pose', PoseWithCovarianceStamped, timeout=1)
+        except rospy.ROSException:
+            failure('no map detection')
+    else:
+        info('aruco_map is not running')
 
 
 @check('Vision position estimate')
 def check_vpe():
+    vis = None
     try:
         vis = rospy.wait_for_message('mavros/vision_pose/pose', PoseStamped, timeout=1)
     except rospy.ROSException:
@@ -138,7 +316,11 @@ def check_vpe():
             vis = rospy.wait_for_message('mavros/mocap/pose', PoseStamped, timeout=1)
         except rospy.ROSException:
             failure('no VPE or MoCap messages')
-            return
+            # check if vpe_publisher is running
+            try:
+                subprocess.check_output(['pgrep', '-x', 'vpe_publisher'])
+            except subprocess.CalledProcessError:
+                return  # it's not running, skip following checks
 
     # check PX4 settings
     est = get_param('SYS_MC_EST_GROUP')
@@ -150,26 +332,29 @@ def check_vpe():
         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)
+            info('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')
+            failure('vision position fusion 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'))
+        info('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')
+            failure('vision position fusion is disabled, change EKF2_AID_MASK parameter')
         if not fuse & (1 << 4):
-            failure('vision yaw fusing is disabled, change EKF2_AID_MASK parameter')
+            failure('vision yaw fusion 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'))
+        info('EKF2_EVA_NOISE is %.3f, EKF2_EVP_NOISE is %.3f',
+                      get_param('EKF2_EVA_NOISE'),
+                      get_param('EKF2_EVP_NOISE'))
+
+    if not vis:
+        return
 
     # check vision pose and estimated pose inconsistency
     try:
@@ -240,7 +425,7 @@ def check_velocity():
             failure('vertical velocity estimation is %.2f m/s; is copter staying still?' % vert)
 
         angular = velocity.twist.angular
-        ANGULAR_VELOCITY_LIMIT = 0.01
+        ANGULAR_VELOCITY_LIMIT = 0.1
         if abs(angular.x) > ANGULAR_VELOCITY_LIMIT:
             failure('pitch rate estimation is %.2f rad/s (%.2f deg/s); is copter staying still?',
                     angular.x, math.degrees(angular.x))
@@ -276,32 +461,32 @@ def check_optical_flow():
         if est == 1:
             fuse = get_param('LPE_FUSION')
             if not fuse & (1 << 1):
-                failure('optical flow fusing is disabled, change LPE_FUSION parameter')
+                failure('optical flow fusion 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:
+            if not numpy.isclose(scale, 1.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'))
+            info('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')
+                failure('optical flow fusion 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'))
+            info('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)')
@@ -312,13 +497,13 @@ def check_rangefinder():
     # TODO: check FPS!
     rng = False
     try:
-        rospy.wait_for_message('mavros/distance_sensor/rangefinder_sub', Range, timeout=0.5)
+        rospy.wait_for_message('mavros/distance_sensor/rangefinder_sub', Range, timeout=4)
         rng = True
     except rospy.ROSException:
         failure('no rangefinder data from Raspberry')
 
     try:
-        rospy.wait_for_message('mavros/distance_sensor/rangefinder', Range, timeout=0.5)
+        rospy.wait_for_message('mavros/distance_sensor/rangefinder', Range, timeout=4)
         rng = True
     except rospy.ROSException:
         failure('no rangefinder data from PX4')
@@ -330,28 +515,26 @@ def check_rangefinder():
     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')
+            info('"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')
+            info('"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')
+            info('EKF2_HGT_MODE != Range sensor, NOT operating over flat surface')
         else:
-            rospy.loginfo('EKF2_HGT_MODE = Range sensor, operating over flat surface')
+            info('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')
+            info('EKF2_RNG_AID != 1, range sensor aiding disabled')
         else:
-            rospy.loginfo('EKF2_RNG_AID = 1, range sensor aiding enabled')
+            info('EKF2_RNG_AID = 1, range sensor aiding enabled')
 
 
 @check('Boot duration')
 def check_boot_duration():
-    proc = Popen('systemd-analyze', stdout=PIPE)
-    proc.wait()
-    output = proc.communicate()[0]
+    output = subprocess.check_output('systemd-analyze')
     r = re.compile(r'([\d\.]+)s$')
     duration = float(r.search(output).groups()[0])
     if duration > 15:
@@ -362,9 +545,7 @@ def check_boot_duration():
 def check_cpu_usage():
     WHITELIST = 'nodelet',
     CMD = "top -n 1 -b -i | tail -n +8 | awk '{ printf(\"%-8s\\t%-8s\\t%-8s\\n\", $1, $9, $12); }'"
-    proc = Popen(CMD, stdout=PIPE, shell=True)
-    proc.wait()
-    output = proc.communicate()[0]
+    output = subprocess.check_output(CMD, shell=True)
     processes = output.split('\n')
     for process in processes:
         if not process:
@@ -376,13 +557,68 @@ def check_cpu_usage():
                     cpu.strip(), cmd.strip(), pid.strip())
 
 
+@check('clever.service')
+def check_clever_service():
+    output = subprocess.check_output('systemctl show -p ActiveState --value clever.service'.split())
+    if 'inactive' in output:
+        failure('clever.service is not running, try sudo systemctl restart clever')
+        return
+    j = journal.Reader()
+    j.this_boot()
+    j.add_match(_SYSTEMD_UNIT='clever.service')
+    j.add_disjunction()
+    j.add_match(UNIT='clever.service')
+    node_errors = []
+    r = re.compile(r'^(.*)\[(FATAL|ERROR)\] \[\d+.\d+\]: (.*)$')
+    for event in j:
+        msg = event['MESSAGE']
+        if ('Stopped Clever ROS package' in msg) or ('Started Clever ROS package' in msg):
+            node_errors = []
+        elif ('[ERROR]' in msg) or ('[FATAL]' in msg):
+            msg = r.search(msg).groups()[2]
+            if msg in node_errors:
+                continue
+            node_errors.append(msg)
+    for error in node_errors:
+        failure(error)
+
+
+@check('Image')
+def check_image():
+    info('version: %s', open('/etc/clever_version').read().strip())
+
+
+@check('Preflight status')
+def check_preflight_status():
+    # Make sure the console is available to us
+    mavlink_exec('\n')
+    cmdr_output = mavlink_exec('commander check')
+    if cmdr_output == '':
+        failure('No data from FCU')
+        return
+    cmdr_lines = cmdr_output.split('\n')
+    r = re.compile(r'^(.*)(Preflight|Prearm) check: (.*)')
+    for line in cmdr_lines:
+        if 'WARN' in line:
+            failure(line[line.find(']') + 2:])
+            continue
+        match = r.search(line)
+        if match is not None:
+            check_status = match.groups()[2]
+            if check_status != 'OK':
+                failure(' '.join([match.groups()[1], 'check:', check_status]))
+
+
 def selfcheck():
+    check_image()
+    check_clever_service()
     check_fcu()
     check_imu()
     check_local_position()
     check_velocity()
     check_global_position()
-    check_camera('main_camera')
+    check_preflight_status()
+    check_main_camera()
     check_aruco()
     check_simpleoffboard()
     check_optical_flow()

clever/src/simple_offboard.cpp

@@ -54,6 +54,7 @@ using mavros_msgs::Thrust;
 
 // tf2
 tf2_ros::Buffer tf_buffer;
+std::shared_ptr<tf2_ros::TransformBroadcaster> transform_broadcaster;
 
 // Parameters
 string local_frame;
@@ -70,6 +71,7 @@ ros::Duration global_position_timeout;
 ros::Duration battery_timeout;
 float default_speed;
 bool auto_release;
+bool land_only_in_offboard;
 std::map<string, string> reference_frames;
 
 // Publishers
@@ -87,6 +89,7 @@ AttitudeTarget att_raw_msg;
 Thrust thrust_msg;
 TwistStamped rates_msg;
 TransformStamped target;
+geometry_msgs::TransformStamped body;
 
 // State
 PoseStamped nav_start;
@@ -127,6 +130,29 @@ void handleMessage(const T& msg)
 	STORAGE = msg;
 }
 
+inline void publishBodyFrame()
+{
+	if (body.child_frame_id.empty()) return;
+
+	tf::Quaternion q;
+	q.setRPY(0, 0, tf::getYaw(local_position.pose.orientation));
+	tf::quaternionTFToMsg(q, body.transform.rotation);
+
+	body.transform.translation.x = local_position.pose.position.x;
+	body.transform.translation.y = local_position.pose.position.y;
+	body.transform.translation.z = local_position.pose.position.z;
+	body.header.frame_id = local_position.header.frame_id;
+	body.header.stamp = local_position.header.stamp;
+	transform_broadcaster->sendTransform(body);
+}
+
+void handleLocalPosition(const PoseStamped& pose)
+{
+	local_position = pose;
+	publishBodyFrame();
+	// TODO: terrain?, home?
+}
+
 // wait for transform without interrupting publishing setpoints
 inline bool waitTransform(const string& target, const string& source,
                           const ros::Time& stamp, const ros::Duration& timeout)
@@ -363,13 +389,12 @@ void publish(const ros::Time stamp)
 
 	if (!target.child_frame_id.empty()) {
 		if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == POSITION) {
-			static tf2_ros::TransformBroadcaster tf_broadcaster;
 			target.header = setpoint_position_transformed.header;
 			target.transform.translation.x = setpoint_position_transformed.pose.position.x;
 			target.transform.translation.y = setpoint_position_transformed.pose.position.y;
 			target.transform.translation.z = setpoint_position_transformed.pose.position.z;
 			target.transform.rotation = setpoint_position_transformed.pose.orientation;
-			tf_broadcaster.sendTransform(target);
+			transform_broadcaster->sendTransform(target);
 		}
 	}
 
@@ -644,6 +669,12 @@ bool land(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)
 
 		checkState();
 
+		if (land_only_in_offboard) {
+			if (state.mode != "OFFBOARD") {
+				throw std::runtime_error("Copter is not in OFFBOARD mode");
+			}
+		}
+
 		static mavros_msgs::SetMode sm;
 		sm.request.custom_mode = "AUTO.LAND";
 
@@ -682,13 +713,16 @@ int main(int argc, char **argv)
 	ros::NodeHandle nh, nh_priv("~");
 
 	tf2_ros::TransformListener tf_listener(tf_buffer);
+	transform_broadcaster = std::make_shared<tf2_ros::TransformBroadcaster>();
 
 	// Params
 	nh.param<string>("mavros/local_position/tf/frame_id", local_frame, "map");
 	nh.param<string>("mavros/local_position/tf/child_frame_id", fcu_frame, "base_link");
 	nh_priv.param("target_frame", target.child_frame_id, string("navigate_target"));
 	nh_priv.param("auto_release", auto_release, true);
+	nh_priv.param("land_only_in_offboard", land_only_in_offboard, true);
 	nh_priv.param("default_speed", default_speed, 0.5f);
+	nh_priv.param<string>("body_frame", body.child_frame_id, "body");
 	nh_priv.getParam("reference_frames", reference_frames);
 
 	state_timeout = ros::Duration(nh_priv.param("state_timeout", 3.0));
@@ -709,11 +743,11 @@ int main(int argc, char **argv)
 
 	// Telemetry subscribers
 	auto state_sub = nh.subscribe("mavros/state", 1, &handleMessage<mavros_msgs::State, state>);
-	auto local_position_sub = nh.subscribe("mavros/local_position/pose", 1, &handleMessage<PoseStamped, local_position>);
 	auto velocity_sub = nh.subscribe("mavros/local_position/velocity", 1, &handleMessage<TwistStamped, velocity>);
 	auto global_position_sub = nh.subscribe("mavros/global_position/global", 1, &handleMessage<NavSatFix, global_position>);
 	auto battery_sub = nh.subscribe("mavros/battery", 1, &handleMessage<BatteryState, battery>);
 	auto statustext_sub = nh.subscribe("mavros/statustext/recv", 1, &handleMessage<mavros_msgs::StatusText, statustext>);
+	auto local_position_sub = nh.subscribe("mavros/local_position/pose", 1, &handleLocalPosition);
 
 	// Setpoint publishers
 	position_pub = nh.advertise<PoseStamped>("mavros/setpoint_position/local", 1);

docs/en/4in1.md

@@ -27,4 +27,4 @@ Using Fig. 1a, 1b, 2a, 2b, map its own control signal to each motor, and connect
 
 For example, motor M3 that rotates counter-clockwise (top left corner) is controlled by signal S4 (green wire). It is connected to port 3.
 
-![Connecting 4 in 1 ESCs](../assets/cl3_connectionESC4in1.jpg)
+![Connecting 4 in 1 ESCs](../assets/en/cl3_connectionESC4in1.jpg)

docs/en/README.md

@@ -3,7 +3,7 @@ Clever
 
 <img src="../assets/clever3.png" align="right" width="300px" alt="Klever">
 
-"Clever" is an educational constructor set of a programmable quadcopter that consists of popular open source components, and a set of necessary documentation and libraries for working with it.
+CLEVER (Russian: *"Клевер"*, meaning *"Clover"*) is an educational kit of a programmable quadcopter that consists of popular open source components, and a set of necessary documentation and libraries for working with it.
 
 The kit includes a Pixhawk/Pixracer flight controller with the PX4 flight stack, a Raspberry Pi 3 as a controlling onboard computer, and a camera module for performing flights with the use of computer vision, and a set of various sensors and other peripherals.
 

docs/en/SUMMARY.md

@@ -4,14 +4,14 @@
 * Assembly
   * [Clever 2 assembly](assemble_2.md)
   * [Clever 3 assembly](assemble_3.md)
-  * [Installation of FPV](fpv.md)
-  * [Safety instruction](safety.md)
+  * [FPV Setup](fpv.md)
+  * [Safety tips](safety.md)
   * [Connecting 4 in 1 ESCs](4in1.md)
   * [Types of power connectors](connectortypes.md)
   * [Blanching](zap.md)
   * [Soldering safety](tb.md)
-  * [Using multimeter](test_connection.md)
-  * [Possible radio failures](radioerrors.md)
+  * [Multimeter usage](test_connection.md)
+  * [RC Troubleshooting](radioerrors.md)
   * [Connecting GPS](gps.md)
 * Configuration
   * [Initial setup](setup.md)
@@ -19,42 +19,50 @@
   * [Pixhawk / Pixracer Firmware](firmware.md)
   * [PX4 Parameters](px4_parameters.md)
   * [PID Setup](calibratePID.md)
-* Work with Raspberry Pi
+* Working with Raspberry Pi
   * [Raspberry Pi](raspberry.md)
   * [RPi Image](microsd_images.md)
   * [RPi Connection to the Pixhawk](connection.md)
   * [Wi-Fi connection](wifi.md)
-  * [SSH access to Raspberry Pi](ssh.md)
-  * [Configuring Wi-Fi](network.md)
-  * [Using QGroundControl via Wi-Fi](gcs_bridge.md)
-  * [Controlling Clever from a smartphone](rc.md)
+  * [Remote shell](ssh.md)
+  * [Wi-Fi Configuration](network.md)
+  * [Using QGroundControl over Wi-Fi](gcs_bridge.md)
+  * [Remote control app](rc.md)
   * [UART settings](uart.md)
   * [Viewing images from cameras](web_video_server.md)
   * [Coordinate systems (frames)](frames.md)
-* Coding
+* Programming
   * [ROS](ros.md)
   * [MAVROS](mavros.md)
   * [Simple OFFBOARD](simple_offboard.md)
-  * [Navigation using ArUco markers](aruco.md)
-  * [Automatic check](selfcheck.md)
-  * [Code examples](snippets.md)
+  * Fiducial markers (ArUco)
+    * [Overview](aruco.md)
+    * [Marker detection](aruco_marker.md)
+    * [Map-based navigation](aruco_map.md)
+  * [Automated self-checks](selfcheck.md)
+  * [Code snippets](snippets.md)
   * [Adjusting the position of the main camera](camera_frame.md)
-  * [Working with the camera](camera.md)
-  * [Working with a LED strip on Raspberry 3](leds.md)
+  * [Computer vision basics](camera.md)
+  * [LED strip](leds.md)
   * [Using rviz and rqt](rviz.md)
-  * [Working with the ultrasonic distance gage](sonar.md)
+  * [Interfacing with a sonar](sonar.md)
+  * [Interfacing with a laser rangefinder](laser.md)
   * [PX4 Simulation](sitl.md)
   * [Software autorun](autolaunch.md)
   * [Controlling the copter from Arduino](arduino.md)
   * [Using an external 3G modem](3g.md)
 * Clever-based projects
   * [Copter spheric guard](shield.md)
+  * [Face recognition system](face_recognition.md)
+  * [Android RC app](android.md)
   * [Copter Hack 2018](copterhack2018.md)
   * [Copter Hack 2017](copterhack2017.md)
 * Supplementary materials
-  * [Contribution to Clever](contributing.md)
+  * [Contribution Guidelines](contributing.md)
   * [Flashing ESCs using BLHeliSuite](esc_firmware.md)
   * [MAVLink](mavlink.md)
   * [PX4 Logs and Topics](flight_logs.md)
+  * [Camera calibration](calibration.md)
+  * [Working with IR sensors on Raspberry Pi 3](ir_sensors.md)
 * Textbook
   * [Theory and Videos](lessons.md)

docs/en/android.md

@@ -0,0 +1,141 @@
+# An Android transmitter
+
+As early as in the frosty January 2018, all owners of Apple mobile devices got a nice Wi-Fi piloting app for iOS. And now, a year later, such an application is available for another operating system. The latest version may be downloaded [**here**](https://vk.com/away.php?to=https%3A%2F%2Fplay.google.com%2Fstore%2Fapps%2Fdetails%3Fid%3Dexpress.copter.cleverrc&cc_key=).
+
+## Introduction
+
+In this article, I will tell you how to write your own or modify an existing transmitter for Android yourself. We will use the popular language *Kotlin*, and we will use *Android Studio* for an IDE. For those who never used it, I recommend reading the following [*materials*](https://www.google.com/search?ei=xQxDXMH0C8OOmgW4mYigDQ&q=%D0%A7%D1%82%D0%BE+%D0%B4%D0%B5%D0%BB%D0%B0%D1%82%D1%8C+%D0%B5%D1%81%D0%BB%D0%B8+%D1%8F+%D0%BD%D0%B5+%D1%83%D0%BC%D0%B5%D1%8E+%D0%BF%D0%B8%D1%81%D0%B0%D1%82%D1%8C+%D0%BF%D0%BE%D0%B4+%D0%B0%D0%BD%D0%B4%D1%80%D0%BE%D0%B8%D0%B4%3F&oq=%D0%A7%D1%82%D0%BE+%D0%B4%D0%B5%D0%BB%D0%B0%D1%82%D1%8C+%D0%B5%D1%81%D0%BB%D0%B8+%D1%8F+%D0%BD%D0%B5+%D1%83%D0%BC%D0%B5%D1%8E+%D0%BF%D0%B8%D1%81%D0%B0%D1%82%D1%8C+%D0%BF%D0%BE%D0%B4+%D0%B0%D0%BD%D0%B4%D1%80%D0%BE%D0%B8%D0%B4%3F&gs_l=psy-ab.3...4413.17423..17726...9.0..2.442.4577.45j5j1j0j1....2..0....1..gws-wiz.....6..0i71j35i39j0i131j0j0i67j0i131i67j0i22i30j33i22i29i30j33i21j33i160.0bZz-WGxoHY). The entire application code can be found [**here**](https://github.com/Tennessium/android). If you want to immediately get an app to further tuning, run the following command:
+
+```Bash
+git clone https://github.com/Tennessium/android
+```
+
+However, to make you fully understand the application, I will tell you about each stage of the project, as if you were building it from scratch.
+
+## Wrapper
+
+Let's start with the simplest thing — the appearance of our application. At [**GitHub**](https://github.com/CopterExpress/clever/tree/master/apps/android/app/src/main/assets), you can find *HTML*, *CSS* and *JavaScript* files, which make up the web page to be used for controlling the copter. To have this page displayed in our application, do the following:
+
+1. Create folder **assets** in the main folder of the app named **app**
+
+2. Add to it all files from [here](https://github.com/CopterExpress/clever/tree/master/apps/android/app/src/main/assets)
+
+If you reached this stage, you already have the web page you want, congratulations! Now we have to display it somehow in the app. To do this, in class *activity* in method **onCreate**, write the following code:
+
+```Kotlin
+main_web.loadUrl("file:///android_asset/index.html")
+```
+
+Where *main_web* is the ID of your *WebView*, which is in the *xml* file of the *activity* selected by you.
+
+Unfortunately, the quadcopter transmitter requires the entire screen of the device, while the interface elements of the system interfere with full-fledged use of the program. For this purpose, at the beginning of method **onCreate**, call the following function:
+
+```Kotlin
+private fun fullScreenCall() {
+	window.setFlags(WindowManager.LayoutParams.FLAG_FULLSCREEN, WindowManager.LayoutParams.FLAG_FULLSCREEN)
+	if (Build.VERSION.SDK_INT < 19) {
+		val v = this.window.decorView
+		v.systemUiVisibility = View.GONE
+	} else {
+		//for higher API versions.
+		val decorView = window.decorView
+		val uiOptions = View.SYSTEM_UI_FLAG_HIDE_NAVIGATION or View.SYSTEM_UI_FLAG_IMMERSIVE_STICKY
+		decorView.systemUiVisibility = uiOptions
+	}
+}
+```
+
+This feature allows getting rid of the system interface elements. Let's go ahead.
+
+This is how the transmitter looks at this stage:
+
+<img src="../assets/IMG_4397.PNG" width="50%">
+
+If you run your application, you will see that the sticks are not functioning. This is due to the fact that *JavaScript* is disabled in our page. To enable it, write the following code:
+
+```Kotlin
+	main_web.settings.apply {
+	domStorageEnabled = true
+	javaScriptEnabled = true
+	loadWithOverviewMode = true
+	useWideViewPort = true
+	setSupportZoom(false)
+}
+```
+
+This piece of code allows the page to use *JavaScript* and at the same time prepares for the next stage - **logics**.
+
+## Receiving data from the web page
+
+To let your phone receive data from the *HTML page*, create a class for interacting with the web interface
+
+```Kotlin
+class WebAppInterface(c: Context) {
+	@JavascriptInterface
+	public fun postMessage(message: String) {
+		val data = JSONObject(message)
+		send("255.255.255.255", 35602, pack(
+		data.getInt("x").toShort(),
+		data.getInt("y").toShort(),
+		data.getInt("z").toShort(),
+		data.getInt("r").toShort()))
+	}
+}
+```
+
+This class will receive messages from the web page sent by the *postMessage* where argument *message* is the message from the page.
+
+Now we have to link classes **WebAppInterface** and **MainActivity**. For this you have to add just one line to method **onCreate**:
+
+```Kotlin
+main_web.addJavascriptInterface(WebAppInterface(this), "appInterface")
+```
+
+## Sending data to the copter
+
+**Important!**
+For working in Internet in the *Android* platform, add the following line to tag *manifest* in file **AndroidManifest.xml**:
+
+```XML
+<uses-permission android:name="android.permission.INTERNET"/>
+```
+
+It will grant your application access to the Internet, and the ability to send data via **Wi-Fi**. And you will now learn how to do that. Let's go ahead.
+
+You have probably noticed function *send* in class **WebAppInterface**. It is this function that sends data to the copter. Let's declare it **outside classes**:
+
+```Kotlin
+fun send(host: String, port: Int, data: ByteArray, senderPort: Int = 0): Boolean {
+	var ret = false
+	var socket: DatagramSocket? = null
+	try {
+		socket = DatagramSocket(senderPort)
+		val address = InetAddress.getByName(host)
+		val packet = DatagramPacket(data, data.size, address, port)
+		socket.send(packet)
+		ret = true
+	} catch (e: Exception) {
+		e.printStackTrace()
+	} finally {
+		socket?.close()
+	}
+	return ret
+}
+```
+
+This function sends data via the [*user datagram protocol*](https://www.google.com/search?q=udp+%D0%BF%D1%80%D0%BE%D1%82%D0%BE%D0%BA%D0%BE%D0%BB&oq=udp+&aqs=chrome.0.69i59j69i57j35i39j0l3.1434j1j7&sourceid=chrome&ie=UTF-8) to the copter. The program sends **bytes**, so it would be a good idea to declare the function for creating an array of **bytes** from four variables:
+
+```Kotlin
+fun pack(x: Short, y: Short, z: Short, r: Short): ByteArray {
+	val pump_on_buf: ByteBuffer = ByteBuffer.allocate(8)
+	pump_on_buf.putShort(r)
+	pump_on_buf.putShort(z)
+	pump_on_buf.putShort(y)
+	pump_on_buf.putShort(x)
+	return pump_on_buf.array().reversedArray()
+}
+```
+
+## Summary
+
+Now your app has the full functionality of its analog for **iOS**. You can customize it as you wish. For any questions about the app, contact us in Telegram @Tenessinum.

docs/en/aruco.md

@@ -1,9 +1,8 @@
-# Navigation using ArUco markers
+# ArUco markers
 
-> **Note** Documentation for the versions [of image](microsd_images.md), starting with **0.15**. For earlier versions, see [documentation for version **0.14**](https://github.com/CopterExpress/clever/blob/v0.14/docs/ru/aruco.md).
+> **Note** The following applies to [image versions](microsd_images.md) **0.16** and up. Older documentation is still available [for version **0.15.1**](https://github.com/CopterExpress/clever/blob/v0.15.1/docs/ru/aruco.md).
 
-[ArUco-Markers](https://docs.opencv.org/3.2.0/d5/dae/tutorial_aruco_detection.html) is a popular technology for positioning
-robotic systems using computer vision.
+[ArUco markers](https://docs.opencv.org/3.2.0/d5/dae/tutorial_aruco_detection.html) are commonly used for vision-based position estimation.
 
 Examples of ArUco markers:
 
@@ -13,159 +12,13 @@ Examples of ArUco markers:
 
 For rapid generation of markers for printing, you may use an online tool: http://chev.me/arucogen/.
 
-## aruco\_pose
+[Clever Raspberry Pi image](microsd_images.md) contains a pre-installed `aruco_pose` ROS package, which can be used for marker detection.
 
-The `aruco_pose` module allows restoring the position of the copter relative to the map of ArUco markers and communicating it to the flight controller using the [Vision Position Estimation](https://dev.px4.io/en/ros/external_position_estimation.html) mechanism.
+## Modes of operation
 
-If the source of the copter position by the markers is available, the option appears for precise autonomous indoor navigation by the positions using the [simple_offboard](simple_offboard.md) module.
+There are several preconfigured modes of operation for ArUco markers on the Clever drone:
 
-### Turning on
+* [single marker detection and navigation](aruco_marker.md);
+* [map-based navigation](aruco_map.md).
 
-Make sure that in the clever launch file \(`~/catkin_ws/src/clever/clever/launch/clever.launch`\), the start of aruco\_pose and [computer vision cameras](camera.md) is turned on:
-
-```xml
-<arg name="main_camera" default="true"/>
-```
-
-```xml
-<arg name="aruco" default="true"/>
-```
-
-After the launch-file is edited, restart package `clever`:
-
-```(bash)
-sudo systemctl restart clever
-```
-
-### Calibrating the ArUco marker map
-
-An automatically generated [ArUco-board](https://docs.opencv.org/trunk/db/da9/tutorial_aruco_board_detection.html) may be used as a map of marks.
-
-The map of marks is adjusted using file `~/catkin_ws/src/clever/clever/launch/aruco.launch`. To use ArUco-board, enter its parameters:
-
-```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"/>
-    <!-- the type of the marker field -->
-    <param name="type" value="gridboard"/>
-
-    <!-- the number of markets along x -->
-    <param name="markers_x" value="1"/>
-
-    <!-- the number of markers along y -->
-    <param name="markers_y" value="6"/>
-
-    <!-- ID of the first marker (left top) -->
-    <param name="first_marker" value="240"/>
-
-    <!-- the length of the marker side in meters -->
-    <param name="markers_side" value="0.3362"/>
-
-    <!-- distance between the murders -->
-    <param name="markers_sep" value="0.46"/>
-</node>
-```
-
-The vertical and horizontal distance between the markers may be set separately:
-
-```xml
-<!-- the horizontal distance between the markers -->
-<param name="markers_sep_x" value="0.97"/>
-
-<!-- the vertical distance between the marker -->
-<param name="markers_sep_y" value="1.435"/>
-```
-
-If a map with a custom order of marker IDs is used, parameter `marker_ids` may be used:
-
-```xml
-<rosparam param="marker_ids">[5, 7, 9, 11, 13, 15]</rosparam>
-```
-
-The markers are numbered from the top left corner of the field.
-
-For monitoring the map that is currently used by the copter for navigation, one can watch the content of topic `aruco_pose/map_image`. In a browser, it may be viewed with [web\_video\_server](web_video_server.md) by following the link [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)
-
-When flying, make sure that the markers glued to the floor correspond to the map.
-
-In topic `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)\) the current result of markers recognitions is available:
-
-TODO
-
-### The system of coordinates
-
-According to [agreement](http://www.ros.org/reps/rep-0103.html), the standard ENU system of coordinates is used in the marker field:
-
-* x — rightward \(conditional East\);
-* y — forward \(conditional North\);
-* z — upward.
-
-_Note_: the definition above is provided for a situation when the marker field is on the floor.
-
-First, the zero is the bottom left point of the marker field. The yaw angle is considered zero when the copter is faced rightward\(along the x-axis\).
-
-![The system of markers coordinates](../assets/aruco-frame.png)
-
-### Configuring the flight controller
-
-Correct Vision Position Estimation requires making sure \(via [QGroundControl](gcs_bridge.md)\) that:
-
-* **For Pixhawk**: Firmware with LPE \(local position estimator\) is installed. For Pixhawk [download firmware `px4fmu-v2_lpe.px4`](https://github.com/PX4/Firmware/releases).
-
-  **For Pixracer**: parameter `SYS_MC_EST_GROUP` should be set to`local_position_estimator, attitude_estimator_q`.
-
-  > **Note** After changing the value of parameter `SYS_MC_EST_GROUP` restart the flight controller.
-* In parameter `LPE_FUSION` **only** flags `vision position`, `land detector` are enabled. The final value _20_.
-* Compass disabled: `ATT_W_MAG` = 0
-* Complimentary filter external heading weight: `ATT_W_EXT_HDG` = 0.5
-* Orientation by yaw by vision enabled: `ATT_EXT_HDG_M` = 2 `MOCAP`.
-* VPE settings: `LPE_VIS_DELAY` = 0 sec, `LPE_VIS_XY` = 0.1 m, `LPE_VIS_Z` = 0.15 m.
-* Recommended settings for land detector:
-  * `COM_DISARM_LAND` = 1 s
-  * `LNDMC_ROT_MAX` = 45 deg
-  * `LNDMC_THR_RANGE` = 0.5
-  * `LNDMC_Z_VEL_MAX` = 1 m/s
-
-<!--
-For the ease of configuring, you may use a ready settings file for [Clever 2](https://github.com/CopterExpress/clever/blob/master/docs/assets/Clever2LPE_160118.params) or for [Clever 3](https://github.com/CopterExpress/clever/blob/master/docs/assets/Clever3_LPE_020218.params) and upload it to the controller using menu Tools - Load from file in tab Parameters in QGroundControl.
-
-![](../assets/Screenshot from 2018-02-27 22-30-50.png)
--->
-
-### Flight
-
-A properly configured copter starts holding position by VPE \(in [modes](modes.md) `POSCTL` or `OFFBOARD`\) automatically.
-
-For [autonomous flights](simple_offboard.md) do you will be able to use functions `navigate`, `set_position`, `set_velocity`. For flying to specific coordinates of the marker field, use frame `aruco_map`:
-
-```python
-# First, the copter has to take off to see the map of marks
-# and for frame aruco_map to appear:
-navigate(0, 0, 2, frame_id='body', speed=0.5, auto_arm=True) #  take off to the altitude of 2 meters
-
-time.sleep(5)
-
-# Flying to coordinate 2:2 of the marker field at the altitude of 2 meters
-navigate(2, 2, 2, speed=1, frame_id='aruco_map')  #  flying to coordinate 2:2 at the altitude of 3 meters
-```
-
-See [other functions](simple_offboard.md) simple_offboard.
-
-### Location of markers on the ceiling
-
-![Markers on the ceiling](../assets/IMG_4175.JPG)
-
-To navigate by the markers placed on the ceiling, you need to set the main camera facing up and [set the corresponding frame of the camera](camera_frame.md).
-
-To set the map of markers in a "turned over" system of coordinates, change parameter `aruco_orientation` in  file `~/catkin_ws/src/clever/clever/aruco.launch`:
-
-```xml
-<param name="aruco_orientation" value="map_upside_down"/>
-```
-
-When this parameter is set, frame aruco\_map will also be "turned over". Thus, to fly at the altitude of 2 meters below the ceiling, argument `z` should be set to 2:
-
-```python
-navigate(x=1, y=2, z=1.1, speed=0.5, frame_id='aruco_map')
+> **Info** Additional documentation for the `aruco_pose` ROS package is available [on GitHub](https://github.com/CopterExpress/clever/blob/master/aruco_pose/README.md).