Add some simple_offboard tests for testing with SITL

* Rework assigning hostname and service files

* docs: small fix in hostname article

* Correct path to setup.bash in clever.service

* docs: correct ip in hostname article

* init_rpi: put normal and .local hostname on one line in hosts

* docs: add English version of hostname article

* clever.service: use sh instead of bash

* docs: Spellcheck english version, add note about hostname vs SSID

* clever.service: return roscore requirement back

.markdownlint.json

@@ -13,31 +13,59 @@
     "MD040": false,
     "MD044": {
         "names": [
+            "COEX",
+            "Copter Express",
+            "Коптер Экспресс",
+            "Клевер",
             "MAVLink",
             "ROS",
             "ROS Kinetic",
             "OpenCV",
+            "Gazebo",
             "GitHub",
             "FPV",
             "PPM",
             "PWM",
             "Python",
             "C++",
+            "JavaScript",
+            "Node.js",
+            "Django",
+            "Flask",
+            "HTTP",
+            "HTTPS",
+            "WebSocket",
+            "RPC",
             "PX4",
+            "ArduPilot",
+            "jMAVSim",
+            "px4.io",
+            "logs.px4.io",
             "QGroundControl",
             "QGC",
             "WireShark",
             "FlightPlot",
             "OFFBOARD",
+            "ACRO",
+            "RPY",
             "LPE",
             "EKF2",
+            "IMU",
+            "VPE",
             "SITL",
             "PID",
             "Wi-Fi",
             "Raspberry Pi",
             "RPi",
             "Linux",
+            "GNU",
+            "GNU/Linux",
             "Windows",
+            "Docker",
+            "RFC",
+            "Travis",
+            "travis-ci.org",
+            "travis-ci.com",
             "macOS",
             "iOS",
             "Android",
@@ -45,6 +73,7 @@
             "Raspbian",
             "Raspbian Jesse",
             "Raspbian Stretch",
+            "Raspbian Buster",
             "Pixhawk",
             "Pixracer",
             "Arduino",
@@ -53,12 +82,19 @@
             "LIRC",
             "GPIO",
             "HC-SR04",
+            "RCW-0001",
+            "RealSense",
+            "NUC",
+            "NVIDIA",
+            "Jetson",
+            "Jetson Nano",
             "STM",
             "LED",
             "USB",
             "FAT32",
             "uORB",
             "SSH",
+            "PuTTY",
             "API",
             "UART",
             "GND",

.travis.yml

@@ -23,7 +23,16 @@ jobs:
 # Check if there are any cached images, copy them to our "images" directory
         - if [ -n "$(ls -A imgcache/*.zip)" ]; then mkdir -p images && cp imgcache/*.zip images; fi
       script:
-        - docker run --privileged --rm -v /dev:/dev -v $(pwd):/builder/repo -e TRAVIS_TAG="${TRAVIS_TAG}" ${DOCKER}
+        - if [[ -z ${TRAVIS_TAG} && "${TRAVIS_PULL_REQUEST}" != "false" ]]; then
+            echo "Commit range is ${TRAVIS_COMMIT_RANGE}" &&
+            if [ $(git diff --name-only ${TRAVIS_COMMIT_RANGE} | grep -v ^"docs/" | wc -l) -eq 0 ]; then
+              echo " === Docs-only change; skipping build ===" &&
+              export SKIP_BUILD=true;
+            fi;
+          fi
+        - if [ -z ${SKIP_BUILD} ]; then
+            docker run --privileged --rm -v /dev:/dev -v $(pwd):/builder/repo -e TRAVIS_TAG="${TRAVIS_TAG}" ${DOCKER};
+          fi
       before_cache:
         - cp images/*.zip imgcache
       before_deploy:
@@ -53,21 +62,21 @@ jobs:
         - markdownlint -V
       script:
         - markdownlint docs
+        - ./check_files_size.py
         - gitbook install
         - gitbook build
-# ***
-# Disable deployments for now, revisit this later
-#   --sfalexrog, 06.02.2019
-# ***
-#      deploy:
-#        provider: pages
-#        local-dir: _book
-#        skip-cleanup: true
-#        github-token: ${GITHUB_OAUTH_TOKEN}
-#        keep-history: true
-#        target-branch: gh-pages
-#        on:
-#          branch: WIP/gitbook-autobuild
+      deploy:
+        provider: pages
+        local-dir: _book
+        skip-cleanup: true
+        github-token: ${GITHUB_OAUTH_TOKEN}
+        keep-history: true
+        target-branch: master
+        repo: CopterExpress/clever-gitbook
+        fqdn: clever.copterexpress.com
+        verbose: true
+        on:
+          branch: master
     - stage: Annotate
       name: Auto-generate changelog
       language: python
@@ -76,6 +85,14 @@ jobs:
         - pip install GitPython PyGithub
       script:
         - PYTHONUNBUFFERED=1 python ./gen_changelog.py
+    - stage: Build
+      name: Editorconfig-lint
+      language: generic
+      before_script:
+        - wget https://github.com/okalachev/editorconfig-checker/releases/download/1.2.1-disable-spaces-amount/ec-linux-amd64
+        - chmod +x ec-linux-amd64
+      script:
+        - ./ec-linux-amd64 -spaces-after-tabs -e "roslib.js|ros3d.js|eventemitter2.js|draw.cpp|BinUtils.swift|\.idea|apps/android/app|Assets.xcassets|test_parser_pass.txt|test_node_failure.txt"
 stages:
   - Build
   - Annotate

README.md

@@ -10,7 +10,7 @@ Copter Express has implemented a large number of different autonomous drone proj
 
 Use it to learn how to assemble, configure, pilot and program autonomous CLEVER drone.
 
-## Preconfigured RPi 3 image
+## Raspberry Pi image
 
 **Preconfigured image for Raspberry Pi 3 with installed and configured software, ready to fly, is available [in the Releases section](https://github.com/CopterExpress/clever/releases).**
 
@@ -30,49 +30,70 @@ API description (in Russian) for autonomous flights is available [on GitBook](ht
 
 ## Manual installation
 
-Install ROS Kinetic according to the [documentation](http://wiki.ros.org/kinetic/Installation).
+Install ROS Kinetic according to the [documentation](http://wiki.ros.org/kinetic/Installation), then [create a Catkin workspace](http://wiki.ros.org/catkin/Tutorials/create_a_workspace).
 
-Clone repo to directory `/home/pi/catkin_ws/src/clever`:
+Clone this repo to directory `~/catkin_ws/src/clever`:
 
 ```bash
 cd ~/catkin_ws/src
 git clone https://github.com/CopterExpress/clever.git clever
 ```
 
-Build ROS packages:
+All the required ROS packages (including `mavros` and `opencv`) can be installed using `rosdep`:
+
+```bash
+cd ~/catkin_ws/
+rosdep install -y --from-paths src --ignore-src
+```
+
+Build ROS packages (on memory constrained platforms you might be going to need to use `-j1` key):
 
 ```bash
 cd ~/catkin_ws
 catkin_make -j1
 ```
 
-Enable systemd service `roscore` (if not enabled):
+To complete `mavros` install you'll need to install `geographiclib` datasets:
 
 ```bash
-sudo systemctl enable /home/pi/catkin_ws/src/clever/deploy/roscore.service
+curl https://raw.githubusercontent.com/mavlink/mavros/master/mavros/scripts/install_geographiclib_datasets.sh | sudo bash
+```
+
+You may optionally install udev rules to provide `/dev/px4fmu` symlink to your PX4-based flight controller connected over USB. Copy `99-px4fmu.rules` to your `/lib/udev/rules.d` folder:
+
+```bash
+cd ~/catkin_ws/src/clever/clever/config
+sudo cp 99-px4fmu.rules /lib/udev/rules.d
+```
+
+Alternatively you may change the `fcu_url` property in `mavros.launch` file to point to your flight controller device.
+
+## Running
+
+Enable systemd service `roscore` (if not running):
+
+```bash
+sudo systemctl enable /home/<username>/catkin_ws/src/clever/deploy/roscore.service
 sudo systemctl start roscore
 ```
 
-Enable systemd service `clever`:
+To start connection to SITL, use:
 
 ```bash
-sudo systemctl enable /home/pi/catkin_ws/src/clever/deploy/clever.service
-sudo systemctl start clever
+roslaunch clever sitl.launch
 ```
 
-### Dependencies
+To start connection to the flight controller, use:
 
-[ROS Kinetic](http://wiki.ros.org/kinetic).
+```bash
+roslaunch clever clever.launch
+```
 
-Necessary ROS packages:
+> Note that the package is configured to connect to `/dev/px4fmu` by default (see [previous section](#manual-installation)). Install udev rules or specify path to your FCU device in `mavros.launch`.
 
-* `opencv3`
-* `mavros`
-* `rosbridge_suite`
-* `web_video_server`
-* `cv_camera`
-* `nodelet`
-* `dynamic_reconfigure`
-* `bondcpp`, branch `master`
-* `roslint`
-* `rosserial`
+Also, you can enable and start the systemd service:
+
+```bash
+sudo systemctl enable /home/<username>/catkin_ws/src/clever/deploy/clever.service
+sudo systemctl start clever
+```

apps/android/app/src/main/assets/main.js

@@ -23,19 +23,18 @@ function throttle(func, ms) {
 }
 
 function postAppMessage(msg) {
-    if (window.webkit != undefined) {
-        if (window.webkit.messageHandlers.appInterface != undefined) {
-            window.webkit.messageHandlers.appInterface.postMessage(JSON.stringify(msg));
-        }
-    }
-    else if (window.appInterface != undefined) {
-        window.appInterface.postMessage(JSON.stringify(msg));
-    }
+	if (window.webkit != undefined) {
+		if (window.webkit.messageHandlers.appInterface != undefined) {
+			window.webkit.messageHandlers.appInterface.postMessage(JSON.stringify(msg));
+		}
+	} else if (window.appInterface != undefined) {
+		window.appInterface.postMessage(JSON.stringify(msg));
+	}
 }
 
 function callNativeApp(name, msg) {
 	try {
-        postAppMessage(msg);
+		postAppMessage(msg);
 		return true;
 	} catch(err) {
 		console.warn('The native context does not exist yet');
@@ -109,12 +108,12 @@ function stickTouchStart(e) {
 
 function stickTouchMove(e) {
 	for (touch of e.changedTouches) {
-            onStickTouchMove(touch);
-        }
-    	//onStickTouchMove(e.changedTouches[0]);
-    	rcPublishThrottled();
-    	e.stopPropagation();
-    	e.preventDefault();
+		onStickTouchMove(touch);
+	}
+	//onStickTouchMove(e.changedTouches[0]);
+	rcPublishThrottled();
+	e.stopPropagation();
+	e.preventDefault();
 }
 
 function stickTouchEnd(e) {
@@ -136,4 +135,4 @@ stickRight.addEventListener('touchmove', stickTouchMove);
 stickLeft.addEventListener('touchstart', stickTouchStart);
 stickRight.addEventListener('touchstart', stickTouchStart);
 stickLeft.addEventListener('touchend', stickTouchEnd);
-stickRight.addEventListener('touchend', stickTouchEnd);
+stickRight.addEventListener('touchend', stickTouchEnd);

aruco_pose/CMakeLists.txt

@@ -173,7 +173,7 @@ target_link_libraries(aruco_pose
 # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
 
 ## Mark executable scripts (Python etc.) for installation
- ## in contrast to setup.py, you can choose the destination
+## in contrast to setup.py, you can choose the destination
 # install(PROGRAMS
 #   scripts/my_python_script
 #   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
@@ -216,4 +216,8 @@ target_link_libraries(aruco_pose
 if (CATKIN_ENABLE_TESTING)
   find_package(rostest REQUIRED)
   add_rostest(test/basic.test)
+  add_rostest(test/test_parser_pass.test)
+  add_rostest(test/test_parser_empty_map.test)
+  add_rostest(test/test_node_failure.test)
+  add_rostest(test/largemap.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:
 
@@ -109,7 +110,7 @@ See examples in [`map`](map/) directory.
 Command for running tests:
 
 ```bash
-rostest aruco_pose basic.test
+catkin_make run_tests && catkin_test_results
 ```
 
 ## Copyright

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/package.xml

@@ -30,6 +30,7 @@
   <depend>rostest</depend>
 
   <test_depend>image_publisher</test_depend>
+  <test_depend>ros_pytest</test_depend>
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>

aruco_pose/src/aruco_detect.cpp

@@ -185,9 +185,13 @@ private:
 					// TODO: check IDs are unique
 					if (send_tf_) {
 						transform.child_frame_id = getChildFrameId(ids[i]);
-						transform.transform.rotation = marker.pose.orientation;
-						fillTranslation(transform.transform.translation, tvecs[i]);
-						br_.sendTransform(transform);
+
+						// check if such static transform exists
+						if (!tf_buffer_.canTransform(transform.header.frame_id, transform.child_frame_id, transform.header.stamp)) {
+							transform.transform.rotation = marker.pose.orientation;
+							fillTranslation(transform.transform.translation, tvecs[i]);
+							br_.sendTransform(transform);
+						}
 					}
 				}
 				array_.markers.push_back(marker);

aruco_pose/src/aruco_map.cpp

@@ -18,6 +18,7 @@
 #include <string>
 #include <vector>
 #include <fstream>
+#include <algorithm>
 #include <ros/ros.h>
 #include <nodelet/nodelet.h>
 #include <pluginlib/class_list_macros.h>
@@ -27,6 +28,7 @@
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
 #include <tf2_ros/transform_broadcaster.h>
+#include <tf2_ros/static_transform_broadcaster.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 #include <message_filters/subscriber.h>
 #include <message_filters/synchronizer.h>
@@ -67,13 +69,15 @@ private:
 	Mat camera_matrix_, dist_coeffs_;
 	geometry_msgs::TransformStamped transform_;
 	geometry_msgs::PoseWithCovarianceStamped pose_;
+	vector<geometry_msgs::TransformStamped> markers_transforms_;
 	tf2_ros::TransformBroadcaster br_;
+	tf2_ros::StaticTransformBroadcaster static_br_;
 	tf2_ros::Buffer tf_buffer_;
 	tf2_ros::TransformListener tf_listener_{tf_buffer_};
 	visualization_msgs::MarkerArray vis_array_;
-	std::string known_tilt_;
+	std::string known_tilt_, map_, markers_frame_, markers_parent_frame_;
 	int image_width_, image_height_, image_margin_;
-	bool auto_flip_;
+	bool auto_flip_, image_axis_;
 
 public:
 	virtual void onInit()
@@ -100,6 +104,9 @@ public:
 		nh_priv_.param("image_width", image_width_, 2000);
 		nh_priv_.param("image_height", image_height_, 2000);
 		nh_priv_.param("image_margin", image_margin_, 200);
+		nh_priv_.param("image_axis", image_axis_, true);
+		nh_priv_.param<std::string>("markers/frame_id", markers_parent_frame_, transform_.child_frame_id);
+		nh_priv_.param<std::string>("markers/child_frame_id_prefix", markers_frame_, "");
 
 		// createStripLine();
 
@@ -124,6 +131,7 @@ public:
 		sync_.reset(new message_filters::Synchronizer<SyncPolicy>(SyncPolicy(10), image_sub_, info_sub_, markers_sub_));
 		sync_->registerCallback(boost::bind(&ArucoMap::callback, this, _1, _2, _3));
 
+		publishMarkersFrames();
 		publishMapImage();
 		vis_markers_pub_.publish(vis_array_);
 
@@ -270,10 +278,50 @@ publish_debug:
 
 			std::istringstream s(line);
 
-			if (!(s >> id >> length >> x >> y >> z >> yaw >> pitch >> roll)) {
-				ROS_ERROR("aruco_map: cannot parse line: %s", line.c_str());
+			// Read first character to see whether it's a comment
+			char first = 0;
+			if (!(s >> first)) {
+				// No non-whitespace characters, must be a blank line
 				continue;
 			}
+
+			if (first == '#') {
+				ROS_DEBUG("aruco_map: Skipping line as a comment: %s", line.c_str());
+				continue;
+			} else if (isdigit(first)) {
+				// Put the digit back into the stream
+				// Note that this is a non-modifying putback, so this should work with istreams
+				// (see https://en.cppreference.com/w/cpp/io/basic_istream/putback)
+				s.putback(first);
+			} else {
+				// Probably garbage data; inform user and throw an exception, possibly killing nodelet
+				ROS_FATAL("aruco_map: Malformed input: %s", line.c_str());
+				ros::shutdown();
+				throw std::runtime_error("Malformed input");
+			}
+
+			if (!(s >> id >> length >> x >> y)) {
+				ROS_ERROR("aruco_map: Not enough data in line: %s; "
+				          "Each marker must have at least id, length, x, y fields", line.c_str());
+				continue;
+			}
+			// Be less strict about z, yaw, pitch roll
+			if (!(s >> z)) {
+				ROS_DEBUG("aruco_map: No z coordinate provided for marker %d, assuming 0", id);
+				z = 0;
+			}
+			if (!(s >> yaw)) {
+				ROS_DEBUG("aruco_map: No yaw provided for marker %d, assuming 0", id);
+				yaw = 0;
+			}
+			if (!(s >> pitch)) {
+				ROS_DEBUG("aruco_map: No pitch provided for marker %d, assuming 0", id);
+				pitch = 0;
+			}
+			if (!(s >> roll)) {
+				ROS_DEBUG("aruco_map: No roll provided for marker %d, assuming 0", id);
+				roll = 0;
+			}
 			addMarker(id, length, x, y, z, yaw, pitch, roll);
 		}
 
@@ -339,6 +387,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);
@@ -368,6 +429,15 @@ publish_debug:
 		board_->ids.push_back(id);
 		board_->objPoints.push_back(obj_points);
 
+		// Add marker's static transform
+		if (!markers_frame_.empty()) {
+			geometry_msgs::TransformStamped marker_transform;
+			marker_transform.header.frame_id = markers_parent_frame_;
+			marker_transform.child_frame_id = markers_frame_ + std::to_string(id);
+			tf::transformTFToMsg(transform, marker_transform.transform);
+			markers_transforms_.push_back(marker_transform);
+		}
+
 		// Add visualization marker
 		visualization_msgs::Marker marker;
 		marker.header.frame_id = transform_.child_frame_id;
@@ -398,6 +468,13 @@ publish_debug:
 		// vis_array_.markers.at(0).points.push_back(p);
 	}
 
+	void publishMarkersFrames()
+	{
+		if (!markers_transforms_.empty()) {
+			static_br_.sendTransform(markers_transforms_);
+		}
+	}
+
 	void publishMapImage()
 	{
 		cv::Size size(image_width_, image_height_);
@@ -405,14 +482,15 @@ publish_debug:
 		cv_bridge::CvImage msg;
 
 		if (!board_->ids.empty()) {
-			_drawPlanarBoard(board_, size, image, image_margin_, 1);
+			_drawPlanarBoard(board_, size, image, image_margin_, 1, image_axis_);
+			msg.encoding = image_axis_ ? sensor_msgs::image_encodings::RGB8 : sensor_msgs::image_encodings::MONO8;
 		} else {
 			// empty map
 			image.create(size, CV_8UC1);
 			image.setTo(cv::Scalar::all(255));
+			msg.encoding = sensor_msgs::image_encodings::MONO8;
 		}
 
-		msg.encoding = sensor_msgs::image_encodings::MONO8;
 		msg.image = image;
 		img_pub_.publish(msg.toImageMsg());
 	}

aruco_pose/src/draw.cpp

@@ -23,12 +23,12 @@ static void _projectPoints( InputArray objectPoints,
 
 
 void _drawPlanarBoard(Board *_board, Size outSize, OutputArray _img, int marginSize,
-                      int borderBits) {
+                      int borderBits, bool drawAxis) {
 
 	CV_Assert(outSize.area() > 0);
 	CV_Assert(marginSize >= 0);
 
-	_img.create(outSize, CV_8UC1);
+	_img.create(outSize, drawAxis ? CV_8UC3 : CV_8UC1);
 	Mat out = _img.getMat();
 	out.setTo(Scalar::all(255));
 	out.adjustROI(-marginSize, -marginSize, -marginSize, -marginSize);
@@ -87,8 +87,12 @@ void _drawPlanarBoard(Board *_board, Size outSize, OutputArray _img, int marginS
 		// dst_sz.width = dst_sz.height = std::min(dst_sz.width, dst_sz.height); //marker should be square
 		double diag = std::round(std::hypot(dst_sz.width, dst_sz.height));
 		int side = std::round(diag / std::sqrt(2));
+		side = std::max(side, 10);
 
 		dictionary.drawMarker(_board->ids[m], side, marker, borderBits);
+		if (drawAxis) {
+			cvtColor(marker, marker, COLOR_GRAY2RGB);
+		}
 
 		// interpolate tiny marker to marker position in markerZone
 		inCorners[0] = Point2f(-0.5f, -0.5f);
@@ -100,74 +104,81 @@ void _drawPlanarBoard(Board *_board, Size outSize, OutputArray _img, int marginS
 		warpAffine(marker, out, transformation, out.size(), INTER_LINEAR,
 						BORDER_TRANSPARENT);
 	}
+
+	// draw axis
+	if (drawAxis) {
+		Size wholeSize; Point ofs;
+		out.locateROI(wholeSize, ofs);
+		auto out_copy = _img.getMat();
+
+		cv::Point center(ofs.x - minX / sizeX * float(out.cols), ofs.y + out.rows + minY / sizeY * float(out.rows));
+		line(out_copy, center, center + Point(300, 0), Scalar(255, 0, 0), 10); // x axis
+		line(out_copy, center, center + Point(0, -300), Scalar(0, 255, 0), 10); // y axis
+		line(out_copy, center, center + Point(-200, 200), Scalar(0, 0, 255), 10); // z axis
+	}
 }
 
 /* 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)
+                        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);
+	// 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);
+	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);
+	// 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);
+	// 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;
+	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,
@@ -179,47 +190,47 @@ static void _projectPoints( InputArray _opoints,
                         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));
+	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;
+	CvMat dpdrot, dpdt, dpdf, dpdc, dpddist;
+	CvMat *pdpdrot = 0, *pdpdt = 0, *pdpdf = 0, *pdpdc = 0, *pdpddist = 0;
 
-    CV_Assert( _ipoints.needed() );
+	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();
+	_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);
+	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;
+	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)));
-    }
+	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 );
+	_cvProjectPoints2(&c_objectPoints, &c_rvec, &c_tvec, &c_cameraMatrix, &c_distCoeffs,
+					  &c_imagePoints, pdpdrot, pdpdt, pdpdf, pdpdc, pdpddist, aspectRatio);
 }
 
 namespace _detail

aruco_pose/src/draw.h

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

aruco_pose/src/genmap.py

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

aruco_pose/src/utils.h

@@ -1,3 +1,14 @@
+/*
+ * Utility functions
+ * Copyright (C) 2018 Copter Express Technologies
+ *
+ * Author: Oleg Kalachev <okalachev@gmail.com>
+ *
+ * Distributed under MIT License (available at https://opensource.org/licenses/MIT).
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ */
+
 #pragma once
 
 #include <cmath>
@@ -19,8 +30,7 @@ static void param(ros::NodeHandle nh, const std::string& param_name, T& param_va
 	}
 }
 
-static void parseCameraInfo(const sensor_msgs::CameraInfoConstPtr& cinfo,
-                     cv::Mat& matrix, cv::Mat& dist)
+static void parseCameraInfo(const sensor_msgs::CameraInfoConstPtr& cinfo, cv::Mat& matrix, cv::Mat& dist)
 {
 	for (unsigned int i = 0; i < 3; ++i)
 		for (unsigned int j = 0; j < 3; ++j)

aruco_pose/test/basic.py

@@ -1,101 +1,149 @@
-#!/usr/bin/env python
-import sys
-import unittest
-import json
 import rospy
-import rostest
+import pytest
 
+import tf2_ros
+import tf2_geometry_msgs
 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 TestArucoPose(unittest.TestCase):
-    def setUp(self):
-        rospy.init_node('test_aruco_detect', anonymous=True)
+@pytest.fixture
+def node():
+    return rospy.init_node('aruco_pose_test', anonymous=True)
 
-    def test_markers(self):
-        markers = rospy.wait_for_message('aruco_detect/markers', MarkerArray, timeout=5)
-        self.assertEqual(len(markers.markers), 4)
-        self.assertEqual(markers.header.frame_id, 'main_camera_optical')
+@pytest.fixture
+def tf_buffer():
+    buf = tf2_ros.Buffer()
+    tf2_ros.TransformListener(buf)
+    return buf
 
-        self.assertEqual(markers.markers[0].id, 2)
-        self.assertAlmostEqual(markers.markers[0].length, 0.33, places=4)
-        self.assertAlmostEqual(markers.markers[0].pose.position.x, 0.36706567854, places=4)
-        self.assertAlmostEqual(markers.markers[0].pose.position.y, 0.290484516644, places=4)
-        self.assertAlmostEqual(markers.markers[0].pose.position.z, 2.18787602301, places=4)
-        self.assertAlmostEqual(markers.markers[0].pose.orientation.x, 0.993997406299, places=4)
-        self.assertAlmostEqual(markers.markers[0].pose.orientation.y, -0.00532003481626, places=4)
-        self.assertAlmostEqual(markers.markers[0].pose.orientation.z, -0.107390951553, places=4)
-        self.assertAlmostEqual(markers.markers[0].pose.orientation.w, 0.0201999263402, places=4)
-        self.assertAlmostEqual(markers.markers[0].c1.x, 415.557739258, places=4)
-        self.assertAlmostEqual(markers.markers[0].c1.y, 335.557739258, places=4)
-        self.assertAlmostEqual(markers.markers[0].c2.x, 509.442260742, places=4)
-        self.assertAlmostEqual(markers.markers[0].c2.y, 335.557739258, places=4)
-        self.assertAlmostEqual(markers.markers[0].c3.x, 509.442260742, places=4)
-        self.assertAlmostEqual(markers.markers[0].c3.y, 429.442260742, places=4)
-        self.assertAlmostEqual(markers.markers[0].c4.x, 415.557739258, places=4)
-        self.assertAlmostEqual(markers.markers[0].c4.y, 429.442260742, places=4)
+def approx(expected):
+    return pytest.approx(expected, abs=1e-4) # compare floats more roughly
 
-        self.assertEqual(markers.markers[3].id, 3)
-        self.assertAlmostEqual(markers.markers[3].length, 0.1, places=4)
-        self.assertAlmostEqual(markers.markers[3].pose.position.x, -0.1805169666, places=4)
-        self.assertAlmostEqual(markers.markers[3].pose.position.y, -0.200697302327, places=4)
-        self.assertAlmostEqual(markers.markers[3].pose.position.z, 0.585767514823, places=4)
-        self.assertAlmostEqual(markers.markers[3].pose.orientation.x, -0.961738074009, places=4)
-        self.assertAlmostEqual(markers.markers[3].pose.orientation.y, -0.0375180244707, places=4)
-        self.assertAlmostEqual(markers.markers[3].pose.orientation.z, -0.0115387773672, places=4)
-        self.assertAlmostEqual(markers.markers[3].pose.orientation.w, 0.271144115664, places=4)
-        self.assertAlmostEqual(markers.markers[3].c1.x, 129.557723999, places=4)
-        self.assertAlmostEqual(markers.markers[3].c1.y, 49.557723999, places=4)
-        self.assertAlmostEqual(markers.markers[3].c2.x, 223.442276001, places=4)
-        self.assertAlmostEqual(markers.markers[3].c2.y, 49.557723999, places=4)
-        self.assertAlmostEqual(markers.markers[3].c3.x, 223.442276001, places=4)
-        self.assertAlmostEqual(markers.markers[3].c3.y, 143.442276001, places=4)
-        self.assertAlmostEqual(markers.markers[3].c4.x, 129.557723999, places=4)
-        self.assertAlmostEqual(markers.markers[3].c4.y, 143.442276001, places=4)
+def test_markers(node):
+    markers = rospy.wait_for_message('aruco_detect/markers', MarkerArray, timeout=5)
+    assert len(markers.markers) == 5
+    assert markers.header.frame_id == 'main_camera_optical'
 
-        self.assertEqual(markers.markers[1].id, 1)
-        self.assertAlmostEqual(markers.markers[1].length, 0.33, places=4)
-        self.assertEqual(markers.markers[2].id, 4)
-        self.assertAlmostEqual(markers.markers[2].length, 0.33, places=4)
+    assert markers.markers[0].id == 2
+    assert markers.markers[0].length == approx(0.33)
+    assert markers.markers[0].pose.position.x == approx(0.36706567854)
+    assert markers.markers[0].pose.position.y == approx(0.290484516644)
+    assert markers.markers[0].pose.position.z == approx(2.18787602301)
+    assert markers.markers[0].pose.orientation.x == approx(0.993997406299)
+    assert markers.markers[0].pose.orientation.y == approx(-0.00532003481626)
+    assert markers.markers[0].pose.orientation.z == approx(-0.107390951553)
+    assert markers.markers[0].pose.orientation.w == approx(0.0201999263402)
+    assert markers.markers[0].c1.x == approx(415.557739258)
+    assert markers.markers[0].c1.y == approx(335.557739258)
+    assert markers.markers[0].c2.x == approx(509.442260742)
+    assert markers.markers[0].c2.y == approx(335.557739258)
+    assert markers.markers[0].c3.x == approx(509.442260742)
+    assert markers.markers[0].c3.y == approx(429.442260742)
+    assert markers.markers[0].c4.x == approx(415.557739258)
+    assert markers.markers[0].c4.y == approx(429.442260742)
 
-    def test_visualization(self):
-        vis = rospy.wait_for_message('aruco_detect/visualization', VisMarkerArray, timeout=5)
-        self.assertEqual(len(vis.markers), 9)
+    assert markers.markers[4].id == 3
+    assert markers.markers[4].length == approx(0.1)
+    assert markers.markers[4].pose.position.x == approx(-0.1805169666)
+    assert markers.markers[4].pose.position.y == approx(-0.200697302327)
+    assert markers.markers[4].pose.position.z == approx(0.585767514823)
+    assert markers.markers[4].pose.orientation.x == approx(-0.961738074009)
+    assert markers.markers[4].pose.orientation.y == approx(-0.0375180244707)
+    assert markers.markers[4].pose.orientation.z == approx(-0.0115387773672)
+    assert markers.markers[4].pose.orientation.w == approx(0.271144115664)
+    assert markers.markers[4].c1.x == approx(129.557723999)
+    assert markers.markers[4].c1.y == approx(49.557723999)
+    assert markers.markers[4].c2.x == approx(223.442276001)
+    assert markers.markers[4].c2.y == approx(49.557723999)
+    assert markers.markers[4].c3.x == approx(223.442276001)
+    assert markers.markers[4].c3.y == approx(143.442276001)
+    assert markers.markers[4].c4.x == approx(129.557723999)
+    assert markers.markers[4].c4.y == approx(143.442276001)
 
-    def test_debug(self):
-        img = rospy.wait_for_message('aruco_detect/debug', Image, timeout=5)
-        self.assertEqual(img.width, 640)
-        self.assertEqual(img.height, 480)
-        self.assertEqual(img.header.frame_id, 'main_camera_optical')
+    assert markers.markers[1].id == 1
+    assert markers.markers[1].length == approx(0.33)
+    assert markers.markers[3].id == 4
+    assert markers.markers[3].length == approx(0.33)
 
-    def test_map(self):
-        pose = rospy.wait_for_message('aruco_map/pose', PoseWithCovarianceStamped, timeout=5)
-        self.assertEqual(pose.header.frame_id, 'main_camera_optical')
-        self.assertAlmostEqual(pose.pose.pose.position.x, -0.629167753342, places=4)
-        self.assertAlmostEqual(pose.pose.pose.position.y, 0.293822650809, places=4)
-        self.assertAlmostEqual(pose.pose.pose.position.z, 2.12641343155, places=4)
-        self.assertAlmostEqual(pose.pose.pose.orientation.x, -0.998383794799, places=4)
-        self.assertAlmostEqual(pose.pose.pose.orientation.y, -5.20919098575e-06, places=4)
-        self.assertAlmostEqual(pose.pose.pose.orientation.z, -0.0300861070302, places=4)
-        self.assertAlmostEqual(pose.pose.pose.orientation.w, 0.0482143590507, places=4)
+    assert markers.markers[2].id == 100
+    assert markers.markers[2].length == approx(0.33)
+    assert markers.markers[2].pose.position.x == approx(-1.37600105389)
+    assert markers.markers[2].pose.position.y == approx(-0.323028530991)
+    assert markers.markers[2].pose.position.z == approx(2.94611272668)
+    assert markers.markers[2].pose.orientation.x == approx(-0.955543925678)
+    assert markers.markers[2].pose.orientation.y == approx(0.0458801909197)
+    assert markers.markers[2].pose.orientation.z == approx(-0.249604946264)
+    assert markers.markers[2].pose.orientation.w == approx(-0.150093920537)
+    assert markers.markers[2].c1.x == approx(52.557723999)
+    assert markers.markers[2].c1.y == approx(205.557723999)
+    assert markers.markers[2].c2.x == approx(113.442276001)
+    assert markers.markers[2].c2.y == approx(205.557723999)
+    assert markers.markers[2].c3.x == approx(113.442276001)
+    assert markers.markers[2].c3.y == approx(265.442260742)
+    assert markers.markers[2].c4.x == approx(52.557723999)
+    assert markers.markers[2].c4.y == approx(265.442260742)
 
-    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_markers_frames(node, tf_buffer):
+    marker_2 = tf_buffer.lookup_transform('main_camera_optical', 'aruco_2', rospy.Time(), rospy.Duration(5))
+    assert marker_2.transform.translation.x == approx(0.36706567854)
+    assert marker_2.transform.translation.y == approx(0.290484516644)
+    assert marker_2.transform.translation.z == approx(2.18787602301)
+    assert marker_2.transform.rotation.x == approx(0.993997406299)
+    assert marker_2.transform.rotation.y == approx(-0.00532003481626)
+    assert marker_2.transform.rotation.z == approx(-0.107390951553)
+    assert marker_2.transform.rotation.w == approx(0.0201999263402)
 
-    def test_map_visualization(self):
-        vis = rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5)
+def test_map_markers_frames(node, tf_buffer):
+    stamp = rospy.get_rostime()
+    timeout = rospy.Duration(5)
 
-    def test_map_debug(self):
-        img = rospy.wait_for_message('aruco_map/debug', Image, timeout=5)
+    marker_1 = tf_buffer.lookup_transform('aruco_map', 'aruco_in_map_1', stamp, timeout)
+    assert marker_1.transform.translation.x == approx(0)
+    assert marker_1.transform.translation.y == approx(0)
+    assert marker_1.transform.translation.z == approx(0)
 
-    # def test_transforms(self):
-    #     pass
+    marker_4 = tf_buffer.lookup_transform('aruco_map', 'aruco_in_map_4', stamp, timeout)
+    assert marker_4.transform.translation.x == approx(1)
+    assert marker_4.transform.translation.y == approx(1)
+    assert marker_4.transform.translation.z == approx(0)
 
+    marker_12 = tf_buffer.lookup_transform('aruco_map', 'aruco_in_map_12', stamp, timeout)
+    assert marker_12.transform.translation.x == approx(0.2)
+    assert marker_12.transform.translation.y == approx(0.5)
+    assert marker_12.transform.translation.z == approx(0)
 
-rostest.rosrun('aruco_pose', 'test_aruco_detect', TestArucoPose, sys.argv)
+def test_visualization(node):
+    vis = rospy.wait_for_message('aruco_detect/visualization', VisMarkerArray, timeout=5)
+    assert len(vis.markers) == 11
+
+def test_debug(node):
+    img = rospy.wait_for_message('aruco_detect/debug', Image, timeout=5)
+    assert img.width == 640
+    assert img.height == 480
+    assert img.header.frame_id == 'main_camera_optical'
+
+def test_map(node):
+    pose = rospy.wait_for_message('aruco_map/pose', PoseWithCovarianceStamped, timeout=5)
+    assert pose.header.frame_id == 'main_camera_optical'
+    assert pose.pose.pose.position.x == approx(-0.629167753342)
+    assert pose.pose.pose.position.y == approx(0.293822650809)
+    assert pose.pose.pose.position.z == approx(2.12641343155)
+    assert pose.pose.pose.orientation.x == approx(-0.998383794799)
+    assert pose.pose.pose.orientation.y == approx(-5.20919098575e-06)
+    assert pose.pose.pose.orientation.z == approx(-0.0300861070302)
+    assert pose.pose.pose.orientation.w == approx(0.0482143590507)
+
+def test_map_image(node):
+    img = rospy.wait_for_message('aruco_map/image', Image, timeout=5)
+    assert img.width == 2000
+    assert img.height == 2000
+    assert img.encoding == 'mono8'
+
+def test_map_visualization(node):
+    vis = rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5)
+
+def test_map_debug(node):
+    img = rospy.wait_for_message('aruco_map/debug', Image, timeout=5)

aruco_pose/test/basic.test

@@ -5,23 +5,27 @@
         <param name="camera_info_url" value="file://$(find aruco_pose)/test/camera_info.yaml" />
     </node>
 
-    <node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager"/>
+    <node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager" required="true"/>
 
-    <node pkg="nodelet" clear_params="true" type="nodelet" name="aruco_detect" args="load aruco_pose/aruco_detect nodelet_manager">
+    <node pkg="nodelet" clear_params="true" type="nodelet" name="aruco_detect" args="load aruco_pose/aruco_detect nodelet_manager" required="true">
         <remap from="image_raw" to="main_camera/image_raw"/>
         <remap from="camera_info" to="main_camera/camera_info"/>
         <param name="length" value="0.33"/>
         <param name="length_override/3" value="0.1"/>
         <param name="estimate_poses" value="true"/>
+        <param name="send_tf" value="true"/>
     </node>
 
-    <node name="aruco_map" pkg="nodelet" type="nodelet" args="load aruco_pose/aruco_map nodelet_manager" clear_params="true">
+    <node name="aruco_map" pkg="nodelet" type="nodelet" args="load aruco_pose/aruco_map nodelet_manager" clear_params="true" required="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/basic.txt"/>
+        <param name="markers/frame_id" value="aruco_map"/>
+        <param name="markers/child_frame_id_prefix" value="aruco_in_map_"/>
     </node>
 
-    <test test-name="test_aruco_pose" pkg="aruco_pose" type="basic.py"/>
+    <param name="test_module" value="$(find aruco_pose)/test/basic.py"/>
+    <test test-name="aruco_pose_test" pkg="ros_pytest" type="ros_pytest_runner"/>
 </launch>

aruco_pose/test/basic.txt

@@ -1,7 +1,11 @@
+# Default markers
 1	0.33	0	0	0	0	0	0
 2	0.33	1	0	0	0	0	0
 3	0.33	0	1	0	0	0	0
 4	0.33	1	1	0	0	0	0
+# Marker with non-zero yaw rotation
 10	0.5	0.5	2	0	1.2	0	0
+# Marker with non-zero pitch and roll rotation
 11	0.2	0.5	0.5	0	0.0	-1	1
+# Marker with yaw, pitch and roll rotation
 12	0.4	0.2	0.5	0	0.1	-1.2	-0.5

aruco_pose/test/largemap.py

@@ -0,0 +1,26 @@
+#!/usr/bin/env python
+import sys
+import unittest
+import json
+import rospy
+import rostest
+
+from sensor_msgs.msg import Image
+from visualization_msgs.msg import MarkerArray as VisMarkerArray
+
+
+class TestArucoPose(unittest.TestCase):
+    def setUp(self):
+        rospy.init_node('test_aruco_largemap', anonymous=True)
+
+    def test_map_image(self):
+        img = rospy.wait_for_message('aruco_map/image', Image, timeout=5)
+        self.assertEqual(img.width, 2000)
+        self.assertEqual(img.height, 2000)
+        self.assertEqual(img.encoding, 'mono8')
+
+    def test_map_visualization(self):
+        vis = rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5)
+
+
+rostest.rosrun('aruco_pose', 'test_aruco_largemap', TestArucoPose, sys.argv)

aruco_pose/test/largemap.test

@@ -0,0 +1,13 @@
+<launch>
+    <node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager" required="true"/>
+
+    <node name="aruco_map" pkg="nodelet" type="nodelet" args="load aruco_pose/aruco_map nodelet_manager" clear_params="true" required="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/largemap.txt"/>
+    </node>
+
+    <test test-name="test_aruco_pose_largemap" pkg="aruco_pose" type="largemap.py"/>
+</launch>

aruco_pose/test/largemap.txt

@@ -0,0 +1,11 @@
+0 0.2 0 0 0 0 0 0
+1 0.2 10 0 0 0 0 0
+2 0.2 20 0 0 0 0 0
+3 0.2 30 0 0 0 0 0
+4 0.2 40 0 0 0 0 0
+5 0.2 50 0 0 0 0 0
+6 0.2 60 0 0 0 0 0
+7 0.2 70 0 0 0 0 0
+8 0.2 80 0 0 0 0 0
+9 0.2 90 0 0 0 0 0
+10 0.2 100 0 0 0 0 0

aruco_pose/test/test_node_failure.py

@@ -0,0 +1,13 @@
+import rospy
+import pytest
+
+from visualization_msgs.msg import MarkerArray as VisMarkerArray
+
+
+@pytest.fixture
+def node():
+    return rospy.init_node('aruco_pose_test', anonymous=True)
+
+def test_node_failure(node):
+    with pytest.raises(rospy.exceptions.ROSException):
+        rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5)

aruco_pose/test/test_node_failure.test

@@ -0,0 +1,14 @@
+<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>
+
+    <param name="test_module" value="$(find aruco_pose)/test/test_node_failure.py"/>
+    <test test-name="test_node_failure" pkg="ros_pytest" type="ros_pytest_runner"/>
+</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,13 @@
+import rospy
+import pytest
+
+from visualization_msgs.msg import MarkerArray as VisMarkerArray
+
+
+@pytest.fixture
+def node():
+    return rospy.init_node('aruco_pose_test_empty_map', anonymous=True)
+
+def test_empty_map(node):
+    markers = rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5)
+    assert len(markers.markers) == 0

aruco_pose/test/test_parser_empty_map.test

@@ -0,0 +1,14 @@
+<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>
+
+    <param name="test_module" value="$(find aruco_pose)/test/test_parser_empty_map.py"/>
+    <test test-name="test_node_empty_map" pkg="ros_pytest" type="ros_pytest_runner"/>
+</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,61 @@
+import rospy
+import pytest
+
+from sensor_msgs.msg import Image
+from aruco_pose.msg import MarkerArray
+from visualization_msgs.msg import MarkerArray as VisMarkerArray
+
+
+@pytest.fixture
+def node():
+    return rospy.init_node('aruco_pose_test', anonymous=True)
+
+def approx(expected):
+    return pytest.approx(expected, abs=1e-4) # compare floats more roughly
+
+def test_markers(node):
+    markers = rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5)
+    assert len(markers.markers) == 6
+
+    assert markers.markers[0].pose.position.x == approx(0)
+    assert markers.markers[0].pose.position.y == approx(0)
+    assert markers.markers[0].pose.position.z == approx(0)
+
+    assert markers.markers[1].pose.position.x == approx(1)
+    assert markers.markers[1].pose.position.y == approx(1)
+    assert markers.markers[1].pose.position.z == approx(1)
+
+    assert markers.markers[2].pose.position.x == approx(1)
+    assert markers.markers[2].pose.position.y == approx(0)
+    assert markers.markers[2].pose.position.z == approx(0.5)
+
+    assert markers.markers[3].pose.position.x == approx(0)
+    assert markers.markers[3].pose.position.y == approx(1)
+    assert markers.markers[3].pose.position.z == approx(0)
+
+    assert markers.markers[4].pose.position.x == approx(1)
+    assert markers.markers[4].pose.position.y == approx(0.5)
+    assert markers.markers[4].pose.position.z == approx(0)
+
+    assert markers.markers[5].pose.position.x == approx(2.2)
+    assert markers.markers[5].pose.position.y == approx(0.2)
+    assert markers.markers[5].pose.position.z == approx(0)
+
+    assert markers.markers[0].scale.x == approx(0.33)
+    assert markers.markers[0].scale.y == approx(0.33)
+    assert markers.markers[1].scale.x == approx(0.225)
+    assert markers.markers[1].scale.y == approx(0.225)
+    assert markers.markers[2].scale.x == approx(0.45)
+    assert markers.markers[2].scale.y == approx(0.45)
+    assert markers.markers[3].scale.x == approx(0.15)
+    assert markers.markers[3].scale.y == approx(0.15)
+    assert markers.markers[4].scale.x == approx(0.25)
+    assert markers.markers[4].scale.y == approx(0.25)
+    assert markers.markers[5].scale.x == approx(0.35)
+    assert markers.markers[5].scale.y == approx(0.35)
+
+def test_map_image(node):
+    img = rospy.wait_for_message('aruco_map/image', Image, timeout=5)
+    assert img.width == 2000
+    assert img.height == 2000
+    assert img.encoding == 'mono8'

aruco_pose/test/test_parser_pass.test

@@ -0,0 +1,14 @@
+<launch>
+    <node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager" required="true"/>
+
+    <node name="aruco_map" pkg="nodelet" type="nodelet" args="load aruco_pose/aruco_map nodelet_manager" clear_params="true" required="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>
+
+    <param name="test_module" value="$(find aruco_pose)/test/test_parser_pass.py"/>
+    <test test-name="test_node_pass" pkg="ros_pytest" type="ros_pytest_runner"/>
+</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

@@ -1,12 +1,12 @@
 [Unit]
 Description=Clever ROS package
 Requires=roscore.service
-After=roscore.service
+After=network.target
 
 [Service]
 User=pi
-EnvironmentFile=/lib/systemd/system/roscore.env
-ExecStart=/opt/ros/kinetic/bin/roslaunch clever clever.launch --wait --screen
+ExecStart=/bin/sh -c ". /home/pi/catkin_ws/devel/setup.sh; \
+                      ROS_HOSTNAME=`hostname`.local exec roslaunch clever clever.launch --wait --screen --skip-log-check"
 Restart=on-failure
 RestartSec=3
 

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,13 @@ 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
+
+echo_stamp "Rename hostname to $NEW_SSID"
+hostnamectl set-hostname $NEW_SSID
+sed -i 's/127\.0\.1\.1.*/127.0.1.1\t'${NEW_SSID}' '${NEW_SSID}'.local/g' /etc/hosts
+# .local (mdns) hostname added to make it accesable when wlan and ethernet interfaces down
 
 echo_stamp "Harware setup"
 /root/hardware_setup.sh

builder/assets/kinetic-rosdep-clever.yaml

@@ -541,3 +541,15 @@ 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]
+ros_pytest:
+  debian:
+    stretch: [ros-kinetic-ros-pytest]

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/roscore.env

@@ -1,10 +0,0 @@
-ROS_ROOT=/opt/ros/kinetic/share/ros
-ROS_DISTRO=kinetic
-ROS_PACKAGE_PATH=/home/pi/catkin_ws/src:/opt/ros/kinetic/share
-ROS_PORT=11311
-ROS_MASTER_URI=http://localhost:11311
-CMAKE_PREFIX_PATH=/home/pi/catkin_ws/devel:/opt/ros/kinetic
-PATH=/opt/ros/kinetic/bin:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin:/usr/games:/usr/local/games:/snap/bin
-LD_LIBRARY_PATH=/opt/ros/kinetic/lib
-PYTHONPATH=/home/pi/catkin_ws/devel/lib/python2.7/dist-packages:/opt/ros/kinetic/lib/python2.7/dist-packages
-ROS_HOSTNAME=raspberrypi.local

builder/assets/roscore.service

@@ -4,8 +4,7 @@ After=network.target
 
 [Service]
 User=pi
-EnvironmentFile=/lib/systemd/system/roscore.env
-ExecStart=/opt/ros/kinetic/bin/roscore
+ExecStart=/bin/sh -c ". /opt/ros/kinetic/setup.sh; ROS_HOSTNAME=`hostname`.local exec roscore"
 Restart=on-failure
 RestartSec=3
 

builder/image-build.sh

@@ -1,17 +1,21 @@
 #! /usr/bin/env bash
 
 #
-# Script for build the image. Used builder script of the target repo
+# Script for build the image. Used builder script of the target repo.
 # For build: docker run --privileged -it --rm -v /dev:/dev -v $(pwd):/builder/repo smirart/builder
 #
 # Copyright (C) 2018 Copter Express Technologies
 #
 # Author: Artem Smirnov <urpylka@gmail.com>
 #
+# Distributed under MIT License (available at https://opensource.org/licenses/MIT).
+# The above copyright notice and this permission notice shall be included in all
+# copies or substantial portions of the Software.
+#
 
 set -e # Exit immidiately on non-zero result
 
-SOURCE_IMAGE="https://downloads.raspberrypi.org/raspbian_lite/images/raspbian_lite-2018-11-15/2018-11-13-raspbian-stretch-lite.zip"
+SOURCE_IMAGE="https://downloads.raspberrypi.org/raspbian_lite/images/raspbian_lite-2019-04-09/2019-04-08-raspbian-stretch-lite.zip"
 
 export DEBIAN_FRONTEND=${DEBIAN_FRONTEND:='noninteractive'}
 export LANG=${LANG:='C.UTF-8'}
@@ -105,13 +109,14 @@ ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} exec ${SCRIPTS_DIR}'/image-network.
 [[ $(arch) == 'armv7l' ]] && NUMBER_THREADS=1 || NUMBER_THREADS=$(nproc --all)
 # Clever
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/clever.service' '/lib/systemd/system/'
-${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/'
+${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${REPO_DIR}'/clever/config/99-px4fmu.rules' '/lib/udev/rules.d/'
+# Add rename script
 ${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 ] && {
-    echo -e "\n${ANSI_RED}The command \"$@\" failed 3 times.${ANSI_RESET}\n" >&2
+  [ $count -gt $max_count ] && {
+    echo -e "\nThe command \"$@\" failed $max_count times.\n" >&2
   }
 
   return $result
 }
 
 # TODO: 'kinetic-rosdep-clever.yaml' should add only if we use our repo?
-echo_stamp "Init rosdep" \
-&& rosdep init \
-&& echo "yaml file:///etc/ros/rosdep/kinetic-rosdep-clever.yaml" >> /etc/ros/rosdep/sources.list.d/20-default.list \
-&& rosdep update
+echo_stamp "Init rosdep"
+my_travis_retry rosdep init
+echo "yaml file:///etc/ros/rosdep/kinetic-rosdep-clever.yaml" >> /etc/ros/rosdep/sources.list.d/20-default.list
+my_travis_retry rosdep update
 
 echo_stamp "Populate rosdep for ROS user"
-sudo -u pi rosdep update
+my_travis_retry sudo -u pi rosdep update
 
 resolve_rosdep() {
   # TEMPLATE: resolve_rosdep <CATKIN_PATH> <ROS_DISTRO> <OS_DISTRO> <OS_VERSION>
@@ -137,6 +142,10 @@ fi
 
 export ROS_IP='127.0.0.1' # needed for running tests
 
+echo_stamp "Reconfiguring Clever repository for simplier unshallowing"
+cd /home/pi/catkin_ws/src/clever
+git config remote.origin.fetch "+refs/heads/*:refs/remotes/origin/*"
+
 echo_stamp "Installing CLEVER" \
 && cd /home/pi/catkin_ws/src/clever \
 && git status \
@@ -146,8 +155,6 @@ 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" \
@@ -162,18 +169,21 @@ gitbook build
 echo_stamp "Installing additional ROS packages"
 apt-get install -y --no-install-recommends \
     ros-kinetic-dynamic-reconfigure \
-    ros-kinetic-tf2-web-republisher \
     ros-kinetic-compressed-image-transport \
     ros-kinetic-rosbridge-suite \
     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)" \
+echo_stamp "Install GeographicLib datasets (needed for mavros)" \
 && wget -qO- https://raw.githubusercontent.com/mavlink/mavros/master/mavros/scripts/install_geographiclib_datasets.sh | bash
 
+echo_stamp "Running tests"
+catkin_make run_tests && catkin_test_results
+
 echo_stamp "Change permissions for catkin_ws"
 chown -Rf pi:pi /home/pi/catkin_ws
 
@@ -182,7 +192,7 @@ cat << EOF >> /home/pi/.bashrc
 LANG='C.UTF-8'
 LC_ALL='C.UTF-8'
 ROS_DISTRO='kinetic'
-export ROS_HOSTNAME='raspberrypi.local'
+export ROS_HOSTNAME=\`hostname\`.local
 source /opt/ros/kinetic/setup.bash
 source /home/pi/catkin_ws/devel/setup.bash
 EOF

builder/image-software.sh

@@ -1,12 +1,16 @@
 #! /usr/bin/env bash
 
 #
-# Script for install software to the image.
+# Script for installing software to the image.
 #
 # Copyright (C) 2018 Copter Express Technologies
 #
 # Author: Artem Smirnov <urpylka@gmail.com>
 #
+# Distributed under MIT License (available at https://opensource.org/licenses/MIT).
+# The above copyright notice and this permission notice shall be included in all
+# copies or substantial portions of the Software.
+#
 
 set -e # Exit immidiately on non-zero result
 
@@ -59,7 +63,7 @@ echo_stamp "Install apt keys & repos"
 curl http://repo.coex.space/aptly_repo_signing.key 2> /dev/null | apt-key add -
 apt-get update \
 && apt-get install --no-install-recommends -y -qq dirmngr=2.1.18-8~deb9u4 > /dev/null \
-&& apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key 421C365BD9FF1F717815A3895523BAEEB01FA116
+&& apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654
 
 echo "deb http://packages.ros.org/ros/ubuntu stretch main" > /etc/apt/sources.list.d/ros-latest.list
 echo "deb http://repo.coex.space/rpi-ros-kinetic stretch main" > /etc/apt/sources.list.d/rpi-ros-kinetic.list
@@ -84,9 +88,9 @@ lsof=4.89+dfsg-0.1 \
 git \
 dnsmasq=2.76-5+rpt1+deb9u1  \
 tmux=2.3-4 \
-vim=2:8.0.0197-4+deb9u1 \
+vim \
 cmake=3.7.2-1 \
-libjpeg8-dev=8d1-2 \
+libjpeg8=8d1-2 \
 tcpdump \
 ltrace \
 libpoco-dev=1.7.6+dfsg1-5+deb9u1 \
@@ -99,14 +103,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 \
+mjpg-streamer=2.0 \
 && echo_stamp "Everything was installed!" "SUCCESS" \
 || (echo_stamp "Some packages wasn't installed!" "ERROR"; exit 1)
 
 echo_stamp "Updating kernel to fix camera bug"
-apt-get install --no-install-recommends -y raspberrypi-kernel=1.20190215-1
+apt-get install --no-install-recommends -y \
+	raspberrypi-kernel=1.20190709~stretch-1 \
+	raspberrypi-bootloader=1.20190709~stretch-1 \
+	libraspberrypi-bin=1.20190709~stretch-1 \
+	libraspberrypi-dev=1.20190709~stretch-1 \
+	libraspberrypi0=1.20190709~stretch-1 \
+	wireless-regdb=2018.05.09-0~rpt1 \
+	wpasupplicant=2:2.6-21~bpo9~rpt1
 
 # Deny byobu to check available updates
 sed -i "s/updates_available//" /usr/share/byobu/status/status
@@ -154,6 +168,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,8 @@ monkey --version
 pigpiod -v
 i2cdetect -V
 butterfly -h
+espeak --version
+mjpg_streamer --version
 
 # ros stuff
 
@@ -46,3 +48,4 @@ rosversion rosserial
 rosversion usb_cam
 rosversion cv_camera
 rosversion web_video_server
+rosversion rosshow

check_files_size.py

@@ -0,0 +1,31 @@
+#!/usr/bin/env python3
+
+import os
+import sys
+
+def human_size(num, suffix='B'):
+    for unit in ['', 'Ki', 'Mi', 'Gi', 'Ti', 'Pi', 'Ei', 'Zi']:
+        if abs(num) < 1024.0:
+            return "%3.1f %s%s" % (num, unit, suffix)
+        num /= 1024.0
+    return "%.1f %s%s" % (num, 'Yi', suffix)
+
+SIZE_LIMIT = 600 * 1024
+EXCLUDE = 'rviz.png', 'ssid.png', 'sitl_docker_demo.png', 'qgc-params.png', 'butterfly.png', \
+    'Clever main.png', 'fpv_3.png', '1_4.png', 'fpv_4.png', 'detal1.png', 'lockradio.png', \
+    'qground.png', 'allElements.png', 'download-log.png', 'explosion.png', 'rqt.png', \
+    'cl3_mountBEC.JPG', 'cl3_mountRpiCamera.JPG'
+
+code = 0
+
+for root, dirs, files in os.walk('docs/'):
+    for f in files:
+        if f not in EXCLUDE:
+            path = os.path.join(root, f)
+            size = os.path.getsize(path)
+            if size > SIZE_LIMIT:
+                print('\x1b[1;31mFile too large ({}): {}\x1b[0m'.format(human_size(size), path), \
+                    file=sys.stderr)
+                code = 1
+
+sys.exit(code)

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)
@@ -231,6 +227,19 @@ target_link_libraries(clever
 #   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 # )
 
+# Only install udev rules when building a Debian package
+# FIXME: Other operating systems may have other prefixes
+string(FIND ${CMAKE_INSTALL_PREFIX} "/opt/ros" _PREFIX_INDEX)
+if (${_PREFIX_INDEX} EQUAL 0)
+  message(STATUS "Building as a Debian package - adding udev rules as installable files")
+  install(FILES
+    config/99-px4fmu.rules
+    DESTINATION /lib/udev/rules.d
+  )
+else()
+  message(STATUS "Building in a user workspace - not adding udev rules")
+endif()
+
 #############
 ## Testing ##
 #############
@@ -243,3 +252,8 @@ target_link_libraries(clever
 
 ## Add folders to be run by python nosetests
 # catkin_add_nosetests(test)
+
+if (CATKIN_ENABLE_TESTING)
+  find_package(rostest REQUIRED)
+  add_rostest(test/basic.test)
+endif()

clever/launch/aruco.launch

@@ -22,8 +22,11 @@
         <remap from="markers" to="aruco_detect/markers"/>
         <param name="map" value="$(find aruco_pose)/map/map.txt"/>
         <param name="known_tilt" value="map"/>
+        <param name="image_axis" value="true"/>
         <param name="frame_id" value="aruco_map_detected" if="$(arg aruco_vpe)"/>
         <param name="frame_id" value="aruco_map" unless="$(arg aruco_vpe)"/>
+        <param name="markers/frame_id" value="aruco_map"/>
+        <param name="markers/child_frame_id_prefix" value="aruco_"/>
     </node>
 
     <!-- vpe publisher from aruco markers -->

clever/launch/clever.launch

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

clever/launch/main_camera.launch

@@ -24,6 +24,7 @@
         <param name="rate" value="100"/> <!-- poll rate -->
         <param name="cv_cap_prop_fps" value="40"/> <!-- camera FPS -->
         <param name="capture_delay" value="0.02"/> <!-- approximate delay on frame retrieving -->
+        <param name="rescale_camera_info" value="true"/> <!-- automatically rescale camera calibration info -->
 
         <!-- camera resolution, NOTE: camera_info file should match it -->
         <param name="image_width" value="320"/>

clever/launch/mavros.launch

@@ -56,7 +56,7 @@
     <node pkg="tf2_ros" type="static_transform_publisher" name="rangefinder_frame" args="0 0 -0.05 0 1.5707963268 0 base_link rangefinder"/>
 
     <!-- Copter visualization -->
-    <node name="copter_visualization" pkg="mavros_extras" type="copter_visualization" if="$(arg viz)">
+    <node name="visualization" pkg="mavros_extras" type="visualization" if="$(arg viz)">
         <remap to="mavros/local_position/pose" from="local_position"/>
         <remap to="mavros/setpoint_position/local" from="local_setpoint"/>
         <param name="fixed_frame_id" value="map"/>

clever/package.xml

@@ -30,13 +30,14 @@
   <depend>nodelet</depend>
   <depend>mavros</depend>
   <depend>mavros_extras</depend>
-  <depend>lxml</depend>
   <depend>cv_camera</depend>
   <depend>cv_bridge</depend>
   <depend>opencv3</depend>
-  <depend>mjpg-streamer</depend>
   <depend>rosbridge_server</depend>
   <depend>web_video_server</depend>
+  <depend>tf2_web_republisher</depend>
+  <depend>python-lxml</depend>
+  <exec_depend>python-pymavlink</exec_depend>
   <!-- Use test_depend for packages you need only for testing: -->
   <!--   <test_depend>gtest</test_depend> -->
 

clever/requirements.txt

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

clever/src/frames.cpp

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

clever/src/selfcheck.py

@@ -1,21 +1,36 @@
 #!/usr/bin/env python
+# coding=utf-8
+
+# Copyright (C) 2018 Copter Express Technologies
+#
+# Author: Oleg Kalachev <okalachev@gmail.com>
+#
+# Distributed under MIT License (available at https://opensource.org/licenses/MIT).
+# The above copyright notice and this permission notice shall be included in all
+# copies or substantial portions of the Software.
 
 import math
-from subprocess import Popen, PIPE
+import subprocess
 import re
 import traceback
+from threading import Event
+import numpy
 import rospy
+from systemd import journal
+import tf2_ros
+import tf2_geometry_msgs
+from pymavlink import mavutil
 from std_srvs.srv import Trigger
 from sensor_msgs.msg import Image, CameraInfo, NavSatFix, Imu, Range
-from mavros_msgs.msg import State, OpticalFlowRad
+from mavros_msgs.msg import State, OpticalFlowRad, Mavlink
 from mavros_msgs.srv import ParamGet
-from geometry_msgs.msg import PoseStamped, TwistStamped, PoseWithCovarianceStamped
+from geometry_msgs.msg import PoseStamped, TwistStamped, PoseWithCovarianceStamped, Vector3Stamped
+from visualization_msgs.msg import MarkerArray as VisualizationMarkerArray
 import tf.transformations as t
 from aruco_pose.msg import MarkerArray
+from mavros import mavlink
 
 
-# TODO: roscore is running
-# TODO: clever.service is running
 # TODO: check attitude is present
 # TODO: disk free space
 # TODO: map, base_link, body
@@ -28,28 +43,41 @@ from aruco_pose.msg import MarkerArray
 rospy.init_node('selfcheck')
 
 
+tf_buffer = tf2_ros.Buffer()
+tf_listener = tf2_ros.TransformListener(tf_buffer)
+
+
 failures = []
+infos = []
+current_check = None
 
 
 def failure(text, *args):
-    failures.append(text % args)
+    msg = text % args
+    rospy.logwarn('%s: %s', current_check, msg)
+    failures.append(msg)
+
+
+def info(text, *args):
+    msg = text % args
+    rospy.loginfo('%s: %s', current_check, msg)
+    infos.append(msg)
 
 
 def check(name):
     def inner(fn):
         def wrapper(*args, **kwargs):
             failures[:] = []
+            infos[:] = []
+            global current_check
+            current_check = name
             try:
                 fn(*args, **kwargs)
-                for f in failures:
-                    rospy.logwarn('%s: %s', name, f)
             except Exception as e:
-                for f in failures:
-                    rospy.logwarn('%s: %s', name, f)
                 traceback.print_exc()
-                rospy.logwarn('%s: exception occurred', name)
+                rospy.logerr('%s: exception occurred', name)
                 return
-            if not failures:
+            if not failures and not infos:
                 rospy.loginfo('%s: OK', name)
         return wrapper
     return inner
@@ -59,7 +87,12 @@ param_get = rospy.ServiceProxy('mavros/param/get', ParamGet)
 
 
 def get_param(name):
-    res = param_get(param_id=name)
+    try:
+        res = param_get(param_id=name)
+    except rospy.ServiceException as e:
+        failure('%s: %s', name, str(e))
+        return None
+
     if not res.success:
         failure('Unable to retrieve PX4 parameter %s', name)
     else:
@@ -68,36 +101,162 @@ def get_param(name):
         return res.value.real
 
 
+recv_event = Event()
+link = mavutil.mavlink.MAVLink('', 255, 1)
+mavlink_pub = rospy.Publisher('mavlink/to', Mavlink, queue_size=1)
+mavlink_recv = ''
+
+
+def mavlink_message_handler(msg):
+    global mavlink_recv
+    if msg.msgid == 126:
+        mav_bytes_msg = mavlink.convert_to_bytes(msg)
+        mav_msg = link.decode(mav_bytes_msg)
+        mavlink_recv += ''.join(chr(x) for x in mav_msg.data[:mav_msg.count])
+        if 'nsh>' in mavlink_recv:
+            # Remove the last line, including newline before prompt
+            mavlink_recv = mavlink_recv[:mavlink_recv.find('nsh>') - 1]
+            recv_event.set()
+
+
+mavlink_sub = rospy.Subscriber('mavlink/from', Mavlink, mavlink_message_handler)
+# FIXME: not sleeping here still breaks things
+rospy.sleep(0.5)
+
+
+def mavlink_exec(cmd, timeout=3.0):
+    global mavlink_recv
+    mavlink_recv = ''
+    recv_event.clear()
+    if not cmd.endswith('\n'):
+        cmd += '\n'
+    msg = mavutil.mavlink.MAVLink_serial_control_message(
+        device=mavutil.mavlink.SERIAL_CONTROL_DEV_SHELL,
+        flags=mavutil.mavlink.SERIAL_CONTROL_FLAG_RESPOND | mavutil.mavlink.SERIAL_CONTROL_FLAG_EXCLUSIVE |
+              mavutil.mavlink.SERIAL_CONTROL_FLAG_MULTI,
+        timeout=3,
+        baudrate=0,
+        count=len(cmd),
+        data=map(ord, cmd.ljust(70, '\0')))
+    msg.pack(link)
+    ros_msg = mavlink.convert_to_rosmsg(msg)
+    mavlink_pub.publish(ros_msg)
+    recv_event.wait(timeout)
+    return mavlink_recv
+
+
+BOARD_ROTATIONS = {
+    0: 'no rotation',
+    1: 'yaw 45°',
+    2: 'yaw 90°',
+    3: 'yaw 135°',
+    4: 'yaw 180°',
+    5: 'yaw 225°',
+    6: 'yaw 270°',
+    7: 'yaw 315°',
+    8: 'roll 180°',
+    9: 'roll 180°, yaw 45°',
+    10: 'roll 180°, yaw 90°',
+    11: 'roll 180°, yaw 135°',
+    12: 'pitch 180°',
+    13: 'roll 180°, yaw 225°',
+    14: 'roll 180°, yaw 270°',
+    15: 'roll 180°, yaw 315°',
+    16: 'roll 90°',
+    17: 'roll 90°, yaw 45°',
+    18: 'roll 90°, yaw 90°',
+    19: 'roll 90°, yaw 135°',
+    20: 'roll 270°',
+    21: 'roll 270°, yaw 45°',
+    22: 'roll 270°, yaw 90°',
+    23: 'roll 270°, yaw 135°',
+    24: 'pitch 90°',
+    25: 'pitch 270°',
+    26: 'roll 270°, yaw 270°',
+    27: 'roll 180°, pitch 270°',
+    28: 'pitch 90°, yaw 180',
+    29: 'pitch 90°, roll 90°',
+    30: 'yaw 293°, pitch 68°, roll 90°',
+    31: 'pitch 90°, roll 270°',
+    32: 'pitch 9°, yaw 180°',
+    33: 'pitch 45°',
+    34: 'pitch 315°',
+}
+
+
 @check('FCU')
 def check_fcu():
     try:
         state = rospy.wait_for_message('mavros/state', State, timeout=3)
         if not state.connected:
             failure('no connection to the FCU (check wiring)')
+            return
+
+        # Make sure the console is available to us
+        mavlink_exec('\n')
+        version_str = mavlink_exec('ver all')
+        if version_str == '':
+            info('no version data available from SITL')
+
+        r = re.compile(r'^FW (git tag|version): (v?\d\.\d\.\d.*)$')
+        is_clever_firmware = False
+        for ver_line in version_str.split('\n'):
+            match = r.search(ver_line)
+            if match is not None:
+                field, version = match.groups()
+                info('firmware %s: %s' % (field, version))
+                if 'clever' in version:
+                    is_clever_firmware = True
+
+        if not is_clever_firmware:
+            failure('not running Clever PX4 firmware, check http://clever.copterexpress.com/firmware.html')
 
         est = get_param('SYS_MC_EST_GROUP')
         if est == 1:
-            rospy.loginfo('Selected estimator: LPE')
+            info('selected estimator: LPE')
             fuse = get_param('LPE_FUSION')
             if fuse & (1 << 4):
-                rospy.loginfo('LPE_FUSION: land detector fusion is enabled')
+                info('LPE_FUSION: land detector fusion is enabled')
             else:
-                rospy.loginfo('LPE_FUSION: land detector fusion is disabled')
+                info('LPE_FUSION: land detector fusion is disabled')
             if fuse & (1 << 7):
-                rospy.loginfo('LPE_FUSION: barometer fusion is enabled')
+                info('LPE_FUSION: barometer fusion is enabled')
             else:
-                rospy.loginfo('LPE_FUSION: barometer fusion is disabled')
+                info('LPE_FUSION: barometer fusion is disabled')
 
         elif est == 2:
-            rospy.loginfo('Selected estimator: EKF2')
+            info('selected estimator: EKF2')
         else:
-            failure('Unknown selected estimator: %s', est)
+            failure('unknown selected estimator: %s', est)
+
+        rot = get_param('SENS_BOARD_ROT')
+        if rot is not None:
+            try:
+                info('board rotation: %s', BOARD_ROTATIONS[rot])
+            except KeyError:
+                failure('unknown board rotation %s', rot)
 
     except rospy.ROSException:
         failure('no MAVROS state (check wiring)')
 
 
-@check('Camera')
+def describe_direction(v):
+    if v.x > 0.9:
+        return 'forward'
+    elif v.x < - 0.9:
+        return 'backward'
+    elif v.y > 0.9:
+        return 'left'
+    elif v.y < -0.9:
+        return 'right'
+    elif v.z > 0.9:
+        return 'upward'
+    elif v.z < -0.9:
+        return 'downward'
+    else:
+        return None
+
+
 def check_camera(name):
     try:
         img = rospy.wait_for_message(name + '/image_raw', Image, timeout=1)
@@ -105,32 +264,98 @@ def check_camera(name):
         failure('%s: no images (is the camera connected properly?)', name)
         return
     try:
-        info = rospy.wait_for_message(name + '/camera_info', CameraInfo, timeout=1)
+        camera_info = rospy.wait_for_message(name + '/camera_info', CameraInfo, timeout=1)
     except rospy.ROSException:
         failure('%s: no calibration info', name)
         return
 
-    if img.width != info.width:
-        failure('%s: calibration width doesn\'t match image width (%d != %d)', name, info.width, img.width)
-    if img.height != info.height:
-        failure('%s: calibration height doesn\'t match image height (%d != %d))', name, info.height, img.height)
+    if img.width != camera_info.width:
+        failure('%s: calibration width doesn\'t match image width (%d != %d)', name, camera_info.width, img.width)
+    if img.height != camera_info.height:
+        failure('%s: calibration height doesn\'t match image height (%d != %d))', name, camera_info.height, img.height)
+
+    try:
+        optical = Vector3Stamped()
+        optical.header.frame_id = img.header.frame_id
+        optical.vector.z = 1
+        cable = Vector3Stamped()
+        cable.header.frame_id = img.header.frame_id
+        cable.vector.y = 1
+
+        optical = describe_direction(tf_buffer.transform(optical, 'base_link').vector)
+        cable = describe_direction(tf_buffer.transform(cable, 'base_link').vector)
+        if not optical or not cable:
+            info('%s: custom camera orientation detected', name)
+        else:
+            info('camera is oriented %s, camera cable goes %s', optical, cable)
+
+    except tf2_ros.TransformException:
+        failure('cannot transform from base_link to camera frame')
 
 
-@check('ArUco detector')
+@check('Main camera')
+def check_main_camera():
+    check_camera('main_camera')
+
+
+def is_process_running(binary, exact=False, full=False):
+    try:
+        args = ['pgrep']
+        if exact:
+            args.append('-x')  # match exactly with the command name
+        if full:
+            args.append('-f')  # use full process name to match
+        args.append(binary)
+        subprocess.check_output(args)
+        return True
+    except subprocess.CalledProcessError:
+        return False
+
+
+@check('ArUco markers')
 def check_aruco():
-    try:
-        rospy.wait_for_message('aruco_detect/markers', MarkerArray, timeout=1)
-    except rospy.ROSException:
-        failure('no markers detection')
+    if is_process_running('aruco_detect', full=True):
+        info('aruco_detect/length = %g m', rospy.get_param('aruco_detect/length'))
+        known_tilt = rospy.get_param('aruco_detect/known_tilt')
+        if known_tilt == 'map':
+            known_tilt += ' (ALL markers are on the floor)'
+        elif known_tilt == 'map_flipped':
+            known_tilt += ' (ALL markers are on the ceiling)'
+        info('aruco_detector/known_tilt = %s', known_tilt)
+        try:
+            rospy.wait_for_message('aruco_detect/markers', MarkerArray, timeout=1)
+        except rospy.ROSException:
+            failure('no markers detection')
+            return
+    else:
+        info('aruco_detect is not running')
         return
-    try:
-        rospy.wait_for_message('aruco_map/pose', PoseWithCovarianceStamped, timeout=1)
-    except rospy.ROSException:
-        failure('no map detection')
+
+    if is_process_running('aruco_map', full=True):
+        known_tilt = rospy.get_param('aruco_map/known_tilt')
+        if known_tilt == 'map':
+            known_tilt += ' (marker\'s map is on the floor)'
+        elif known_tilt == 'map_flipped':
+            known_tilt += ' (marker\'s map is on the ceiling)'
+        info('aruco_map/known_tilt = %s', known_tilt)
+
+        try:
+            visualization = rospy.wait_for_message('aruco_map/visualization', VisualizationMarkerArray, timeout=1)
+            info('map has %s markers', len(visualization.markers))
+        except:
+            failure('cannot read aruco_map/visualization topic')
+
+        try:
+            rospy.wait_for_message('aruco_map/pose', PoseWithCovarianceStamped, timeout=1)
+        except rospy.ROSException:
+            failure('no map detection')
+    else:
+        info('aruco_map is not running')
 
 
 @check('Vision position estimate')
 def check_vpe():
+    vis = None
     try:
         vis = rospy.wait_for_message('mavros/vision_pose/pose', PoseStamped, timeout=1)
     except rospy.ROSException:
@@ -138,7 +363,11 @@ def check_vpe():
             vis = rospy.wait_for_message('mavros/mocap/pose', PoseStamped, timeout=1)
         except rospy.ROSException:
             failure('no VPE or MoCap messages')
-            return
+            # check if vpe_publisher is running
+            try:
+                subprocess.check_output(['pgrep', '-x', 'vpe_publisher'])
+            except subprocess.CalledProcessError:
+                return  # it's not running, skip following checks
 
     # check PX4 settings
     est = get_param('SYS_MC_EST_GROUP')
@@ -150,27 +379,30 @@ def check_vpe():
         if vision_yaw_w == 0:
             failure('vision yaw weight is zero, change ATT_W_EXT_HDG parameter')
         else:
-            rospy.loginfo('Vision yaw weight: %.2f', vision_yaw_w)
+            info('Vision yaw weight: %.2f', vision_yaw_w)
         fuse = get_param('LPE_FUSION')
         if not fuse & (1 << 2):
-            failure('vision position fusing is disabled, change LPE_FUSION parameter')
+            failure('vision position fusion is disabled, change LPE_FUSION parameter')
         delay = get_param('LPE_VIS_DELAY')
         if delay != 0:
             failure('LPE_VIS_DELAY parameter is %s, but it should be zero', delay)
-        rospy.loginfo('LPE_VIS_XY is %.2f m, LPE_VIS_Z is %.2f m', get_param('LPE_VIS_XY'), get_param('LPE_VIS_Z'))
+        info('LPE_VIS_XY is %.2f m, LPE_VIS_Z is %.2f m', get_param('LPE_VIS_XY'), get_param('LPE_VIS_Z'))
     elif est == 2:
         fuse = get_param('EKF2_AID_MASK')
         if not fuse & (1 << 3):
-            failure('vision position fusing is disabled, change EKF2_AID_MASK parameter')
+            failure('vision position fusion is disabled, change EKF2_AID_MASK parameter')
         if not fuse & (1 << 4):
-            failure('vision yaw fusing is disabled, change EKF2_AID_MASK parameter')
+            failure('vision yaw fusion is disabled, change EKF2_AID_MASK parameter')
         delay = get_param('EKF2_EV_DELAY')
         if delay != 0:
             failure('EKF2_EV_DELAY is %.2f, but it should be zero', delay)
-        rospy.loginfo('EKF2_EVA_NOISE is %.3f, EKF2_EVP_NOISE is %.3f',
+        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:
         pose = rospy.wait_for_message('mavros/local_position/pose', PoseStamped, timeout=1)
@@ -240,7 +472,7 @@ def check_velocity():
             failure('vertical velocity estimation is %.2f m/s; is copter staying still?' % vert)
 
         angular = velocity.twist.angular
-        ANGULAR_VELOCITY_LIMIT = 0.01
+        ANGULAR_VELOCITY_LIMIT = 0.1
         if abs(angular.x) > ANGULAR_VELOCITY_LIMIT:
             failure('pitch rate estimation is %.2f rad/s (%.2f deg/s); is copter staying still?',
                     angular.x, math.degrees(angular.x))
@@ -276,32 +508,32 @@ def check_optical_flow():
         if est == 1:
             fuse = get_param('LPE_FUSION')
             if not fuse & (1 << 1):
-                failure('optical flow fusing is disabled, change LPE_FUSION parameter')
+                failure('optical flow fusion is disabled, change LPE_FUSION parameter')
             if not fuse & (1 << 1):
                 failure('flow gyro compensation is disabled, change LPE_FUSION parameter')
             scale = get_param('LPE_FLW_SCALE')
-            if scale != 0:
+            if not numpy.isclose(scale, 1.0):
                 failure('LPE_FLW_SCALE parameter is %.2f, but it should be 1.0', scale)
 
-            rospy.loginfo('LPE_FLW_QMIN is %s, LPE_FLW_R is %.4f, LPE_FLW_RR is %.4f, SENS_FLOW_MINHGT is %.3f, SENS_FLOW_MAXHGT is %.3f',
-                get_param('LPE_FLW_QMIN'),
-                get_param('LPE_FLW_R'),
-                get_param('LPE_FLW_RR'),
-                get_param('SENS_FLOW_MINHGT'),
-                get_param('SENS_FLOW_MAXHGT'))
+            info('LPE_FLW_QMIN is %s, LPE_FLW_R is %.4f, LPE_FLW_RR is %.4f, SENS_FLOW_MINHGT is %.3f, SENS_FLOW_MAXHGT is %.3f',
+                          get_param('LPE_FLW_QMIN'),
+                          get_param('LPE_FLW_R'),
+                          get_param('LPE_FLW_RR'),
+                          get_param('SENS_FLOW_MINHGT'),
+                          get_param('SENS_FLOW_MAXHGT'))
         elif est == 2:
             fuse = get_param('EKF2_AID_MASK')
             if not fuse & (1 << 1):
-                failure('optical flow fusing is disabled, change EKF2_AID_MASK parameter')
+                failure('optical flow fusion is disabled, change EKF2_AID_MASK parameter')
             delay = get_param('EKF2_OF_DELAY')
             if delay != 0:
                 failure('EKF2_OF_DELAY is %.2f, but it should be zero', delay)
-            rospy.loginfo('EKF2_OF_QMIN is %s, EKF2_OF_N_MIN is %.4f, EKF2_OF_N_MAX is %.4f, SENS_FLOW_MINHGT is %.3f, SENS_FLOW_MAXHGT is %.3f',
-                get_param('EKF2_OF_QMIN'),
-                get_param('EKF2_OF_N_MIN'),
-                get_param('EKF2_OF_N_MAX'),
-                get_param('SENS_FLOW_MINHGT'),
-                get_param('SENS_FLOW_MAXHGT'))
+            info('EKF2_OF_QMIN is %s, EKF2_OF_N_MIN is %.4f, EKF2_OF_N_MAX is %.4f, SENS_FLOW_MINHGT is %.3f, SENS_FLOW_MAXHGT is %.3f',
+                          get_param('EKF2_OF_QMIN'),
+                          get_param('EKF2_OF_N_MIN'),
+                          get_param('EKF2_OF_N_MAX'),
+                          get_param('SENS_FLOW_MINHGT'),
+                          get_param('SENS_FLOW_MAXHGT'))
 
     except rospy.ROSException:
         failure('no optical flow data (from Raspberry)')
@@ -312,13 +544,13 @@ def check_rangefinder():
     # TODO: check FPS!
     rng = False
     try:
-        rospy.wait_for_message('mavros/distance_sensor/rangefinder_sub', Range, timeout=0.5)
+        rospy.wait_for_message('mavros/distance_sensor/rangefinder_sub', Range, timeout=4)
         rng = True
     except rospy.ROSException:
         failure('no rangefinder data from Raspberry')
 
     try:
-        rospy.wait_for_message('mavros/distance_sensor/rangefinder', Range, timeout=0.5)
+        rospy.wait_for_message('mavros/distance_sensor/rangefinder', Range, timeout=4)
         rng = True
     except rospy.ROSException:
         failure('no rangefinder data from PX4')
@@ -330,28 +562,26 @@ def check_rangefinder():
     if est == 1:
         fuse = get_param('LPE_FUSION')
         if not fuse & (1 << 5):
-            rospy.loginfo('"pub agl as lpos down" in LPE_FUSION is disabled, NOT operating over flat surface')
+            info('"pub agl as lpos down" in LPE_FUSION is disabled, NOT operating over flat surface')
         else:
-            rospy.loginfo('"pub agl as lpos down" in LPE_FUSION is enabled, operating over flat surface')
+            info('"pub agl as lpos down" in LPE_FUSION is enabled, operating over flat surface')
 
     elif est == 2:
         hgt = get_param('EKF2_HGT_MODE')
         if hgt != 2:
-            rospy.loginfo('EKF2_HGT_MODE != Range sensor, NOT operating over flat surface')
+            info('EKF2_HGT_MODE != Range sensor, NOT operating over flat surface')
         else:
-            rospy.loginfo('EKF2_HGT_MODE = Range sensor, operating over flat surface')
+            info('EKF2_HGT_MODE = Range sensor, operating over flat surface')
         aid = get_param('EKF2_RNG_AID')
         if aid != 1:
-            rospy.loginfo('EKF2_RNG_AID != 1, range sensor aiding disabled')
+            info('EKF2_RNG_AID != 1, range sensor aiding disabled')
         else:
-            rospy.loginfo('EKF2_RNG_AID = 1, range sensor aiding enabled')
+            info('EKF2_RNG_AID = 1, range sensor aiding enabled')
 
 
 @check('Boot duration')
 def check_boot_duration():
-    proc = Popen('systemd-analyze', stdout=PIPE)
-    proc.wait()
-    output = proc.communicate()[0]
+    output = subprocess.check_output('systemd-analyze')
     r = re.compile(r'([\d\.]+)s$')
     duration = float(r.search(output).groups()[0])
     if duration > 15:
@@ -362,9 +592,7 @@ def check_boot_duration():
 def check_cpu_usage():
     WHITELIST = 'nodelet',
     CMD = "top -n 1 -b -i | tail -n +8 | awk '{ printf(\"%-8s\\t%-8s\\t%-8s\\n\", $1, $9, $12); }'"
-    proc = Popen(CMD, stdout=PIPE, shell=True)
-    proc.wait()
-    output = proc.communicate()[0]
+    output = subprocess.check_output(CMD, shell=True)
     processes = output.split('\n')
     for process in processes:
         if not process:
@@ -376,13 +604,77 @@ def check_cpu_usage():
                     cpu.strip(), cmd.strip(), pid.strip())
 
 
+@check('clever.service')
+def check_clever_service():
+    try:
+        output = subprocess.check_output('systemctl show -p ActiveState --value clever.service'.split(),
+                                         stderr=subprocess.STDOUT)
+    except subprocess.CalledProcessError as e:
+        failure('systemctl returned %s: %s', e.returncode, e.output)
+        return
+    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():
+    try:
+        info('version: %s', open('/etc/clever_version').read().strip())
+    except IOError:
+        info('no /etc/clever_version file, not the Clever image?')
+
+
+@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;
@@ -120,18 +123,41 @@ TwistStamped velocity;
 NavSatFix global_position;
 BatteryState battery;
 
-// Common subcriber callback template that stores message to the variable
+// Common subscriber callback template that stores message to the variable
 template<typename T, T& STORAGE>
 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)
+                          const ros::Time& stamp, const ros::Duration& timeout) // editorconfig-checker-disable-line
 {
-	ros::Rate r(10);
+	ros::Rate r(100);
 	auto start = ros::Time::now();
 	while (ros::ok()) {
 		if (ros::Time::now() - start > timeout) return false;
@@ -175,31 +201,29 @@ bool getTelemetry(GetTelemetry::Request& req, GetTelemetry::Response& res)
 		res.mode = state.mode;
 	}
 
-	waitTransform(local_frame, req.frame_id, stamp, telemetry_transform_timeout);
+	try {
+		waitTransform(req.frame_id, fcu_frame, stamp, telemetry_transform_timeout);
+		auto transform = tf_buffer.lookupTransform(req.frame_id, fcu_frame, stamp);
+		res.x = transform.transform.translation.x;
+		res.y = transform.transform.translation.y;
+		res.z = transform.transform.translation.z;
 
-	if (!TIMEOUT(local_position, local_position_timeout)) {
-		try {
-			// transform pose
-			PoseStamped pose;
-			tf_buffer.transform(local_position, pose, req.frame_id);
-			res.x = pose.pose.position.x;
-			res.y = pose.pose.position.y;
-			res.z = pose.pose.position.z;
-
-			// Tait-Bryan angles, order z-y-x
-			double yaw, pitch, roll;
-			tf2::getEulerYPR(pose.pose.orientation, yaw, pitch, roll);
-			res.yaw = yaw;
-			res.pitch = pitch;
-			res.roll = roll;
-		} catch (const tf2::TransformException& e) {}
+		double yaw, pitch, roll;
+		tf2::getEulerYPR(transform.transform.rotation, yaw, pitch, roll);
+		res.yaw = yaw;
+		res.pitch = pitch;
+		res.roll = roll;
+	} catch (const tf2::TransformException& e) {
+		ROS_DEBUG("simple_offboard: %s", e.what());
 	}
 
 	if (!TIMEOUT(velocity, velocity_timeout)) {
 		try {
 			// transform velocity
+			waitTransform(req.frame_id, fcu_frame, velocity.header.stamp, telemetry_transform_timeout);
 			Vector3Stamped vec, vec_out;
-			vec.header = velocity.header;
+			vec.header.stamp = velocity.header.stamp;
+			vec.header.frame_id = velocity.header.frame_id;
 			vec.vector = velocity.twist.linear;
 			tf_buffer.transform(vec, vec_out, req.frame_id);
 
@@ -325,6 +349,10 @@ PoseStamped globalToLocal(double lat, double lon)
 	x_offset = distance * sin(azimuth_radians);
 	y_offset = distance * cos(azimuth_radians);
 
+	if (!waitTransform(local_frame, fcu_frame, global_position.header.stamp, ros::Duration(0.2))) {
+		throw std::runtime_error("No local position");
+	}
+
 	auto local = tf_buffer.lookupTransform(local_frame, fcu_frame, global_position.header.stamp);
 
 	PoseStamped pose;
@@ -363,13 +391,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);
 		}
 	}
 
@@ -454,10 +481,12 @@ inline void checkState()
 		throw std::runtime_error("No connection to FCU, https://clever.copterexpress.com/connection.html");
 }
 
+#define ENSURE_FINITE(var) { if (!std::isfinite(var)) throw std::runtime_error(#var " argument cannot be NaN or Inf"); }
+
 bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, float vy, float vz,
-           float pitch, float roll, float yaw, float pitch_rate, float roll_rate, float yaw_rate,
-           float lat, float lon, float thrust, float speed, string frame_id, bool auto_arm,
-           uint8_t& success, string& message)
+           float pitch, float roll, float yaw, float pitch_rate, float roll_rate, float yaw_rate, // editorconfig-checker-disable-line
+           float lat, float lon, float thrust, float speed, string frame_id, bool auto_arm, // editorconfig-checker-disable-line
+           uint8_t& success, string& message) // editorconfig-checker-disable-line
 {
 	auto stamp = ros::Time::now();
 
@@ -465,6 +494,20 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl
 		if (busy)
 			throw std::runtime_error("Busy");
 
+		ENSURE_FINITE(x);
+		ENSURE_FINITE(y);
+		ENSURE_FINITE(z);
+		ENSURE_FINITE(vx);
+		ENSURE_FINITE(vy);
+		ENSURE_FINITE(vz);
+		ENSURE_FINITE(pitch);
+		ENSURE_FINITE(roll);
+		ENSURE_FINITE(pitch_rate);
+		ENSURE_FINITE(roll_rate);
+		ENSURE_FINITE(lat);
+		ENSURE_FINITE(lon);
+		ENSURE_FINITE(thrust);
+
 		busy = true;
 
 		// Checks
@@ -514,7 +557,9 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl
 
 		if (sp_type == NAVIGATE_GLOBAL) {
 			// Calculate x and from lat and lot in request's frame
-			auto xy_in_req_frame = tf_buffer.transform(globalToLocal(lat, lon), frame_id);
+			auto pose_local = globalToLocal(lat, lon);
+			pose_local.header.stamp = stamp; // TODO: fix
+			auto xy_in_req_frame = tf_buffer.transform(pose_local, frame_id);
 			x = xy_in_req_frame.pose.position.x;
 			y = xy_in_req_frame.pose.position.y;
 		}
@@ -644,6 +689,12 @@ bool land(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)
 
 		checkState();
 
+		if (land_only_in_offboard) {
+			if (state.mode != "OFFBOARD") {
+				throw std::runtime_error("Copter is not in OFFBOARD mode");
+			}
+		}
+
 		static mavros_msgs::SetMode sm;
 		sm.request.custom_mode = "AUTO.LAND";
 
@@ -682,13 +733,16 @@ int main(int argc, char **argv)
 	ros::NodeHandle nh, nh_priv("~");
 
 	tf2_ros::TransformListener tf_listener(tf_buffer);
+	transform_broadcaster = std::make_shared<tf2_ros::TransformBroadcaster>();
 
 	// Params
 	nh.param<string>("mavros/local_position/tf/frame_id", local_frame, "map");
 	nh.param<string>("mavros/local_position/tf/child_frame_id", fcu_frame, "base_link");
 	nh_priv.param("target_frame", target.child_frame_id, string("navigate_target"));
 	nh_priv.param("auto_release", auto_release, true);
+	nh_priv.param("land_only_in_offboard", land_only_in_offboard, true);
 	nh_priv.param("default_speed", default_speed, 0.5f);
+	nh_priv.param<string>("body_frame", body.child_frame_id, "body");
 	nh_priv.getParam("reference_frames", reference_frames);
 
 	state_timeout = ros::Duration(nh_priv.param("state_timeout", 3.0));
@@ -709,11 +763,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 velocity_sub = nh.subscribe("mavros/local_position/velocity_body", 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/test/basic.py

@@ -0,0 +1,29 @@
+#!/usr/bin/env python
+import rospy
+import pytest
+from mavros_msgs.msg import State
+
+@pytest.fixture()
+def node():
+    return rospy.init_node('clever_test', anonymous=True)
+
+def test_state(node):
+    state = rospy.wait_for_message('mavros/state', State, timeout=10)
+    assert state.connected == False
+    assert state.armed == False
+    assert state.guided == False
+    assert state.mode == ''
+
+def test_simple_offboard_services_available():
+    rospy.wait_for_service('get_telemetry', timeout=5)
+    rospy.wait_for_service('navigate', timeout=5)
+    rospy.wait_for_service('navigate_global', timeout=5)
+    rospy.wait_for_service('set_position', timeout=5)
+    rospy.wait_for_service('set_velocity', timeout=5)
+    rospy.wait_for_service('set_attitude', timeout=5)
+    rospy.wait_for_service('set_rates', timeout=5)
+    rospy.wait_for_service('land', timeout=5)
+
+def test_web_video_server(node):
+    import urllib2
+    urllib2.urlopen("http://localhost:8080").read()

clever/test/basic.test

@@ -0,0 +1,37 @@
+<launch>
+    <!-- Verify all the required nodes basically work -->
+
+    <node pkg="mavros" type="mavros_node" name="mavros" required="true" output="screen">
+        <param name="fcu_url" value="udp://@127.0.1:14557"/>
+        <rosparam command="load" file="$(find clever)/launch/mavros_config.yaml"/>
+    </node>
+
+    <node name="visualization" pkg="mavros_extras" type="visualization" required="true">
+        <remap to="mavros/local_position/pose" from="local_position"/>
+        <remap to="mavros/setpoint_position/local" from="local_setpoint"/>
+        <param name="fixed_frame_id" value="map"/>
+        <param name="child_frame_id" value="base_link"/>
+        <param name="marker_scale" value="1"/>
+        <param name="max_track_size" value="20"/>
+        <param name="num_rotors" value="4"/>
+    </node>
+
+    <node name="web_video_server" pkg="web_video_server" type="web_video_server" required="true" output="screen">
+        <param name="default_stream_type" value="ros_compressed"/>
+        <param name="publish_rate" value="1.0"/>
+    </node>
+
+    <node pkg="tf2_ros" type="static_transform_publisher" name="map_flipped_frame" args="0 0 0 3.1415926 3.1415926 0 map map_flipped" required="true"/>
+
+    <node name="simple_offboard" pkg="clever" type="simple_offboard" required="true" output="screen">
+        <param name="reference_frames/body" value="map"/>
+        <param name="reference_frames/base_link" value="map"/>
+    </node>
+
+    <node name="tf2_web_republisher" pkg="tf2_web_republisher" type="tf2_web_republisher" required="true"/>
+
+    <node name="rc" pkg="clever" type="rc" required="true" output="screen"/>
+
+    <param name="test_module" value="$(find clever)/test/basic.py"/>
+    <test test-name="basic_test" pkg="ros_pytest" type="ros_pytest_runner"/>
+</launch>

clever/test/sitl.py

@@ -0,0 +1,197 @@
+#!/usr/bin/env python
+
+import math
+import rospy
+import pytest
+from pytest import approx
+from numpy import isfinite
+from geometry_msgs.msg import PoseStamped
+from mavros_msgs.msg import State
+from sensor_msgs.msg import NavSatFix
+from clever import srv
+from std_srvs.srv import Trigger
+
+import tf2_ros
+import tf2_geometry_msgs
+
+def roughly(expected):
+    return approx(expected, abs=1e-1)
+
+def very_roughly(expected):
+    return approx(expected, abs=1)
+
+@pytest.fixture()
+def node():
+    return rospy.init_node('clever_test', anonymous=True)
+
+def wait_service(name, service_class):
+    res = rospy.ServiceProxy(name, service_class)
+    res.wait_for_service(5)
+    return res
+
+@pytest.fixture
+def get_telemetry():
+    return wait_service('get_telemetry', srv.GetTelemetry)
+
+@pytest.fixture
+def navigate():
+    return wait_service('navigate', srv.Navigate)
+
+@pytest.fixture
+def navigate():
+    res = rospy.ServiceProxy('navigate', srv.Navigate)
+    res.wait_for_service(5)
+    return res
+
+@pytest.fixture
+def land():
+    return wait_service('land', Trigger)
+
+@pytest.fixture
+def tf_buffer():
+    buf = tf2_ros.Buffer()
+    tf2_ros.TransformListener(buf)
+    return buf
+
+def wait_connection():
+    start = rospy.get_rostime()
+    while rospy.get_rostime() - start <= rospy.Duration(15):
+        state = rospy.wait_for_message('mavros/state', State, timeout=10)
+        if state.connected:
+            return True
+    assert False, "no connection to SITL"
+
+def minimal_rate(func, rate):
+    start = rospy.get_rostime()
+    i = 0
+    while rospy.get_rostime() - start <= rospy.Duration(2):
+        func()
+        i += 1
+    result_rate = i / 2
+    assert result_rate >= rate, 'Rate too low: %s Hz' % result_rate
+
+def test_state_initial(node):
+    wait_connection()
+    state = rospy.wait_for_message('mavros/state', State, timeout=10)
+    assert state.connected == True
+    assert state.armed == False
+
+def test_telem_initial(node, get_telemetry):
+    # wait for local position
+    rospy.wait_for_message('mavros/local_position/pose', PoseStamped, timeout=15)
+    rospy.wait_for_message('mavros/global_position/global', NavSatFix, timeout=30)
+
+    telem = get_telemetry()
+    assert telem.frame_id == 'map'
+    assert telem.connected == True
+    assert telem.armed == False
+    assert telem.mode != ''
+    assert telem.x == roughly(0.0)
+    assert telem.y == roughly(0.0)
+    assert telem.z == roughly(0.0)
+    assert telem.lat == approx(47.397742)
+    assert telem.lon == approx(8.545594)
+    assert telem.vx == roughly(0.0)
+    assert telem.vy == roughly(0.0)
+    assert telem.vz == roughly(0.0)
+    assert telem.pitch == roughly(0.0)
+    assert telem.roll == roughly(0.0)
+    assert telem.yaw == roughly(1.56)
+    assert isfinite(telem.voltage)
+    assert isfinite(telem.cell_voltage)
+
+def test_telem_rate(node, get_telemetry):
+    minimal_rate(lambda: get_telemetry(), 20)
+    minimal_rate(lambda: get_telemetry(frame_id='body'), 20)
+    minimal_rate(lambda: get_telemetry(frame_id='base_link'), 200)
+
+def test_takeoff_without_auto_arm(node, navigate):
+    res = navigate(z=2, frame_id='body')
+    assert res.success == False
+    assert res.message == 'Copter is not in OFFBOARD mode, use auto_arm?'
+
+def test_takeoff(node, navigate, get_telemetry, tf_buffer):
+    res = navigate(z=2, speed=1, frame_id='body', auto_arm=True)
+    assert res.success == True, res.message
+    rospy.sleep(5)
+    telem = get_telemetry()
+    assert telem.z == very_roughly(2.0)
+    assert telem.x == very_roughly(0.0)
+    assert telem.y == very_roughly(0.0)
+    assert telem.pitch == roughly(0.0)
+    assert telem.roll == roughly(0.0)
+    assert telem.yaw == roughly(1.56)
+
+def test_navigate_nans(node, navigate):
+    res = navigate(x=float('nan'))
+    assert res.success == False
+    assert res.message == 'x argument cannot be NaN or Inf'
+    res = navigate(y=float('nan'))
+    assert res.success == False
+    assert res.message == 'y argument cannot be NaN or Inf'
+    res = navigate(z=float('nan'))
+    assert res.success == False
+    assert res.message == 'z argument cannot be NaN or Inf'
+    res = navigate(x=float('inf'))
+    assert res.success == False
+    assert res.message == 'x argument cannot be NaN or Inf'
+    res = navigate(y=float('inf'))
+    assert res.success == False
+    assert res.message == 'y argument cannot be NaN or Inf'
+    res = navigate(z=float('inf'))
+    assert res.success == False
+    assert res.message == 'z argument cannot be NaN or Inf'
+    res = navigate(x=float('-inf'))
+    assert res.success == False
+    assert res.message == 'x argument cannot be NaN or Inf'
+    res = navigate(y=float('-inf'))
+    assert res.success == False
+    assert res.message == 'y argument cannot be NaN or Inf'
+    res = navigate(z=float('-inf'))
+    assert res.success == False
+    assert res.message == 'z argument cannot be NaN or Inf'
+
+def test_navigate(node, navigate, get_telemetry, tf_buffer):
+    res = navigate(x=1, y=2, z=3, speed=1)
+    assert res.success == True, res.message
+    nav_target = tf_buffer.lookup_transform('map', 'navigate_target', rospy.get_rostime(), rospy.Duration(0.2))
+    assert nav_target.transform.translation.x == approx(1)
+    assert nav_target.transform.translation.y == approx(2)
+    assert nav_target.transform.translation.z == approx(3)
+    rospy.sleep(10)
+    telem = get_telemetry()
+    assert telem.x == very_roughly(1.0)
+    assert telem.y == very_roughly(2.0)
+    assert telem.z == very_roughly(3.0)
+    assert telem.pitch == roughly(0.0)
+    assert telem.roll == roughly(0.0)
+    assert telem.yaw == roughly(0.0)
+
+    res = navigate(x=-1, y=-2, z=-1, frame_id='body', speed=1)
+    assert res.success == True, res.message
+    nav_target = tf_buffer.lookup_transform('map', 'navigate_target', rospy.get_rostime(), rospy.Duration(0.2))
+    assert nav_target.transform.translation.x == very_roughly(0)
+    assert nav_target.transform.translation.y == very_roughly(0)
+    assert nav_target.transform.translation.z == very_roughly(2)
+    rospy.sleep(10)
+    telem = get_telemetry()
+    assert telem.x == very_roughly(0)
+    assert telem.y == very_roughly(0)
+    assert telem.z == very_roughly(2)
+    assert telem.pitch == roughly(0)
+    assert telem.roll == roughly(0)
+    assert telem.yaw == roughly(0)
+
+# TODO
+# test navigate_global, set_velocity, set_attitude, set_rates
+
+def test_land(node, get_telemetry, land):
+    res = land()
+    assert res.success, res.message
+    telem = get_telemetry()
+    assert telem.mode == 'AUTO.LAND'
+    assert telem.armed == True, 'Drone unexpectedly disarmed while landing'
+    rospy.sleep(12)
+    telem = get_telemetry()
+    assert telem.mode == 'AUTO.LAND'
+    assert telem.armed == False, 'Drone is not disarmed after landing'

clever/test/sitl.test

@@ -0,0 +1,20 @@
+<launch>
+    <arg name="ip" default="127.0.0.1"/>
+
+    <!-- mavros -->
+    <include file="$(find clever)/launch/mavros.launch">
+        <arg name="fcu_conn" value="udp"/>
+        <arg name="fcu_ip" value="$(arg ip)"/>
+        <arg name="gcs_bridge" value="false"/>
+        <arg name="viz" value="false"/>
+        <arg name="respawn" default="false"/>
+    </include>
+
+    <node name="simple_offboard" pkg="clever" type="simple_offboard" required="true" output="screen">
+        <param name="reference_frames/body" value="map"/>
+        <param name="reference_frames/base_link" value="map"/>
+    </node>
+
+    <param name="test_module" value="$(find clever)/test/sitl.py"/>
+    <test test-name="basic_test" pkg="ros_pytest" type="ros_pytest_runner" time-limit="120.0"/>
+</launch>

clever/www/js/gcs.js

@@ -3,7 +3,7 @@ var titleEl = document.querySelector('title');
 var modeEl = document.querySelector('.mode');
 var batteryEl = document.querySelector('.battery');
 
-var url = 'ws://' + location.host + ':9090';
+var url = 'ws://' + location.hostname + ':9090';
 var ros = new ROSLIB.Ros({ url: url });
 
 function speak(txt) {

clever/www/js/viz.js

@@ -1,5 +1,5 @@
 var ros = new ROSLIB.Ros({
-	url : 'ws://' + location.host + ':9090'
+	url : 'ws://' + location.hostname + ':9090'
 });
 
 var titleEl = document.querySelector('title');

docs/assets/Clever2LPE_160118.params

@@ -1,735 +0,0 @@
-# Onboard parameters for Vehicle 1
-#
-# Stack: PX4 Pro
-# Vehicle: Multi-Rotor
-# Version: 1.7.0 
-# Git Revision: cbdb08bb6109c1ea
-#
-# Vehicle-Id Component-Id Name Value Type
-1	1	ATT_ACC_COMP	1	6
-1	1	ATT_BIAS_MAX	0.050000000745058060	9
-1	1	ATT_EXT_HDG_M	0	6
-1	1	ATT_MAG_DECL	0.000000000000000000	9
-1	1	ATT_MAG_DECL_A	1	6
-1	1	ATT_W_ACC	0.200000002980232239	9
-1	1	ATT_W_EXT_HDG	0.100000001490116119	9
-1	1	ATT_W_GYRO_BIAS	0.100000001490116119	9
-1	1	ATT_W_MAG	0.100000001490116119	9
-1	1	BAT_A_PER_V	15.391030311584472656	9
-1	1	BAT_CAPACITY	-1.000000000000000000	9
-1	1	BAT_CNT_V_CURR	0.000805664050858468	9
-1	1	BAT_CNT_V_VOLT	0.000805664050858468	9
-1	1	BAT_CRIT_THR	0.070000000298023224	9
-1	1	BAT_EMERGEN_THR	0.050000000745058060	9
-1	1	BAT_LOW_THR	0.150000005960464478	9
-1	1	BAT_N_CELLS	4	6
-1	1	BAT_R_INTERNAL	-1.000000000000000000	9
-1	1	BAT_SOURCE	0	6
-1	1	BAT_V_CHARGED	4.199999809265136719	9
-1	1	BAT_V_DIV	10.887768745422363281	9
-1	1	BAT_V_EMPTY	3.400000095367431641	9
-1	1	BAT_V_LOAD_DROP	0.300000011920928955	9
-1	1	BAT_V_OFFS_CURR	0.000000000000000000	9
-1	1	CAL_ACC0_EN	1	6
-1	1	CAL_ACC0_ID	1246218	6
-1	1	CAL_ACC0_XOFF	0.081070423126220703	9
-1	1	CAL_ACC0_XSCALE	0.996176421642303467	9
-1	1	CAL_ACC0_YOFF	-0.182177066802978516	9
-1	1	CAL_ACC0_YSCALE	1.000774979591369629	9
-1	1	CAL_ACC0_ZOFF	0.506614208221435547	9
-1	1	CAL_ACC0_ZSCALE	0.991645038127899170	9
-1	1	CAL_ACC1_EN	1	6
-1	1	CAL_ACC1_ID	1114634	6
-1	1	CAL_ACC1_XOFF	0.864228248596191406	9
-1	1	CAL_ACC1_XSCALE	1.004844427108764648	9
-1	1	CAL_ACC1_YOFF	0.980560302734375000	9
-1	1	CAL_ACC1_YSCALE	0.980703771114349365	9
-1	1	CAL_ACC1_ZOFF	0.873121738433837891	9
-1	1	CAL_ACC1_ZSCALE	0.989114820957183838	9
-1	1	CAL_ACC2_XOFF	0.000000000000000000	9
-1	1	CAL_ACC2_XSCALE	1.000000000000000000	9
-1	1	CAL_ACC2_YOFF	0.000000000000000000	9
-1	1	CAL_ACC2_YSCALE	1.000000000000000000	9
-1	1	CAL_ACC2_ZOFF	0.000000000000000000	9
-1	1	CAL_ACC2_ZSCALE	1.000000000000000000	9
-1	1	CAL_AIR_CMODEL	0	6
-1	1	CAL_AIR_TUBED_MM	1.500000000000000000	9
-1	1	CAL_AIR_TUBELEN	0.200000002980232239	9
-1	1	CAL_GYRO0_EN	1	6
-1	1	CAL_GYRO0_ID	2163722	6
-1	1	CAL_GYRO0_XOFF	0.013700588606297970	9
-1	1	CAL_GYRO0_XSCALE	1.000000000000000000	9
-1	1	CAL_GYRO0_YOFF	0.049494847655296326	9
-1	1	CAL_GYRO0_YSCALE	1.000000000000000000	9
-1	1	CAL_GYRO0_ZOFF	-0.032555881887674332	9
-1	1	CAL_GYRO0_ZSCALE	1.000000000000000000	9
-1	1	CAL_GYRO1_EN	1	6
-1	1	CAL_GYRO1_ID	2228490	6
-1	1	CAL_GYRO1_XOFF	-0.007464688271284103	9
-1	1	CAL_GYRO1_XSCALE	1.000000000000000000	9
-1	1	CAL_GYRO1_YOFF	0.008172192610800266	9
-1	1	CAL_GYRO1_YSCALE	1.000000000000000000	9
-1	1	CAL_GYRO1_ZOFF	-0.010663406923413277	9
-1	1	CAL_GYRO1_ZSCALE	1.000000000000000000	9
-1	1	CAL_GYRO2_XOFF	0.000000000000000000	9
-1	1	CAL_GYRO2_XSCALE	1.000000000000000000	9
-1	1	CAL_GYRO2_YOFF	0.000000000000000000	9
-1	1	CAL_GYRO2_YSCALE	1.000000000000000000	9
-1	1	CAL_GYRO2_ZOFF	0.000000000000000000	9
-1	1	CAL_GYRO2_ZSCALE	1.000000000000000000	9
-1	1	CAL_MAG0_EN	1	6
-1	1	CAL_MAG0_ID	131594	6
-1	1	CAL_MAG0_ROT	-1	6
-1	1	CAL_MAG0_XOFF	-0.020210459828376770	9
-1	1	CAL_MAG0_XSCALE	1.033145427703857422	9
-1	1	CAL_MAG0_YOFF	0.100117556750774384	9
-1	1	CAL_MAG0_YSCALE	1.017574071884155273	9
-1	1	CAL_MAG0_ZOFF	-0.113783776760101318	9
-1	1	CAL_MAG0_ZSCALE	0.933741152286529541	9
-1	1	CAL_MAG1_ID	0	6
-1	1	CAL_MAG1_ROT	-1	6
-1	1	CAL_MAG1_XOFF	0.000000000000000000	9
-1	1	CAL_MAG1_XSCALE	1.000000000000000000	9
-1	1	CAL_MAG1_YOFF	0.000000000000000000	9
-1	1	CAL_MAG1_YSCALE	1.000000000000000000	9
-1	1	CAL_MAG1_ZOFF	0.000000000000000000	9
-1	1	CAL_MAG1_ZSCALE	1.000000000000000000	9
-1	1	CAL_MAG2_ID	0	6
-1	1	CAL_MAG2_ROT	-1	6
-1	1	CAL_MAG2_XOFF	0.000000000000000000	9
-1	1	CAL_MAG2_XSCALE	1.000000000000000000	9
-1	1	CAL_MAG2_YOFF	0.000000000000000000	9
-1	1	CAL_MAG2_YSCALE	1.000000000000000000	9
-1	1	CAL_MAG2_ZOFF	0.000000000000000000	9
-1	1	CAL_MAG2_ZSCALE	1.000000000000000000	9
-1	1	CAL_MAG3_ID	0	6
-1	1	CAL_MAG3_ROT	-1	6
-1	1	CAL_MAG3_XOFF	0.000000000000000000	9
-1	1	CAL_MAG3_XSCALE	1.000000000000000000	9
-1	1	CAL_MAG3_YOFF	0.000000000000000000	9
-1	1	CAL_MAG3_YSCALE	1.000000000000000000	9
-1	1	CAL_MAG3_ZOFF	0.000000000000000000	9
-1	1	CAL_MAG3_ZSCALE	1.000000000000000000	9
-1	1	CAL_MAG_SIDES	63	6
-1	1	CBRK_AIRSPD_CHK	0	6
-1	1	CBRK_ENGINEFAIL	284953	6
-1	1	CBRK_FLIGHTTERM	121212	6
-1	1	CBRK_GPSFAIL	240024	6
-1	1	CBRK_IO_SAFETY	22027	6
-1	1	CBRK_RATE_CTRL	0	6
-1	1	CBRK_SUPPLY_CHK	894281	6
-1	1	CBRK_USB_CHK	197848	6
-1	1	CBRK_VELPOSERR	0	6
-1	1	COM_ARM_AUTH	256010	6
-1	1	COM_ARM_EKF_AB	0.004999999888241291	9
-1	1	COM_ARM_EKF_GB	0.000869999988935888	9
-1	1	COM_ARM_EKF_HGT	1.000000000000000000	9
-1	1	COM_ARM_EKF_POS	0.500000000000000000	9
-1	1	COM_ARM_EKF_VEL	0.500000000000000000	9
-1	1	COM_ARM_EKF_YAW	0.500000000000000000	9
-1	1	COM_ARM_IMU_ACC	0.699999988079071045	9
-1	1	COM_ARM_IMU_GYR	0.200000002980232239	9
-1	1	COM_ARM_MIS_REQ	0	6
-1	1	COM_ARM_SWISBTN	0	6
-1	1	COM_ARM_WO_GPS	1	6
-1	1	COM_DISARM_LAND	1	6
-1	1	COM_DL_LOSS_T	10	6
-1	1	COM_DL_REG_T	0	6
-1	1	COM_EF_C2T	5.000000000000000000	9
-1	1	COM_EF_THROT	0.500000000000000000	9
-1	1	COM_EF_TIME	10.000000000000000000	9
-1	1	COM_FLIGHT_UUID	34	6
-1	1	COM_FLTMODE1	8	6
-1	1	COM_FLTMODE2	-1	6
-1	1	COM_FLTMODE3	-1	6
-1	1	COM_FLTMODE4	1	6
-1	1	COM_FLTMODE5	-1	6
-1	1	COM_FLTMODE6	2	6
-1	1	COM_HOME_H_T	5.000000000000000000	9
-1	1	COM_HOME_V_T	10.000000000000000000	9
-1	1	COM_LOW_BAT_ACT	2	6
-1	1	COM_OBL_ACT	0	6
-1	1	COM_OBL_RC_ACT	0	6
-1	1	COM_OF_LOSS_T	0.000000000000000000	9
-1	1	COM_POSCTL_NAVL	0	6
-1	1	COM_POS_FS_DELAY	1	6
-1	1	COM_POS_FS_GAIN	10	6
-1	1	COM_POS_FS_PROB	30	6
-1	1	COM_RC_ARM_HYST	1000	6
-1	1	COM_RC_IN_MODE	0	6
-1	1	COM_RC_LOSS_T	0.500000000000000000	9
-1	1	COM_RC_OVERRIDE	0	6
-1	1	COM_RC_STICK_OV	12.000000000000000000	9
-1	1	FW_CLMBOUT_DIFF	10.000000000000000000	9
-1	1	FW_MAN_P_SC	1.000000000000000000	9
-1	1	FW_MAN_R_SC	1.000000000000000000	9
-1	1	FW_MAN_Y_SC	1.000000000000000000	9
-1	1	GF_ACTION	1	6
-1	1	GF_ALTMODE	0	6
-1	1	GF_COUNT	-1	6
-1	1	GF_MAX_HOR_DIST	0.000000000000000000	9
-1	1	GF_MAX_VER_DIST	0.000000000000000000	9
-1	1	GF_SOURCE	0	6
-1	1	GPS_DUMP_COMM	0	6
-1	1	GPS_UBX_DYNMODEL	7	6
-1	1	IMU_ACCEL_CUTOFF	30.000000000000000000	9
-1	1	IMU_GYRO_CUTOFF	30.000000000000000000	9
-1	1	LED_RGB_MAXBRT	15	6
-1	1	LNDMC_ALT_MAX	10000.000000000000000000	9
-1	1	LNDMC_FFALL_THR	2.000000000000000000	9
-1	1	LNDMC_FFALL_TTRI	0.300000011920928955	9
-1	1	LNDMC_ROT_MAX	20.000000000000000000	9
-1	1	LNDMC_THR_RANGE	0.100000001490116119	9
-1	1	LNDMC_XY_VEL_MAX	1.500000000000000000	9
-1	1	LNDMC_Z_VEL_MAX	0.500000000000000000	9
-1	1	LND_FLIGHT_T_HI	0	6
-1	1	LND_FLIGHT_T_LO	959960540	6
-1	1	LPE_ACC_XY	0.012000000104308128	9
-1	1	LPE_ACC_Z	0.019999999552965164	9
-1	1	LPE_BAR_Z	3.000000000000000000	9
-1	1	LPE_EPH_MAX	3.000000000000000000	9
-1	1	LPE_EPV_MAX	5.000000000000000000	9
-1	1	LPE_FAKE_ORIGIN	1	6
-1	1	LPE_FGYRO_HP	0.001000000047497451	9
-1	1	LPE_FLW_OFF_Z	0.000000000000000000	9
-1	1	LPE_FLW_QMIN	150	6
-1	1	LPE_FLW_R	7.000000000000000000	9
-1	1	LPE_FLW_RR	7.000000000000000000	9
-1	1	LPE_FLW_SCALE	1.299999952316284180	9
-1	1	LPE_FUSION	28	6
-1	1	LPE_GPS_DELAY	0.289999991655349731	9
-1	1	LPE_GPS_VXY	0.250000000000000000	9
-1	1	LPE_GPS_VZ	0.250000000000000000	9
-1	1	LPE_GPS_XY	1.000000000000000000	9
-1	1	LPE_GPS_Z	3.000000000000000000	9
-1	1	LPE_LAND_VXY	0.050000000745058060	9
-1	1	LPE_LAND_Z	0.029999999329447746	9
-1	1	LPE_LAT	47.397743225097656250	9
-1	1	LPE_LDR_OFF_Z	0.000000000000000000	9
-1	1	LPE_LDR_Z	0.029999999329447746	9
-1	1	LPE_LON	8.545594215393066406	9
-1	1	LPE_PN_B	0.001000000047497451	9
-1	1	LPE_PN_P	0.100000001490116119	9
-1	1	LPE_PN_T	0.001000000047497451	9
-1	1	LPE_PN_V	0.100000001490116119	9
-1	1	LPE_SNR_OFF_Z	0.000000000000000000	9
-1	1	LPE_SNR_Z	0.050000000745058060	9
-1	1	LPE_T_MAX_GRADE	1.000000000000000000	9
-1	1	LPE_VIC_P	0.001000000047497451	9
-1	1	LPE_VIS_DELAY	0.000000000000000000	9
-1	1	LPE_VIS_XY	0.100000001490116119	9
-1	1	LPE_VIS_Z	0.100000001490116119	9
-1	1	LPE_VXY_PUB	0.300000011920928955	9
-1	1	LPE_X_LP	5.000000000000000000	9
-1	1	LPE_Z_PUB	1.000000000000000000	9
-1	1	MAV_BROADCAST	0	6
-1	1	MAV_COMP_ID	1	6
-1	1	MAV_FWDEXTSP	1	6
-1	1	MAV_PROTO_VER	0	6
-1	1	MAV_RADIO_ID	0	6
-1	1	MAV_SYS_ID	1	6
-1	1	MAV_TEST_PAR	1	6
-1	1	MAV_TYPE	2	6
-1	1	MAV_USEHILGPS	0	6
-1	1	MC_ACRO_EXPO	0.689999997615814209	9
-1	1	MC_ACRO_P_MAX	360.000000000000000000	9
-1	1	MC_ACRO_R_MAX	360.000000000000000000	9
-1	1	MC_ACRO_SUPEXPO	0.699999988079071045	9
-1	1	MC_ACRO_Y_MAX	360.000000000000000000	9
-1	1	MC_BAT_SCALE_EN	0	6
-1	1	MC_PITCHRATE_D	0.003000000026077032	9
-1	1	MC_PITCHRATE_FF	0.000000000000000000	9
-1	1	MC_PITCHRATE_I	0.045000001788139343	9
-1	1	MC_PITCHRATE_MAX	220.000000000000000000	9
-1	1	MC_PITCHRATE_P	0.144999995827674866	9
-1	1	MC_PITCH_P	6.500000000000000000	9
-1	1	MC_PITCH_TC	0.200000002980232239	9
-1	1	MC_PR_INT_LIM	0.300000011920928955	9
-1	1	MC_RATT_TH	0.800000011920928955	9
-1	1	MC_ROLLRATE_D	0.003000000026077032	9
-1	1	MC_ROLLRATE_FF	0.000000000000000000	9
-1	1	MC_ROLLRATE_I	0.045000001788139343	9
-1	1	MC_ROLLRATE_MAX	220.000000000000000000	9
-1	1	MC_ROLLRATE_P	0.144999995827674866	9
-1	1	MC_ROLL_P	6.500000000000000000	9
-1	1	MC_ROLL_TC	0.200000002980232239	9
-1	1	MC_RR_INT_LIM	0.300000011920928955	9
-1	1	MC_TPA_BREAK_D	1.000000000000000000	9
-1	1	MC_TPA_BREAK_I	1.000000000000000000	9
-1	1	MC_TPA_BREAK_P	1.000000000000000000	9
-1	1	MC_TPA_RATE_D	0.000000000000000000	9
-1	1	MC_TPA_RATE_I	0.000000000000000000	9
-1	1	MC_TPA_RATE_P	0.000000000000000000	9
-1	1	MC_YAWRATE_D	0.000000000000000000	9
-1	1	MC_YAWRATE_FF	0.000000000000000000	9
-1	1	MC_YAWRATE_I	0.100000001490116119	9
-1	1	MC_YAWRATE_MAX	200.000000000000000000	9
-1	1	MC_YAWRATE_P	0.200000002980232239	9
-1	1	MC_YAWRAUTO_MAX	45.000000000000000000	9
-1	1	MC_YAW_FF	0.500000000000000000	9
-1	1	MC_YAW_P	2.799999952316284180	9
-1	1	MC_YR_INT_LIM	0.300000011920928955	9
-1	1	MIS_ALTMODE	1	6
-1	1	MIS_DIST_1WP	900.000000000000000000	9
-1	1	MIS_DIST_WPS	900.000000000000000000	9
-1	1	MIS_LTRMIN_ALT	-1.000000000000000000	9
-1	1	MIS_ONBOARD_EN	1	6
-1	1	MIS_TAKEOFF_ALT	2.500000000000000000	9
-1	1	MIS_YAWMODE	1	6
-1	1	MIS_YAW_ERR	12.000000000000000000	9
-1	1	MIS_YAW_TMT	-1.000000000000000000	9
-1	1	MNT_MODE_IN	-1	6
-1	1	MOT_SLEW_MAX	0.000000000000000000	9
-1	1	MPC_ACC_DOWN_MAX	5.000000000000000000	9
-1	1	MPC_ACC_HOR	5.000000000000000000	9
-1	1	MPC_ACC_HOR_MAX	5.000000000000000000	9
-1	1	MPC_ACC_UP_MAX	5.000000000000000000	9
-1	1	MPC_ALT_MODE	0	6
-1	1	MPC_CRUISE_90	3.000000000000000000	9
-1	1	MPC_DEC_HOR_SLOW	5.000000000000000000	9
-1	1	MPC_HOLD_DZ	0.100000001490116119	9
-1	1	MPC_HOLD_MAX_XY	0.800000011920928955	9
-1	1	MPC_HOLD_MAX_Z	0.600000023841857910	9
-1	1	MPC_JERK_MAX	0.000000000000000000	9
-1	1	MPC_JERK_MIN	1.000000000000000000	9
-1	1	MPC_LAND_ALT1	10.000000000000000000	9
-1	1	MPC_LAND_ALT2	5.000000000000000000	9
-1	1	MPC_LAND_SPEED	0.500000000000000000	9
-1	1	MPC_MANTHR_MAX	0.899999976158142090	9
-1	1	MPC_MANTHR_MIN	0.079999998211860657	9
-1	1	MPC_MAN_TILT_MAX	35.000000000000000000	9
-1	1	MPC_MAN_Y_MAX	200.000000000000000000	9
-1	1	MPC_THR_HOVER	0.500000000000000000	9
-1	1	MPC_THR_MAX	0.899999976158142090	9
-1	1	MPC_THR_MIN	0.119999997317790985	9
-1	1	MPC_TILTMAX_AIR	45.000000000000000000	9
-1	1	MPC_TILTMAX_LND	12.000000000000000000	9
-1	1	MPC_TKO_RAMP_T	0.400000005960464478	9
-1	1	MPC_TKO_SPEED	1.500000000000000000	9
-1	1	MPC_VELD_LP	5.000000000000000000	9
-1	1	MPC_VEL_MANUAL	10.000000000000000000	9
-1	1	MPC_XY_CRUISE	5.000000000000000000	9
-1	1	MPC_XY_MAN_EXPO	0.000000000000000000	9
-1	1	MPC_XY_P	0.949999988079071045	9
-1	1	MPC_XY_VEL_D	0.009999999776482582	9
-1	1	MPC_XY_VEL_I	0.019999999552965164	9
-1	1	MPC_XY_VEL_MAX	8.000000000000000000	9
-1	1	MPC_XY_VEL_P	0.090000003576278687	9
-1	1	MPC_Z_MAN_EXPO	0.000000000000000000	9
-1	1	MPC_Z_P	1.000000000000000000	9
-1	1	MPC_Z_VEL_D	0.000000000000000000	9
-1	1	MPC_Z_VEL_I	0.019999999552965164	9
-1	1	MPC_Z_VEL_MAX_DN	1.000000000000000000	9
-1	1	MPC_Z_VEL_MAX_UP	3.000000000000000000	9
-1	1	MPC_Z_VEL_P	0.200000002980232239	9
-1	1	NAV_ACC_RAD	2.000000000000000000	9
-1	1	NAV_AH_ALT	600.000000000000000000	9
-1	1	NAV_AH_LAT	-265847810	6
-1	1	NAV_AH_LON	1518423250	6
-1	1	NAV_DLL_ACT	0	6
-1	1	NAV_DLL_AH_T	120.000000000000000000	9
-1	1	NAV_DLL_CHSK	0	6
-1	1	NAV_DLL_CH_ALT	600.000000000000000000	9
-1	1	NAV_DLL_CH_LAT	-266072120	6
-1	1	NAV_DLL_CH_LON	1518453890	6
-1	1	NAV_DLL_CH_T	120.000000000000000000	9
-1	1	NAV_DLL_N	2	6
-1	1	NAV_FORCE_VT	1	6
-1	1	NAV_FT_DST	8.000000000000000000	9
-1	1	NAV_FT_FS	1	6
-1	1	NAV_FT_RS	0.500000000000000000	9
-1	1	NAV_FW_ALT_RAD	10.000000000000000000	9
-1	1	NAV_GPSF_LT	30.000000000000000000	9
-1	1	NAV_GPSF_P	0.000000000000000000	9
-1	1	NAV_GPSF_R	15.000000000000000000	9
-1	1	NAV_GPSF_TR	0.699999988079071045	9
-1	1	NAV_LOITER_RAD	50.000000000000000000	9
-1	1	NAV_MC_ALT_RAD	0.800000011920928955	9
-1	1	NAV_MIN_FT_HT	8.000000000000000000	9
-1	1	NAV_RCL_ACT	3	6
-1	1	NAV_RCL_LT	120.000000000000000000	9
-1	1	NAV_TRAFF_AVOID	1	6
-1	1	PWM_AUX_DIS1	-1	6
-1	1	PWM_AUX_DIS2	-1	6
-1	1	PWM_AUX_DIS3	-1	6
-1	1	PWM_AUX_DIS4	-1	6
-1	1	PWM_AUX_DIS5	-1	6
-1	1	PWM_AUX_DIS6	-1	6
-1	1	PWM_AUX_DISARMED	1500	6
-1	1	PWM_AUX_MAX	2000	6
-1	1	PWM_AUX_MIN	1000	6
-1	1	PWM_AUX_REV1	0	6
-1	1	PWM_AUX_REV2	0	6
-1	1	PWM_AUX_REV3	0	6
-1	1	PWM_AUX_REV4	0	6
-1	1	PWM_AUX_REV5	0	6
-1	1	PWM_AUX_REV6	0	6
-1	1	PWM_AUX_TRIM1	0.000000000000000000	9
-1	1	PWM_AUX_TRIM2	0.000000000000000000	9
-1	1	PWM_AUX_TRIM3	0.000000000000000000	9
-1	1	PWM_AUX_TRIM4	0.000000000000000000	9
-1	1	PWM_AUX_TRIM5	0.000000000000000000	9
-1	1	PWM_AUX_TRIM6	0.000000000000000000	9
-1	1	PWM_DISARMED	900	6
-1	1	PWM_MAIN_DIS1	-1	6
-1	1	PWM_MAIN_DIS2	-1	6
-1	1	PWM_MAIN_DIS3	-1	6
-1	1	PWM_MAIN_DIS4	-1	6
-1	1	PWM_MAIN_DIS5	-1	6
-1	1	PWM_MAIN_DIS6	-1	6
-1	1	PWM_MAIN_DIS7	-1	6
-1	1	PWM_MAIN_DIS8	-1	6
-1	1	PWM_MAIN_REV1	0	6
-1	1	PWM_MAIN_REV2	0	6
-1	1	PWM_MAIN_REV3	0	6
-1	1	PWM_MAIN_REV4	0	6
-1	1	PWM_MAIN_REV5	0	6
-1	1	PWM_MAIN_REV6	0	6
-1	1	PWM_MAIN_REV7	0	6
-1	1	PWM_MAIN_REV8	0	6
-1	1	PWM_MAIN_TRIM1	0.000000000000000000	9
-1	1	PWM_MAIN_TRIM2	0.000000000000000000	9
-1	1	PWM_MAIN_TRIM3	0.000000000000000000	9
-1	1	PWM_MAIN_TRIM4	0.000000000000000000	9
-1	1	PWM_MAIN_TRIM5	0.000000000000000000	9
-1	1	PWM_MAIN_TRIM6	0.000000000000000000	9
-1	1	PWM_MAIN_TRIM7	0.000000000000000000	9
-1	1	PWM_MAIN_TRIM8	0.000000000000000000	9
-1	1	PWM_MAX	1950	6
-1	1	PWM_MIN	1075	6
-1	1	PWM_RATE	400	6
-1	1	PWM_SBUS_MODE	0	6
-1	1	RC10_DZ	0.000000000000000000	9
-1	1	RC10_MAX	2000.000000000000000000	9
-1	1	RC10_MIN	1000.000000000000000000	9
-1	1	RC10_REV	1.000000000000000000	9
-1	1	RC10_TRIM	1500.000000000000000000	9
-1	1	RC11_DZ	0.000000000000000000	9
-1	1	RC11_MAX	2000.000000000000000000	9
-1	1	RC11_MIN	1000.000000000000000000	9
-1	1	RC11_REV	1.000000000000000000	9
-1	1	RC11_TRIM	1500.000000000000000000	9
-1	1	RC12_DZ	0.000000000000000000	9
-1	1	RC12_MAX	2000.000000000000000000	9
-1	1	RC12_MIN	1000.000000000000000000	9
-1	1	RC12_REV	1.000000000000000000	9
-1	1	RC12_TRIM	1500.000000000000000000	9
-1	1	RC13_DZ	0.000000000000000000	9
-1	1	RC13_MAX	2000.000000000000000000	9
-1	1	RC13_MIN	1000.000000000000000000	9
-1	1	RC13_REV	1.000000000000000000	9
-1	1	RC13_TRIM	1500.000000000000000000	9
-1	1	RC14_DZ	0.000000000000000000	9
-1	1	RC14_MAX	2000.000000000000000000	9
-1	1	RC14_MIN	1000.000000000000000000	9
-1	1	RC14_REV	1.000000000000000000	9
-1	1	RC14_TRIM	1500.000000000000000000	9
-1	1	RC15_DZ	0.000000000000000000	9
-1	1	RC15_MAX	2000.000000000000000000	9
-1	1	RC15_MIN	1000.000000000000000000	9
-1	1	RC15_REV	1.000000000000000000	9
-1	1	RC15_TRIM	1500.000000000000000000	9
-1	1	RC16_DZ	0.000000000000000000	9
-1	1	RC16_MAX	2000.000000000000000000	9
-1	1	RC16_MIN	1000.000000000000000000	9
-1	1	RC16_REV	1.000000000000000000	9
-1	1	RC16_TRIM	1500.000000000000000000	9
-1	1	RC17_DZ	0.000000000000000000	9
-1	1	RC17_MAX	2000.000000000000000000	9
-1	1	RC17_MIN	1000.000000000000000000	9
-1	1	RC17_REV	1.000000000000000000	9
-1	1	RC17_TRIM	1500.000000000000000000	9
-1	1	RC18_DZ	0.000000000000000000	9
-1	1	RC18_MAX	2000.000000000000000000	9
-1	1	RC18_MIN	1000.000000000000000000	9
-1	1	RC18_REV	1.000000000000000000	9
-1	1	RC18_TRIM	1500.000000000000000000	9
-1	1	RC1_DZ	10.000000000000000000	9
-1	1	RC1_MAX	1999.000000000000000000	9
-1	1	RC1_MIN	1000.000000000000000000	9
-1	1	RC1_REV	1.000000000000000000	9
-1	1	RC1_TRIM	1499.000000000000000000	9
-1	1	RC2_DZ	10.000000000000000000	9
-1	1	RC2_MAX	2000.000000000000000000	9
-1	1	RC2_MIN	1001.000000000000000000	9
-1	1	RC2_REV	1.000000000000000000	9
-1	1	RC2_TRIM	1499.000000000000000000	9
-1	1	RC3_DZ	10.000000000000000000	9
-1	1	RC3_MAX	1989.000000000000000000	9
-1	1	RC3_MIN	1000.000000000000000000	9
-1	1	RC3_REV	1.000000000000000000	9
-1	1	RC3_TRIM	1000.000000000000000000	9
-1	1	RC4_DZ	10.000000000000000000	9
-1	1	RC4_MAX	1999.000000000000000000	9
-1	1	RC4_MIN	1018.000000000000000000	9
-1	1	RC4_REV	1.000000000000000000	9
-1	1	RC4_TRIM	1501.000000000000000000	9
-1	1	RC5_DZ	10.000000000000000000	9
-1	1	RC5_MAX	2000.000000000000000000	9
-1	1	RC5_MIN	1000.000000000000000000	9
-1	1	RC5_REV	1.000000000000000000	9
-1	1	RC5_TRIM	1500.000000000000000000	9
-1	1	RC6_DZ	10.000000000000000000	9
-1	1	RC6_MAX	2000.000000000000000000	9
-1	1	RC6_MIN	1000.000000000000000000	9
-1	1	RC6_REV	1.000000000000000000	9
-1	1	RC6_TRIM	1500.000000000000000000	9
-1	1	RC7_DZ	10.000000000000000000	9
-1	1	RC7_MAX	2000.000000000000000000	9
-1	1	RC7_MIN	1000.000000000000000000	9
-1	1	RC7_REV	1.000000000000000000	9
-1	1	RC7_TRIM	1500.000000000000000000	9
-1	1	RC8_DZ	10.000000000000000000	9
-1	1	RC8_MAX	2000.000000000000000000	9
-1	1	RC8_MIN	1000.000000000000000000	9
-1	1	RC8_REV	1.000000000000000000	9
-1	1	RC8_TRIM	1500.000000000000000000	9
-1	1	RC9_DZ	0.000000000000000000	9
-1	1	RC9_MAX	2000.000000000000000000	9
-1	1	RC9_MIN	1000.000000000000000000	9
-1	1	RC9_REV	1.000000000000000000	9
-1	1	RC9_TRIM	1500.000000000000000000	9
-1	1	RC_ACRO_TH	0.500000000000000000	9
-1	1	RC_ARMSWITCH_TH	0.250000000000000000	9
-1	1	RC_ASSIST_TH	0.250000000000000000	9
-1	1	RC_AUTO_TH	0.750000000000000000	9
-1	1	RC_CHAN_CNT	8	6
-1	1	RC_FAILS_THR	0	6
-1	1	RC_FLT_CUTOFF	10.000000000000000000	9
-1	1	RC_FLT_SMP_RATE	50.000000000000000000	9
-1	1	RC_GEAR_TH	0.250000000000000000	9
-1	1	RC_KILLSWITCH_TH	0.250000000000000000	9
-1	1	RC_LOITER_TH	0.500000000000000000	9
-1	1	RC_MAN_TH	0.500000000000000000	9
-1	1	RC_MAP_ACRO_SW	0	6
-1	1	RC_MAP_ARM_SW	0	6
-1	1	RC_MAP_AUX1	0	6
-1	1	RC_MAP_AUX2	0	6
-1	1	RC_MAP_AUX3	0	6
-1	1	RC_MAP_AUX4	0	6
-1	1	RC_MAP_AUX5	0	6
-1	1	RC_MAP_FAILSAFE	0	6
-1	1	RC_MAP_FLAPS	0	6
-1	1	RC_MAP_FLTMODE	5	6
-1	1	RC_MAP_GEAR_SW	0	6
-1	1	RC_MAP_KILL_SW	6	6
-1	1	RC_MAP_LOITER_SW	0	6
-1	1	RC_MAP_MAN_SW	0	6
-1	1	RC_MAP_MODE_SW	0	6
-1	1	RC_MAP_OFFB_SW	0	6
-1	1	RC_MAP_PARAM1	0	6
-1	1	RC_MAP_PARAM2	0	6
-1	1	RC_MAP_PARAM3	0	6
-1	1	RC_MAP_PITCH	2	6
-1	1	RC_MAP_POSCTL_SW	0	6
-1	1	RC_MAP_RATT_SW	0	6
-1	1	RC_MAP_RETURN_SW	0	6
-1	1	RC_MAP_ROLL	1	6
-1	1	RC_MAP_STAB_SW	0	6
-1	1	RC_MAP_THROTTLE	3	6
-1	1	RC_MAP_TRANS_SW	0	6
-1	1	RC_MAP_YAW	4	6
-1	1	RC_OFFB_TH	0.500000000000000000	9
-1	1	RC_POSCTL_TH	0.500000000000000000	9
-1	1	RC_RATT_TH	0.500000000000000000	9
-1	1	RC_RETURN_TH	0.500000000000000000	9
-1	1	RC_RSSI_PWM_CHAN	0	6
-1	1	RC_RSSI_PWM_MAX	1000	6
-1	1	RC_RSSI_PWM_MIN	2000	6
-1	1	RC_STAB_TH	0.500000000000000000	9
-1	1	RC_TRANS_TH	0.250000000000000000	9
-1	1	RTL_DESCEND_ALT	10.000000000000000000	9
-1	1	RTL_LAND_DELAY	0.000000000000000000	9
-1	1	RTL_MIN_DIST	5.000000000000000000	9
-1	1	RTL_RETURN_ALT	30.000000000000000000	9
-1	1	SDLOG_DIRS_MAX	0	6
-1	1	SDLOG_MODE	0	6
-1	1	SDLOG_PROFILE	3	6
-1	1	SDLOG_UTC_OFFSET	0	6
-1	1	SENS_BARO_QNH	1013.250000000000000000	9
-1	1	SENS_BOARD_ROT	0	6
-1	1	SENS_BOARD_X_OFF	0.576516091823577881	9
-1	1	SENS_BOARD_Y_OFF	2.084044218063354492	9
-1	1	SENS_BOARD_Z_OFF	0.000000000000000000	9
-1	1	SENS_DPRES_ANSC	0.000000000000000000	9
-1	1	SENS_DPRES_OFF	0.000000000000000000	9
-1	1	SENS_EN_LL40LS	0	6
-1	1	SENS_EN_MB12XX	0	6
-1	1	SENS_EN_SF0X	0	6
-1	1	SENS_EN_SF1XX	0	6
-1	1	SENS_EN_THERMAL	-1	6
-1	1	SENS_EN_TRANGER	0	6
-1	1	SYS_AUTOCONFIG	0	6
-1	1	SYS_AUTOSTART	4001	6
-1	1	SYS_CAL_ACCEL	0	6
-1	1	SYS_CAL_BARO	0	6
-1	1	SYS_CAL_GYRO	0	6
-1	1	SYS_CAL_TDEL	24	6
-1	1	SYS_CAL_TMAX	10	6
-1	1	SYS_CAL_TMIN	5	6
-1	1	SYS_COMPANION	921600	6
-1	1	SYS_FMU_TASK	0	6
-1	1	SYS_HITL	0	6
-1	1	SYS_LOGGER	1	6
-1	1	SYS_MC_EST_GROUP	1	6
-1	1	SYS_PARAM_VER	1	6
-1	1	SYS_RESTART_TYPE	0	6
-1	1	SYS_STCK_EN	1	6
-1	1	SYS_USE_IO	1	6
-1	1	TC_A0_ID	0	6
-1	1	TC_A0_SCL_0	1.000000000000000000	9
-1	1	TC_A0_SCL_1	1.000000000000000000	9
-1	1	TC_A0_SCL_2	1.000000000000000000	9
-1	1	TC_A0_TMAX	100.000000000000000000	9
-1	1	TC_A0_TMIN	0.000000000000000000	9
-1	1	TC_A0_TREF	25.000000000000000000	9
-1	1	TC_A0_X0_0	0.000000000000000000	9
-1	1	TC_A0_X0_1	0.000000000000000000	9
-1	1	TC_A0_X0_2	0.000000000000000000	9
-1	1	TC_A0_X1_0	0.000000000000000000	9
-1	1	TC_A0_X1_1	0.000000000000000000	9
-1	1	TC_A0_X1_2	0.000000000000000000	9
-1	1	TC_A0_X2_0	0.000000000000000000	9
-1	1	TC_A0_X2_1	0.000000000000000000	9
-1	1	TC_A0_X2_2	0.000000000000000000	9
-1	1	TC_A0_X3_0	0.000000000000000000	9
-1	1	TC_A0_X3_1	0.000000000000000000	9
-1	1	TC_A0_X3_2	0.000000000000000000	9
-1	1	TC_A1_ID	0	6
-1	1	TC_A1_SCL_0	1.000000000000000000	9
-1	1	TC_A1_SCL_1	1.000000000000000000	9
-1	1	TC_A1_SCL_2	1.000000000000000000	9
-1	1	TC_A1_TMAX	100.000000000000000000	9
-1	1	TC_A1_TMIN	0.000000000000000000	9
-1	1	TC_A1_TREF	25.000000000000000000	9
-1	1	TC_A1_X0_0	0.000000000000000000	9
-1	1	TC_A1_X0_1	0.000000000000000000	9
-1	1	TC_A1_X0_2	0.000000000000000000	9
-1	1	TC_A1_X1_0	0.000000000000000000	9
-1	1	TC_A1_X1_1	0.000000000000000000	9
-1	1	TC_A1_X1_2	0.000000000000000000	9
-1	1	TC_A1_X2_0	0.000000000000000000	9
-1	1	TC_A1_X2_1	0.000000000000000000	9
-1	1	TC_A1_X2_2	0.000000000000000000	9
-1	1	TC_A1_X3_0	0.000000000000000000	9
-1	1	TC_A1_X3_1	0.000000000000000000	9
-1	1	TC_A1_X3_2	0.000000000000000000	9
-1	1	TC_A2_ID	0	6
-1	1	TC_A2_SCL_0	1.000000000000000000	9
-1	1	TC_A2_SCL_1	1.000000000000000000	9
-1	1	TC_A2_SCL_2	1.000000000000000000	9
-1	1	TC_A2_TMAX	100.000000000000000000	9
-1	1	TC_A2_TMIN	0.000000000000000000	9
-1	1	TC_A2_TREF	25.000000000000000000	9
-1	1	TC_A2_X0_0	0.000000000000000000	9
-1	1	TC_A2_X0_1	0.000000000000000000	9
-1	1	TC_A2_X0_2	0.000000000000000000	9
-1	1	TC_A2_X1_0	0.000000000000000000	9
-1	1	TC_A2_X1_1	0.000000000000000000	9
-1	1	TC_A2_X1_2	0.000000000000000000	9
-1	1	TC_A2_X2_0	0.000000000000000000	9
-1	1	TC_A2_X2_1	0.000000000000000000	9
-1	1	TC_A2_X2_2	0.000000000000000000	9
-1	1	TC_A2_X3_0	0.000000000000000000	9
-1	1	TC_A2_X3_1	0.000000000000000000	9
-1	1	TC_A2_X3_2	0.000000000000000000	9
-1	1	TC_A_ENABLE	0	6
-1	1	TC_B0_ID	0	6
-1	1	TC_B0_SCL	1.000000000000000000	9
-1	1	TC_B0_TMAX	75.000000000000000000	9
-1	1	TC_B0_TMIN	5.000000000000000000	9
-1	1	TC_B0_TREF	40.000000000000000000	9
-1	1	TC_B0_X0	0.000000000000000000	9
-1	1	TC_B0_X1	0.000000000000000000	9
-1	1	TC_B0_X2	0.000000000000000000	9
-1	1	TC_B0_X3	0.000000000000000000	9
-1	1	TC_B0_X4	0.000000000000000000	9
-1	1	TC_B0_X5	0.000000000000000000	9
-1	1	TC_B1_ID	0	6
-1	1	TC_B1_SCL	1.000000000000000000	9
-1	1	TC_B1_TMAX	75.000000000000000000	9
-1	1	TC_B1_TMIN	5.000000000000000000	9
-1	1	TC_B1_TREF	40.000000000000000000	9
-1	1	TC_B1_X0	0.000000000000000000	9
-1	1	TC_B1_X1	0.000000000000000000	9
-1	1	TC_B1_X2	0.000000000000000000	9
-1	1	TC_B1_X3	0.000000000000000000	9
-1	1	TC_B1_X4	0.000000000000000000	9
-1	1	TC_B1_X5	0.000000000000000000	9
-1	1	TC_B2_ID	0	6
-1	1	TC_B2_SCL	1.000000000000000000	9
-1	1	TC_B2_TMAX	75.000000000000000000	9
-1	1	TC_B2_TMIN	5.000000000000000000	9
-1	1	TC_B2_TREF	40.000000000000000000	9
-1	1	TC_B2_X0	0.000000000000000000	9
-1	1	TC_B2_X1	0.000000000000000000	9
-1	1	TC_B2_X2	0.000000000000000000	9
-1	1	TC_B2_X3	0.000000000000000000	9
-1	1	TC_B2_X4	0.000000000000000000	9
-1	1	TC_B2_X5	0.000000000000000000	9
-1	1	TC_B_ENABLE	0	6
-1	1	TC_G0_ID	0	6
-1	1	TC_G0_SCL_0	1.000000000000000000	9
-1	1	TC_G0_SCL_1	1.000000000000000000	9
-1	1	TC_G0_SCL_2	1.000000000000000000	9
-1	1	TC_G0_TMAX	100.000000000000000000	9
-1	1	TC_G0_TMIN	0.000000000000000000	9
-1	1	TC_G0_TREF	25.000000000000000000	9
-1	1	TC_G0_X0_0	0.000000000000000000	9
-1	1	TC_G0_X0_1	0.000000000000000000	9
-1	1	TC_G0_X0_2	0.000000000000000000	9
-1	1	TC_G0_X1_0	0.000000000000000000	9
-1	1	TC_G0_X1_1	0.000000000000000000	9
-1	1	TC_G0_X1_2	0.000000000000000000	9
-1	1	TC_G0_X2_0	0.000000000000000000	9
-1	1	TC_G0_X2_1	0.000000000000000000	9
-1	1	TC_G0_X2_2	0.000000000000000000	9
-1	1	TC_G0_X3_0	0.000000000000000000	9
-1	1	TC_G0_X3_1	0.000000000000000000	9
-1	1	TC_G0_X3_2	0.000000000000000000	9
-1	1	TC_G1_ID	0	6
-1	1	TC_G1_SCL_0	1.000000000000000000	9
-1	1	TC_G1_SCL_1	1.000000000000000000	9
-1	1	TC_G1_SCL_2	1.000000000000000000	9
-1	1	TC_G1_TMAX	100.000000000000000000	9
-1	1	TC_G1_TMIN	0.000000000000000000	9
-1	1	TC_G1_TREF	25.000000000000000000	9
-1	1	TC_G1_X0_0	0.000000000000000000	9
-1	1	TC_G1_X0_1	0.000000000000000000	9
-1	1	TC_G1_X0_2	0.000000000000000000	9
-1	1	TC_G1_X1_0	0.000000000000000000	9
-1	1	TC_G1_X1_1	0.000000000000000000	9
-1	1	TC_G1_X1_2	0.000000000000000000	9
-1	1	TC_G1_X2_0	0.000000000000000000	9
-1	1	TC_G1_X2_1	0.000000000000000000	9
-1	1	TC_G1_X2_2	0.000000000000000000	9
-1	1	TC_G1_X3_0	0.000000000000000000	9
-1	1	TC_G1_X3_1	0.000000000000000000	9
-1	1	TC_G1_X3_2	0.000000000000000000	9
-1	1	TC_G2_ID	0	6
-1	1	TC_G2_SCL_0	1.000000000000000000	9
-1	1	TC_G2_SCL_1	1.000000000000000000	9
-1	1	TC_G2_SCL_2	1.000000000000000000	9
-1	1	TC_G2_TMAX	100.000000000000000000	9
-1	1	TC_G2_TMIN	0.000000000000000000	9
-1	1	TC_G2_TREF	25.000000000000000000	9
-1	1	TC_G2_X0_0	0.000000000000000000	9
-1	1	TC_G2_X0_1	0.000000000000000000	9
-1	1	TC_G2_X0_2	0.000000000000000000	9
-1	1	TC_G2_X1_0	0.000000000000000000	9
-1	1	TC_G2_X1_1	0.000000000000000000	9
-1	1	TC_G2_X1_2	0.000000000000000000	9
-1	1	TC_G2_X2_0	0.000000000000000000	9
-1	1	TC_G2_X2_1	0.000000000000000000	9
-1	1	TC_G2_X2_2	0.000000000000000000	9
-1	1	TC_G2_X3_0	0.000000000000000000	9
-1	1	TC_G2_X3_1	0.000000000000000000	9
-1	1	TC_G2_X3_2	0.000000000000000000	9
-1	1	TC_G_ENABLE	0	6
-1	1	THR_MDL_FAC	0.000000000000000000	9
-1	1	TRIG_MODE	0	6
-1	1	TRIM_PITCH	0.000000000000000000	9
-1	1	TRIM_ROLL	0.000000000000000000	9
-1	1	TRIM_YAW	0.000000000000000000	9
-1	1	VT_B_DEC_MSS	2.000000000000000000	9
-1	1	VT_B_REV_DEL	0.000000000000000000	9