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

@@ -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
+  depth: 50
-matrix:
+jobs:
   fast_finish: true
   include:
-    - name: "Raspberry Pi Image Build"
+    - stage: Build
+      name: "Raspberry Pi Image Build"
       cache:
         directories:
         - imgcache
@@ -22,7 +23,16 @@ matrix:
 # 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:
@@ -39,7 +49,9 @@ matrix:
         on:
           tags: true
         draft: true
-    - name: "Documentation"
+        name: ${TRAVIS_TAG}
+    - stage: Build
+      name: "Documentation"
       language: node_js
       node_js:
         - "10"
@@ -52,20 +64,29 @@ matrix:
         - markdownlint docs
         - gitbook install
         - gitbook build
-# ***
+      deploy:
-# Disable deployments for now, revisit this later
+        provider: pages
-#   --sfalexrog, 06.02.2019
+        local-dir: _book
-# ***
+        skip-cleanup: true
-#      deploy:
+        github-token: ${GITHUB_OAUTH_TOKEN}
-#        provider: pages
+        keep-history: true
-#        local-dir: _book
+        target-branch: master
-#        skip-cleanup: true
+        repo: CopterExpress/clever-gitbook
-#        github-token: ${GITHUB_OAUTH_TOKEN}
+        fqdn: clever.copterexpress.com
-#        keep-history: true
+        verbose: true
-#        target-branch: gh-pages
+        on:
-#        on:
+          branch: master
-#          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",
+      "role" : "appLauncher",
-      "subtype" : "42mm"
+      "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,11 @@ 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)
+  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/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/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);
@@ -177,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

@@ -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>
@@ -73,6 +74,10 @@ private:
 	visualization_msgs::MarkerArray vis_array_;
 	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()
@@ -95,6 +100,9 @@ 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("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);
@@ -153,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()) {
@@ -183,7 +198,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 +232,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 +243,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
@@ -255,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)) {
+			// Read first character to see whether it's a comment
-				ROS_ERROR("aruco_map: cannot parse line: %s", line.c_str());
+			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");
@@ -338,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/draw.cpp

@@ -6,6 +6,22 @@
 using namespace cv;
 using namespace cv::aruco;
 
+static void _cvProjectPoints2( const CvMat* object_points, const CvMat* rotation_vector,
+                       const CvMat* translation_vector, const CvMat* camera_matrix,
+                       const CvMat* distortion_coeffs, CvMat* image_points,
+                       CvMat* dpdrot CV_DEFAULT(NULL), CvMat* dpdt CV_DEFAULT(NULL),
+                       CvMat* dpdf CV_DEFAULT(NULL), CvMat* dpdc CV_DEFAULT(NULL),
+                       CvMat* dpddist CV_DEFAULT(NULL),
+                       double aspect_ratio CV_DEFAULT(0));
+
+static void _projectPoints( InputArray objectPoints,
+                    InputArray rvec, InputArray tvec,
+                    InputArray cameraMatrix, InputArray distCoeffs,
+                    OutputArray imagePoints,
+                    OutputArray jacobian = noArray(),
+                    double aspectRatio = 0 );
+
+
 void _drawPlanarBoard(Board *_board, Size outSize, OutputArray _img, int marginSize,
                       int borderBits) {
 
@@ -74,24 +90,708 @@ void _drawPlanarBoard(Board *_board, Size outSize, OutputArray _img, int marginS
 
 		dictionary.drawMarker(_board->ids[m], side, marker, borderBits);
 
-		try {
+		// interpolate tiny marker to marker position in markerZone
-			if((outCorners[0].y == outCorners[1].y) && (outCorners[1].x == outCorners[2].x)) {
+		inCorners[0] = Point2f(-0.5f, -0.5f);
-				// marker is aligned to image axes
+		inCorners[1] = Point2f(marker.cols - 0.5f, -0.5f);
-				marker.copyTo(out(Rect(outCorners[0], dst_sz)));
+		inCorners[2] = Point2f(marker.cols - 0.5f, marker.rows - 0.5f);
-				continue;
-			}
 
-			// interpolate tiny marker to marker position in markerZone
+		// remove perspective
-			inCorners[0] = Point2f(-0.5f, -0.5f);
+		Mat transformation = getAffineTransform(inCorners, outCorners);
-			inCorners[1] = Point2f(marker.cols - 0.5f, -0.5f);
+		warpAffine(marker, out, transformation, out.size(), INTER_LINEAR,
-			inCorners[2] = Point2f(marker.cols - 0.5f, marker.rows - 0.5f);
+						BORDER_TRANSPARENT);
-
-			// 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);
-		}
 	}
 }
+
+/* Draw a (potentially partially visible) line. */
+static void linePartial(InputOutputArray image, Point3f p1, Point3f p2, const Scalar& color,
+        int thickness = 1, int lineType = LINE_8, int shift = 0)
+{
+    // If both points are behind the screen, don't draw anything
+    if (p1.z <= 0 && p2.z <= 0)
+    {
+        return;
+    }
+    Point2f p1p{p1.x, p1.y};
+    Point2f p2p{p2.x, p2.y};
+    // If points are on the different sides of the plane, compute intersection point
+    if (p1.z * p2.z < 0)
+    {
+        // Compute intersection point with the screen
+        // We denote alpha as such:
+        // xi = (1 - alpha) * x1 + alpha * x2
+        // yi = (1 - alpha) * y1 + alpha * y2
+        // zi = (1 - alpha) * z1 + alpha * z2 = 0
+        // Thus, alpha can be expressed as
+        // alpha = z1 / (z1 - z2)
+        float alpha = p1.z / (p1.z - p2.z);
+        Point2f pi{(1 - alpha) * p1.x + alpha * p2.x, (1 - alpha) * p1.y + alpha * p2.y};
+        // Now, if z1 is negative, we draw the line from (xi, yi) to (x2, y2), else we draw from (x1, y1) to (xi, yi)
+        if (p1.z < 0)
+        {
+            p1p = pi;
+        }
+        else
+        {
+            p2p = pi;
+        }
+    }
+    line(image, p1p, p2p, color, thickness, lineType, shift);
+}
+
+void _drawAxis(InputOutputArray _image, InputArray _cameraMatrix, InputArray _distCoeffs,
+              InputArray _rvec, InputArray _tvec, float length) {
+
+    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);
+
+    // draw axis lines
+    linePartial(_image, imagePointsZ[0], imagePointsZ[1], Scalar(0, 0, 255), 3);
+    linePartial(_image, imagePointsZ[0], imagePointsZ[2], Scalar(0, 255, 0), 3);
+    linePartial(_image, imagePointsZ[0], imagePointsZ[3], Scalar(255, 0, 0), 3);
+}
+
+static CvMat _cvMat(const cv::Mat& m)
+{
+    CvMat self;
+    CV_DbgAssert(m.dims <= 2);
+    self = cvMat(m.rows, m.dims == 1 ? 1 : m.cols, m.type(), m.data);
+    self.step = (int)m.step[0];
+    self.type = (self.type & ~cv::Mat::CONTINUOUS_FLAG) | (m.flags & cv::Mat::CONTINUOUS_FLAG);
+    return self;
+}
+
+static void _projectPoints( InputArray _opoints,
+                        InputArray _rvec,
+                        InputArray _tvec,
+                        InputArray _cameraMatrix,
+                        InputArray _distCoeffs,
+                        OutputArray _ipoints,
+                        OutputArray _jacobian,
+                        double aspectRatio )
+{
+    Mat opoints = _opoints.getMat();
+    int npoints = opoints.checkVector(3), depth = opoints.depth();
+    CV_Assert(npoints >= 0 && (depth == CV_32F || depth == CV_64F));
+
+    CvMat dpdrot, dpdt, dpdf, dpdc, dpddist;
+    CvMat *pdpdrot=0, *pdpdt=0, *pdpdf=0, *pdpdc=0, *pdpddist=0;
+
+    CV_Assert( _ipoints.needed() );
+
+    _ipoints.create(npoints, 1, CV_MAKETYPE(depth, 3), -1, true);
+    Mat imagePoints = _ipoints.getMat();
+    CvMat c_imagePoints = _cvMat(imagePoints);
+    CvMat c_objectPoints = _cvMat(opoints);
+    Mat cameraMatrix = _cameraMatrix.getMat();
+
+    Mat rvec = _rvec.getMat(), tvec = _tvec.getMat();
+    CvMat c_cameraMatrix = _cvMat(cameraMatrix);
+    CvMat c_rvec = _cvMat(rvec), c_tvec = _cvMat(tvec);
+
+    double dc0buf[5]={0};
+    Mat dc0(5,1,CV_64F,dc0buf);
+    Mat distCoeffs = _distCoeffs.getMat();
+    if( distCoeffs.empty() )
+        distCoeffs = dc0;
+    CvMat c_distCoeffs = _cvMat(distCoeffs);
+    int ndistCoeffs = distCoeffs.rows + distCoeffs.cols - 1;
+
+    Mat jacobian;
+    if( _jacobian.needed() )
+    {
+        _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

@@ -4,3 +4,5 @@
 #include <opencv2/aruco.hpp>
 
 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,53 @@
+#!/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.
+
+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,17 @@
+/*
+ * 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>
 #include <ros/ros.h>
 #include <tf/transform_datatypes.h>
 #include <geometry_msgs/Quaternion.h>
@@ -90,14 +102,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;
+	double yaw, pitch, roll;
-	q.setRPY(0, 0, -tf::getYaw(to) + tf::getYaw(from));
+	tf::Matrix3x3(q).getEulerYPR(yaw, pitch, roll);
-	tf::Quaternion pq;
+	return (abs(pitch) > M_PI / 2) || (abs(roll) > M_PI / 2);
-	tf::quaternionMsgToTF(from, pq);
+}
-	pq = pq * q;
+
-	tf::quaternionTFToMsg(pq, to);
+/* 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,76 +17,76 @@ class TestArucoPose(unittest.TestCase):
 
     def test_markers(self):
         markers = rospy.wait_for_message('aruco_detect/markers', MarkerArray, timeout=5)
-        assert len(markers.markers) == 4
+        self.assertEqual(len(markers.markers), 4)
-        assert markers.header.frame_id == 'main_camera_optical'
+        self.assertEqual(markers.header.frame_id, 'main_camera_optical')
 
-        assert markers.markers[0].id == 2
+        self.assertEqual(markers.markers[0].id, 2)
-        assert markers.markers[0].length == approx(0.33)
+        self.assertAlmostEqual(markers.markers[0].length, 0.33, places=4)
-        assert markers.markers[0].pose.position.x == approx(0.36706567854)
+        self.assertAlmostEqual(markers.markers[0].pose.position.x, 0.36706567854, places=4)
-        assert markers.markers[0].pose.position.y == approx(0.290484516644)
+        self.assertAlmostEqual(markers.markers[0].pose.position.y, 0.290484516644, places=4)
-        assert markers.markers[0].pose.position.z == approx(2.18787602301)
+        self.assertAlmostEqual(markers.markers[0].pose.position.z, 2.18787602301, places=4)
-        assert markers.markers[0].pose.orientation.x == approx(0.993997406299)
+        self.assertAlmostEqual(markers.markers[0].pose.orientation.x, 0.993997406299, places=4)
-        assert markers.markers[0].pose.orientation.y == approx(-0.00532003481626)
+        self.assertAlmostEqual(markers.markers[0].pose.orientation.y, -0.00532003481626, places=4)
-        assert markers.markers[0].pose.orientation.z == approx(-0.107390951553)
+        self.assertAlmostEqual(markers.markers[0].pose.orientation.z, -0.107390951553, places=4)
-        assert markers.markers[0].pose.orientation.w == approx(0.0201999263402)
+        self.assertAlmostEqual(markers.markers[0].pose.orientation.w, 0.0201999263402, places=4)
-        assert markers.markers[0].c1.x == approx(415.557739258)
+        self.assertAlmostEqual(markers.markers[0].c1.x, 415.557739258, places=4)
-        assert markers.markers[0].c1.y == approx(335.557739258)
+        self.assertAlmostEqual(markers.markers[0].c1.y, 335.557739258, places=4)
-        assert markers.markers[0].c2.x == approx(509.442260742)
+        self.assertAlmostEqual(markers.markers[0].c2.x, 509.442260742, places=4)
-        assert markers.markers[0].c2.y == approx(335.557739258)
+        self.assertAlmostEqual(markers.markers[0].c2.y, 335.557739258, places=4)
-        assert markers.markers[0].c3.x == approx(509.442260742)
+        self.assertAlmostEqual(markers.markers[0].c3.x, 509.442260742, places=4)
-        assert markers.markers[0].c3.y == approx(429.442260742)
+        self.assertAlmostEqual(markers.markers[0].c3.y, 429.442260742, places=4)
-        assert markers.markers[0].c4.x == approx(415.557739258)
+        self.assertAlmostEqual(markers.markers[0].c4.x, 415.557739258, places=4)
-        assert markers.markers[0].c4.y == approx(429.442260742)
+        self.assertAlmostEqual(markers.markers[0].c4.y, 429.442260742, places=4)
 
-        assert markers.markers[3].id == 3
+        self.assertEqual(markers.markers[3].id, 3)
-        assert markers.markers[3].length == approx(0.1)
+        self.assertAlmostEqual(markers.markers[3].length, 0.1, places=4)
-        assert markers.markers[3].pose.position.x == approx(-0.1805169666)
+        self.assertAlmostEqual(markers.markers[3].pose.position.x, -0.1805169666, places=4)
-        assert markers.markers[3].pose.position.y == approx(-0.200697302327)
+        self.assertAlmostEqual(markers.markers[3].pose.position.y, -0.200697302327, places=4)
-        assert markers.markers[3].pose.position.z == approx(0.585767514823)
+        self.assertAlmostEqual(markers.markers[3].pose.position.z, 0.585767514823, places=4)
-        assert markers.markers[3].pose.orientation.x == approx(-0.961738074009)
+        self.assertAlmostEqual(markers.markers[3].pose.orientation.x, -0.961738074009, places=4)
-        assert markers.markers[3].pose.orientation.y == approx(-0.0375180244707)
+        self.assertAlmostEqual(markers.markers[3].pose.orientation.y, -0.0375180244707, places=4)
-        assert markers.markers[3].pose.orientation.z == approx(-0.0115387773672)
+        self.assertAlmostEqual(markers.markers[3].pose.orientation.z, -0.0115387773672, places=4)
-        assert markers.markers[3].pose.orientation.w == approx(0.271144115664)
+        self.assertAlmostEqual(markers.markers[3].pose.orientation.w, 0.271144115664, places=4)
-        assert markers.markers[3].c1.x == approx(129.557723999)
+        self.assertAlmostEqual(markers.markers[3].c1.x, 129.557723999, places=4)
-        assert markers.markers[3].c1.y == approx(49.557723999)
+        self.assertAlmostEqual(markers.markers[3].c1.y, 49.557723999, places=4)
-        assert markers.markers[3].c2.x == approx(223.442276001)
+        self.assertAlmostEqual(markers.markers[3].c2.x, 223.442276001, places=4)
-        assert markers.markers[3].c2.y == approx(49.557723999)
+        self.assertAlmostEqual(markers.markers[3].c2.y, 49.557723999, places=4)
-        assert markers.markers[3].c3.x == approx(223.442276001)
+        self.assertAlmostEqual(markers.markers[3].c3.x, 223.442276001, places=4)
-        assert markers.markers[3].c3.y == approx(143.442276001)
+        self.assertAlmostEqual(markers.markers[3].c3.y, 143.442276001, places=4)
-        assert markers.markers[3].c4.x == approx(129.557723999)
+        self.assertAlmostEqual(markers.markers[3].c4.x, 129.557723999, places=4)
-        assert markers.markers[3].c4.y == approx(143.442276001)
+        self.assertAlmostEqual(markers.markers[3].c4.y, 143.442276001, places=4)
 
-        assert markers.markers[1].id == 1
+        self.assertEqual(markers.markers[1].id, 1)
-        assert markers.markers[1].length == approx(0.33)
+        self.assertAlmostEqual(markers.markers[1].length, 0.33, places=4)
-        assert markers.markers[2].id == 4
+        self.assertEqual(markers.markers[2].id, 4)
-        assert markers.markers[2].length == approx(0.33)
+        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)
-        assert len(vis.markers) == 9
+        self.assertEqual(len(vis.markers), 9)
 
     def test_debug(self):
         img = rospy.wait_for_message('aruco_detect/debug', Image, timeout=5)
-        assert img.width == 640
+        self.assertEqual(img.width, 640)
-        assert img.height == 480
+        self.assertEqual(img.height, 480)
-        assert img.header.frame_id == 'main_camera_optical'
+        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'
+        self.assertEqual(pose.header.frame_id, 'main_camera_optical')
-        assert pose.pose.pose.position.x == approx(-0.629167753342)
+        self.assertAlmostEqual(pose.pose.pose.position.x, -0.629167753342, places=4)
-        assert pose.pose.pose.position.y == approx(0.293822650809)
+        self.assertAlmostEqual(pose.pose.pose.position.y, 0.293822650809, places=4)
-        assert pose.pose.pose.position.z == approx(2.12641343155)
+        self.assertAlmostEqual(pose.pose.pose.position.z, 2.12641343155, places=4)
-        assert pose.pose.pose.orientation.x == approx(-0.998383794799)
+        self.assertAlmostEqual(pose.pose.pose.orientation.x, -0.998383794799, places=4)
-        assert pose.pose.pose.orientation.y == approx(-5.20919098575e-06)
+        self.assertAlmostEqual(pose.pose.pose.orientation.y, -5.20919098575e-06, places=4)
-        assert pose.pose.pose.orientation.z == approx(-0.0300861070302)
+        self.assertAlmostEqual(pose.pose.pose.orientation.z, -0.0300861070302, places=4)
-        assert pose.pose.pose.orientation.w == approx(0.0482143590507)
+        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)
-        assert img.width == 2000
+        self.assertEqual(img.width, 2000)
-        assert img.height == 2000
+        self.assertEqual(img.height, 2000)
-        assert img.encoding == 'mono8'
+        self.assertEqual(img.encoding, 'mono8')
 
     def test_map_visualization(self):
         vis = rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5)

aruco_pose/test/basic.txt

@@ -1,5 +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/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/clever_rename

@@ -0,0 +1,35 @@
+#!/usr/bin/env bash
+
+# Set Clever hostname to the specified value
+
+set -e
+
+NEW_NAME_OPT=$1
+
+if [[ -z ${NEW_NAME_OPT} ]]; then
+  echo "Please specify new name for this Clever"
+  exit 1
+fi
+
+NEW_NAME=$(echo ${NEW_NAME_OPT} | tr '[:upper:]' '[:lower:]')
+
+echo "Setting name to ${NEW_NAME}"
+
+echo "Backing up /etc/hostname"
+cp /etc/hostname /etc/hostname.bak
+echo "Writing new /etc/hostname"
+echo ${NEW_NAME} > /etc/hostname
+
+echo "Backing up /etc/hosts"
+cp /etc/hosts /etc/hosts.bak
+echo "Rewriting /etc/hosts with new values"
+sed -i 's/127\.0\.1\.1.*/127.0.1.1\t'${NEW_NAME}'/g' /etc/hosts
+
+echo "Changing hostname in /lib/systemd/system/roscore.env"
+sed -i 's/ROS_HOSTNAME=.*/ROS_HOSTNAME='${NEW_NAME}'.local/g' /lib/systemd/system/roscore.env
+
+echo "Changing hostname in .bashrc"
+sed -i 's/export ROS_HOSTNAME=.*/export ROS_HOSTNAME='${NEW_NAME}'.local/g' /home/pi/.bashrc
+
+echo "Done, reboot your Clever to see the results"
+

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
 
@@ -32,7 +36,9 @@ echo_stamp() {
 }
 
 echo_stamp "Rename SSID"
-sudo sed -i.OLD "s/CLEVER/CLEVER-$(head -c 100 /dev/urandom | xxd -ps -c 100 | sed -e 's/[^0-9]//g' | cut -c 1-4)/g" /etc/wpa_supplicant/wpa_supplicant.conf
+NEW_SSID='CLEVER-'$(head -c 100 /dev/urandom | xxd -ps -c 100 | sed -e "s/[^0-9]//g" | cut -c 1-4)
+sudo sed -i.OLD "s/CLEVER/${NEW_SSID}/" /etc/wpa_supplicant/wpa_supplicant.conf
+clever_rename ${NEW_SSID}
 
 echo_stamp "Harware setup"
 /root/hardware_setup.sh

builder/assets/kinetic-rosdep-clever.yaml

@@ -537,4 +537,16 @@ 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]
+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/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

@@ -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-06-29/2018-06-27-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'}
@@ -108,9 +112,13 @@ ${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/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/'
+# Add rename script
+${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/clever_rename' '/usr/local/bin/clever_rename'
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} exec ${SCRIPTS_DIR}'/image-ros.sh' ${REPO_URL} ${IMAGE_VERSION} false false ${NUMBER_THREADS}
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} exec ${SCRIPTS_DIR}'/image-validate.sh'
 

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 ] && {
+  [ $count -gt $max_count ] && {
-    echo -e "\n${ANSI_RED}The command \"$@\" failed 3 times.${ANSI_RESET}\n" >&2
+    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" \
+echo_stamp "Init rosdep"
-&& rosdep init \
+my_travis_retry rosdep init
-&& echo "yaml file:///etc/ros/rosdep/kinetic-rosdep-clever.yaml" >> /etc/ros/rosdep/sources.list.d/20-default.list \
+echo "yaml file:///etc/ros/rosdep/kinetic-rosdep-clever.yaml" >> /etc/ros/rosdep/sources.list.d/20-default.list
-&& rosdep update
+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>
@@ -78,7 +83,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 +140,12 @@ 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 "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 \
@@ -144,6 +155,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" \
@@ -164,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,14 +88,14 @@ 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 \
 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 \
@@ -99,12 +103,24 @@ 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.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
 # sed -i "s/updates_available//" /home/pi/.byobu/status
@@ -113,6 +129,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 +138,10 @@ pip --version
 pip3 --version
 
 echo_stamp "Install and enable Butterfly (web terminal)"
-my_travis_retry pip3 install --prefer-binary butterfly
+echo_stamp "Workaround for tornado >= 6.0 breaking butterfly"
-my_travis_retry pip3 install --prefer-binary butterfly[systemd]
+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 +158,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
@@ -147,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
 
@@ -44,3 +45,6 @@ rosversion compressed_image_transport
 rosversion rosbridge_suite
 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/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/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

@@ -21,11 +21,12 @@
         <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"/> <!-- poll rate -->
-        <param name="rate" value="100"/>
+        <param name="cv_cap_prop_fps" value="40"/> <!-- camera FPS -->
-        <param name="cv_cap_prop_fps" value="40"/>
+        <param name="capture_delay" value="0.02"/> <!-- approximate delay on frame retrieving -->
-        <param name="capture_delay" value="0.02"/>
+        <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"/>
         <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">
+        <rosparam param="plugin_whitelist">
-            - safety_area
+            - altitude
-            - image_pub
+            - command
-            - vibration
+            - distance_sensor
-            - rangefinder
+            - ftp
-            - 3dr_radio
+            - global_position
-            - actuator_control
+            - imu
-            - hil_controls
+            - local_position
-            - vfr_hud
+            - manual_control
-            - vision_speed_estimate
+            # - mocap_pose_estimate
-            - fake_gps
+            - param
-            - cam_imu_sync
+            - px4flow
-            - hil
+            - rc_io
-            - adsb
+            - setpoint_attitude
-            - waypoint
+            - setpoint_position
-            - obstacle_distance
+            - setpoint_raw
-            - setpoint_accel
+            - setpoint_velocity
-            - trajectory
+            - sys_status
-            - wind_estimation
+            - sys_time
-            - home_position
+            - vision_pose_estimate
+            # - vision_speed_estimate
+            # - waypoint
         </rosparam>
     </node>
 

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,5 +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/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

@@ -1,19 +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 geometry_msgs.msg import PoseStamped, TwistStamped
+from mavros_msgs.srv import ParamGet
+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
@@ -26,42 +42,174 @@ import tf.transformations as t
 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:
                 traceback.print_exc()
-                rospy.logwarn('%s: exception occured', 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
 
 
+param_get = rospy.ServiceProxy('mavros/param/get', ParamGet)
+
+
+def get_param(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:
+        if res.value.integer != 0:
+            return res.value.integer
+        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:
+            info('selected estimator: LPE')
+            fuse = get_param('LPE_FUSION')
+            if fuse & (1 << 4):
+                info('LPE_FUSION: land detector fusion is enabled')
+            else:
+                info('LPE_FUSION: land detector fusion is disabled')
+            if fuse & (1 << 7):
+                info('LPE_FUSION: barometer fusion is enabled')
+            else:
+                info('LPE_FUSION: barometer fusion is disabled')
+
+        elif est == 2:
+            info('selected estimator: EKF2')
+        else:
+            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)
@@ -69,27 +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:
+    if img.width != camera_info.width:
-        failure('%s: calibration width doesn\'t match image width (%d != %d)', name, info.width, img.width)
+        failure('%s: calibration width doesn\'t match image width (%d != %d)', name, camera_info.width, img.width)
-    if img.height != info.height:
+    if img.height != camera_info.height:
-        failure('%s: calibration height doesn\'t match image height (%d != %d))', name, info.height, img.height)
+        failure('%s: calibration height doesn\'t match image height (%d != %d))', name, camera_info.height, img.height)
 
-
-@check('Aruco detector')
-def check_aruco():
     try:
-        rospy.wait_for_message('aruco_pose/debug', Image, timeout=1)
+        optical = Vector3Stamped()
-    except rospy.ROSException:
+        optical.header.frame_id = img.header.frame_id
-        failure('no aruco_pose/debug messages')
+        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('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():
+    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
+
+    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:
@@ -97,7 +316,45 @@ 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')
+    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:
+            info('Vision yaw weight: %.2f', vision_yaw_w)
+        fuse = get_param('LPE_FUSION')
+        if not fuse & (1 << 2):
+            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)
+        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 fusion is disabled, change EKF2_AID_MASK parameter')
+        if not fuse & (1 << 4):
+            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)
+        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:
@@ -168,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))
@@ -195,6 +452,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 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 not numpy.isclose(scale, 1.0):
+                failure('LPE_FLW_SCALE parameter is %.2f, but it should be 1.0', scale)
+
+            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 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)
+            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)')
 
@@ -202,21 +495,46 @@ 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=4)
+        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=4)
+        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):
+            info('"pub agl as lpos down" in LPE_FUSION is disabled, NOT operating over flat surface')
+        else:
+            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:
+            info('EKF2_HGT_MODE != Range sensor, NOT operating over flat surface')
+        else:
+            info('EKF2_HGT_MODE = Range sensor, operating over flat surface')
+        aid = get_param('EKF2_RNG_AID')
+        if aid != 1:
+            info('EKF2_RNG_AID != 1, range sensor aiding disabled')
+        else:
+            info('EKF2_RNG_AID = 1, range sensor aiding enabled')
+
 
 @check('Boot duration')
 def check_boot_duration():
-    proc = Popen('systemd-analyze', stdout=PIPE)
+    output = subprocess.check_output('systemd-analyze')
-    proc.wait()
-    output = proc.communicate()[0]
     r = re.compile(r'([\d\.]+)s$')
     duration = float(r.search(output).groups()[0])
     if duration > 15:
@@ -227,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)
+    output = subprocess.check_output(CMD, shell=True)
-    proc.wait()
-    output = proc.communicate()[0]
     processes = output.split('\n')
     for process in processes:
         if not process:
@@ -241,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);
 		}
 	}
 
@@ -528,12 +553,13 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl
 			nav_speed = speed;
 		}
 
-		if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == POSITION || sp_type == VELOCITY) {
+		// if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == POSITION || sp_type == VELOCITY) {
-			if (std::isnan(yaw) && yaw_rate == 0) {
+		// 	if (std::isnan(yaw) && yaw_rate == 0) {
-				// keep yaw unchanged
+		// 		// keep yaw unchanged
-				yaw = tf2::getYaw(local_position.pose.orientation);
+		//		// 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
@@ -643,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";
 
@@ -681,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));
@@ -708,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);

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({
+var viewer, tfClient;
-	divID: 'viz',
-	width: 1000,
-	height: 600,
-	antialias: true
-});
 
-var tfClient = new ROSLIB.TFClient({
+function setScene(fixedFrame) {
-	ros: ros,
+	viewer = new ROS3D.Viewer({
-	angularThres: 0.01,
+		divID: 'viz',
-	transThres: 0.01,
+		width: 1000,
-	rate: 10.0,
+		height: 600,
-	fixedFrame : 'map'
+		antialias: true
-});
+	});
 
-// vehicle markers
+	tfClient = new ROSLIB.TFClient({
-var vehicleMarkers = new ROS3D.MarkerArrayClient({
+		ros: ros,
-	ros: ros,
+		angularThres: 0.01,
-	tfClient: tfClient,
+		transThres: 0.01,
-	topic: '/vehicle_marker',
+		rate: 10.0,
-	rootObject: viewer.scene
+		fixedFrame : fixedFrame
-});
+	});
 
-// camera markers
+	var map = new ROS3D.Grid({
-var cameraMarkers = new ROS3D.MarkerArrayClient({
+		ros: ros,
-	ros: ros,
+		tfClient: tfClient,
-	tfClient: tfClient,
+		rootObject: viewer.scene
-	topic: '/main_camera/camera_markers',
+	});
-	rootObject: viewer.scene
-});
 
-// detected aruco markers
+	viewer.scene.add(map);
-var cameraMarkers = new ROS3D.MarkerArrayClient({
+}
-	ros: ros,
-	tfClient: tfClient,
-	topic: '/aruco_detect/visualization',
-	rootObject: viewer.scene
-});
 
-var map = new ROS3D.Grid({
+function addAxes() {
-	ros: ros,
+	var axes = new ROS3D.Axes({
-	tfClient: tfClient,
+		ros: ros,
-	// frameID: 'map',
+		tfClient: tfClient,
-	rootObject: viewer.scene
+		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>