More build scripts cleanup

Including removing echo_stamp, which is not needed

.gitattributes

@@ -3,6 +3,7 @@ roslib.js linguist-vendored
 eventemitter2.js linguist-vendored
 ros3d.js linguist-vendored
 three.min.js linguist-vendored
+json-to-pretty-yaml.js linguist-vendored
 aruco_pose/vendor/* linguist-vendored
 blockly/* linguist-vendored
 highlight/* linguist-vendored

.github/workflows/build-image.yaml

@@ -1,4 +1,4 @@
-name: Build RPi image
+name: RPi image
 
 on:
   push:
@@ -7,9 +7,10 @@ on:
     branches: [ master ]
   release:
     types: [ created ]
+  workflow_dispatch:
 
 jobs:
-  build-image:
+  build:
     runs-on: ubuntu-latest
     steps:
       - uses: actions/checkout@v2
@@ -18,7 +19,7 @@ jobs:
           docker run --privileged --rm -v /dev:/dev -v $(pwd):/builder/repo -e TRAVIS_TAG="${{ github.event.release.tag_name }}" sfalexrog/img-tool:qemu-update
       - name: Compress image
         run: |
-          sudo chmod -R 777 images && zip -9 $(echo images/clover_*).zip images/clover_* && ls -l images
+          cd images && sudo chmod -R 777 . && zip -9 $(echo clover_*).zip clover_* && ls -l . && unzip -l clover_*.zip
       - name: Upload image
         uses: softprops/action-gh-release@v1
         if: ${{ github.event_name == 'release' }}

.github/workflows/build.yml

@@ -5,19 +5,48 @@ on:
     branches: [ '*' ]
   pull_request:
     branches: [ master ]
+  workflow_dispatch:
 
 jobs:
-  build-melodic:
+  # melodic:
+  #   runs-on: ubuntu-latest
+  #   steps:
+  #     - uses: actions/checkout@v2
+  #     - name: Native Melodic build
+  #       run: |
+  #         docker run --rm -v $(pwd):/root/catkin_ws/src/clover ros:melodic-ros-base /root/catkin_ws/src/clover/builder/standalone-install.sh
+  noetic:
     runs-on: ubuntu-latest
+    container: ros:noetic-ros-base
+    defaults:
+      run:
+        working-directory: catkin_ws
+        shell: bash
     steps:
     - uses: actions/checkout@v2
-      - name: Native Melodic build
+      with:
+        path: catkin_ws/src/clover
+    - name: Install requirements
+      run: apt-get update && apt-get -y install python3-pip fakeroot python3-bloom debhelper dpkg-dev
+    - name: Install dependencies
+      run: rosdep update && rosdep install --from-paths src --ignore-src -y
+    - name: Install GeographicLib datasets
+      run: wget -qO- https://raw.githubusercontent.com/mavlink/mavros/master/mavros/scripts/install_geographiclib_datasets.sh | bash
+    - name: catkin_make
+      run: source /opt/ros/$ROS_DISTRO/setup.bash && catkin_make
+    - name: Run tests
+      run: source devel/setup.bash && catkin_make run_tests && catkin_test_results
+    - name: Build Debian packages
       run: |
-          docker run --rm -v $(pwd):/root/catkin_ws/src/clover ros:melodic-ros-base /root/catkin_ws/src/clover/builder/standalone-install.sh
+        source devel/setup.bash
-  build-noetic:
+        for file in `find . -name "package.xml"`; do
-    runs-on: ubuntu-latest
+          cd $(dirname ${file})
-    steps:
+          bloom-generate rosdebian --os-name ubuntu --os-version $(lsb_release -cs) --ros-distro $ROS_DISTRO  
-      - uses: actions/checkout@v2
+          fakeroot debian/rules binary
-      - name: Native Noetic build
+          cd -
-        run: |
+        done
-          docker run --rm -v $(pwd):/root/catkin_ws/src/clover ros:noetic-ros-base /root/catkin_ws/src/clover/builder/standalone-install.sh
+    - uses: actions/upload-artifact@v3
+      with:
+        name: debian-packages
+        path: catkin_ws/src/clover/*.deb
+        retention-days: 1

.github/workflows/docs.yml

@@ -4,11 +4,21 @@ on:
   push:
     branches: [ '*' ]
   pull_request:
-    branches: [ master ]
+    branches: [ '*' ]
+  workflow_dispatch:
+
+permissions:
+  contents: read
+  pages: write
+  id-token: write
+
+defaults:
+  run:
+    shell: bash
 
 jobs:
-  documentation:
+  docs:
-    runs-on: ubuntu-18.04
+    runs-on: ubuntu-22.04
     steps:
       - uses: actions/checkout@v2
       - name: Use Node.js
@@ -18,9 +28,8 @@ jobs:
         run: |
           sudo sh -c "echo ttf-mscorefonts-installer msttcorefonts/accepted-mscorefonts-eula select true | debconf-set-selections"
           sudo apt-get update && sudo apt-get install -y calibre msttcorefonts
-          npm install gitbook-cli -g
+          builder/assets/install_gitbook.sh
-          gitbook fetch 3.2.3 && npm i npm@3.10.10 --prefix=~/.gitbook/versions/3.2.3/ # fixing https://travis-ci.org/github/CopterExpress/clover/jobs/766541125#L932
+          npm install markdownlint-cli@0.28.1 -g # FIXME: https://github.com/DavidAnson/markdownlint/issues/435
-          npm install markdownlint-cli -g
           npm install svgexport -g
           gitbook -V
           markdownlint -V
@@ -35,7 +44,11 @@ jobs:
          gitbook install
          gitbook build
       - name: Generate PDF
-        if: ${{ github.event_name == 'push' && github.ref == 'refs/heads/master' }}
+        id: generate-pdf
+        env:
+          GITBOOK_SKIP_PDF: ${{ secrets.GITBOOK_SKIP_PDF }}
+        continue-on-error: ${{ env.GITBOOK_SKIP_PDF != '' }}
+        if: ${{ github.event_name == 'push' }}
         run: |
           for i in 1 2 3 4; do gitbook pdf ./ _book/clover.pdf && break || sleep 1; done
           sudo apt-get -q install ghostscript
@@ -44,11 +57,33 @@ jobs:
           rm _book/clover_ru.pdf && mv _book/clover_ru_compressed.pdf _book/clover_ru.pdf
           rm _book/clover_en.pdf && mv _book/clover_en_compressed.pdf _book/clover_en.pdf
           ls -lah _book/clover*.pdf
-      - name: Deploy
+          echo '::set-output name=GITBOOK_PDF_OK::1'
-        uses: JamesIves/github-pages-deploy-action@4.1.3
+      - name: Download older PDFs
-        if: ${{ github.event_name == 'push' && github.ref == 'refs/heads/master' }}
+        if: ${{ !steps.generate-pdf.outputs.GITBOOK_PDF_OK }}
+        run: |
+          rm -f _book/clover*.pdf
+          wget --no-verbose https://clover.coex.tech/clover_ru.pdf -P _book/
+          wget --no-verbose https://clover.coex.tech/clover_en.pdf -P _book/
+      - name: Upload artifact
+        # if: ${{ github.event_name == 'push' && github.ref == 'refs/heads/master' }}
+        uses: actions/upload-pages-artifact@v1
         with:
-          branch: gh-pages
+          path: _book
-          folder: _book
+
-          clean: true
+  deploy-docs:
-          single-commit: true # to avoid multiple copies of large pdf files
+    if: ${{ github.event_name == 'push' && github.ref == 'refs/heads/master' }}
+    concurrency:
+        group: "pages"
+        cancel-in-progress: true
+    environment:
+      name: github-pages
+      url: ${{ steps.deployment.outputs.page_url }}
+    runs-on: ubuntu-latest
+    needs: docs
+    steps:
+      - name: Deploy to GitHub Pages
+        env:
+          FREEZE_DOCS: ${{ secrets.FREEZE_DOCS }}
+        if: ${{ !env.FREEZE_DOCS }}
+        id: deployment
+        uses: actions/deploy-pages@v1

.github/workflows/editorconfig.yaml

@@ -1,10 +1,11 @@
-name: Editorconfig lint
+name: Editorconfig
 
 on:
   push:
     branches: [ '*' ]
   pull_request:
     branches: [ master ]
+  workflow_dispatch:
 
 jobs:
   editorconfig:
@@ -15,4 +16,4 @@ jobs:
         run: |
           wget --no-verbose https://github.com/okalachev/editorconfig-checker/releases/download/1.2.1-disable-spaces-amount/ec-linux-amd64
           chmod +x ec-linux-amd64
-          ./ec-linux-amd64 -spaces-after-tabs -e "roslib.js|ros3d.js|eventemitter2.js|draw.cpp|BinUtils.swift|\.idea|apps/android/app|blockly/|clover_blocks/programs/|highlight/|python.js|Assets.xcassets|test_parser_pass.txt|test_node_failure.txt|aruco_pose/vendor|\.stl|\.dxf|\.dae"
+          ./ec-linux-amd64 -spaces-after-tabs -e "roslib.js|ros3d.js|eventemitter2.js|json-to-pretty-yaml.js|draw.cpp|BinUtils.swift|\.idea|apps/android/app|blockly/|clover_blocks/programs/|highlight/|python.js|Assets.xcassets|test_parser_pass.txt|test_node_failure.txt|aruco_pose/vendor|\.stl|\.dxf|\.dae|\.material"

.gitignore

@@ -6,3 +6,4 @@ _book/
 package-lock.json
 clover_blocks/programs/*.*
 !clover_blocks/programs/examples/*
+/.vscode/

README.md

@@ -1,6 +1,6 @@
 # clover🍀: create autonomous drones easily
 
-<img src="docs/assets/clover42-main.png" align="right" width="400px" alt="COEX Clover Drone">
+<img src="docs/assets/clover42-main-margin.png" align="right" width="400px" alt="COEX Clover Drone">
 
 Clover is an open source [ROS](https://www.ros.org)-based framework, providing user-friendly tools to control [PX4](https://px4.io)-powered drones. Clover is available as a ROS package, but is shipped mainly as a preconfigured image for Raspberry Pi. Once you've installed Raspberry Pi on your drone and flashed the image to its microSD card, taking the drone up in the air is a matter of minutes.
 
@@ -20,7 +20,7 @@ Clover drone is used on a wide range of educational events, including [Copter Ha
 
 Preconfigured image for Raspberry Pi with installed and configured software, ready to fly, is available [in the Releases section](https://github.com/CopterExpress/clover/releases).
 
-![GitHub Workflow Status](https://img.shields.io/github/workflow/status/CopterExpress/clover/CI)
+![GitHub Workflow Status](https://img.shields.io/github/actions/workflow/status/CopterExpress/clover/build-image.yaml?branch=master)
 ![GitHub all releases](https://img.shields.io/github/downloads/CopterExpress/clover/total)
 
 Image features:

apps/ios/cleverrc/README.md

@@ -1,5 +1,4 @@
-iOS-приложение для управления Клевером
+# iOS-приложение для управления Клевером
---------------------------------------
 
 Для установки зависимостей необходим [CocoaPods](https://cocoapods.org):
 

aruco_pose/CMakeLists.txt

@@ -83,11 +83,10 @@ add_message_files(
 )
 
 ## Generate services in the 'srv' folder
-# add_service_files(
+add_service_files(
-#   FILES
+  FILES
-#   Service1.srv
+  SetMarkers.srv
-#   Service2.srv
+)
-# )
 
 ## Generate actions in the 'action' folder
 # add_action_files(
@@ -120,6 +119,7 @@ generate_messages(
 ## Generate dynamic reconfigure parameters in the 'cfg' folder
 generate_dynamic_reconfigure_options(
   cfg/Detector.cfg
+  cfg/Map.cfg
 )
 
 ###################################
@@ -202,11 +202,11 @@ set_property(TARGET aruco_pose
 # )
 
 ## Mark executables and/or libraries for installation
-# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
+install(TARGETS ${PROJECT_NAME}
-#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
+)
 
 ## Mark cpp header files for installation
 # install(DIRECTORY include/${PROJECT_NAME}/
@@ -226,6 +226,10 @@ catkin_install_python(PROGRAMS src/genmap.py
   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
 
+install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+
+install(DIRECTORY map DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+
 #############
 ## Testing ##
 #############
@@ -247,4 +251,5 @@ if (CATKIN_ENABLE_TESTING)
   add_rostest(test/test_node_failure.test)
   add_rostest(test/largemap.test)
   add_rostest(test/crash_opencv.test)
+  add_rostest(test/duplicate.test)
 endif()

aruco_pose/README.md

@@ -43,7 +43,8 @@ It's recommended to run it within the same nodelet manager with the camera nodel
 * `~frame_id_prefix` (*string*) – prefix for TF transforms names, marker's ID is appended (default: `aruco_`)
 * `~length` (*double*) – markers' sides length
 * `~length_override` (*map*) – lengths of markers with specified ids
-* `~known_tilt` (*string*) – known tilt (pitch and roll) of all the markers as a frame
+* `~known_vertical` (*string*) – known vertical (Z axis) of all the markers as a frame
+* `~flip_vertical` – flip vertical vector
 
 ### Topics
 
@@ -51,6 +52,7 @@ It's recommended to run it within the same nodelet manager with the camera nodel
 
 * `image_raw` (*sensor_msgs/Image*) – camera image
 * `camera_info` (*sensor_msgs/CameraInfo*) – camera calibration info
+* `map_markers` (*aruco_pose/MarkerArray*) – list of markers to disable TF transform publishing
 
 #### Published
 
@@ -70,10 +72,12 @@ It's recommended to run it within the same nodelet manager with the camera nodel
 
 * `~map` – path to text file with markers list
 * `~frame_id` – published frame id (default: `aruco_map`)
-* `~known_tilt` – debug image width
+* `~known_vertical` – known vertical (Z axis) of markers map as a frame
+* `~flip_vertical` – flip vertical vector
 * `~image_width` – debug image width (default: 2000)
 * `~image_height` – debug image height (default: 2000)
 * `~image_margin` – debug image margin (default: 200)
+* `~image_axis` – whether debug image should contain axis (default: true)
 * `~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:
@@ -97,6 +101,7 @@ See examples in [`map`](map/) directory.
 #### Published
 
 * `~pose` (*geometry_msgs/PoseWithCovarianceStamped*) – estimated map pose
+* `~map` (*aruco_pose/MarkerArray*) – list of markers in the loaded map
 * `~image` (*sensor_msgs/Image*) – planarized map image
 * `~visualization` (*visualization_msgs/MarkerArray*) – markers map visualization for rviz
 * `~debug` (*sensor_msgs/Image*) – debug image with detected markers and map axis

aruco_pose/cfg/Detector.cfg

@@ -4,12 +4,17 @@ PACKAGE = "aruco_pose"
 from dynamic_reconfigure.parameter_generator_catkin import *
 import cv2.aruco
 
+try:
         p = cv2.aruco.DetectorParameters_create()
+except AttributeError:
+        p = cv2.aruco.DetectorParameters()
 
 gen = ParameterGenerator()
 
 gen.add("enabled", bool_t, 0, "if detection enabled", True)
 
+gen.add("length", double_t, 0, "markers' side length", min=0, max=10)
+
 gen.add("adaptiveThreshConstant",                 double_t, 0,
         "Constant for adaptive thresholding before finding contours",
          p.adaptiveThreshConstant, 0, 100)

aruco_pose/cfg/Map.cfg

@@ -0,0 +1,14 @@
+#!/usr/bin/env python
+PACKAGE = "aruco_pose"
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+gen.add("enabled", bool_t, 0, "if map detection enabled", True)
+
+gen.add("map", str_t, 0, "full path for the map file")
+
+gen.add("image_axis", bool_t, 0, "debug image axis", default=True)
+
+exit(gen.generate(PACKAGE, "aruco_pose", "Map"))

aruco_pose/map/cmit.txt

@@ -1,3 +1,4 @@
+# id	length	x	y	z	rot_z	rot_y	rot_x
 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	

aruco_pose/map/office.txt

@@ -1,3 +1,4 @@
+# id	length	x	y	z	rot_z	rot_y	rot_x
 107	0.33	0	0	0	0	0	0
 106	0.33	0.77	0	0	0	0	0
 105	0.33	0	0.77	0	0	0	0

aruco_pose/map/office_ceiling.txt

@@ -1,3 +1,4 @@
+# id	length	x	y	z	rot_z	rot_y	rot_x
 14	0.365	0.000	0.0	0	0	0	0
 15	0.365	1.335	0.0	0	0	0	0
 30	0.365	2.865	0.0	0	0	0	0

aruco_pose/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
-<package format="2">
+<package format="3">
   <name>aruco_pose</name>
-  <version>0.21.1</version>
+  <version>0.24.0</version>
   <description>Positioning with ArUco markers</description>
 
   <maintainer email="okalachev@gmail.com">Oleg Kalachev</maintainer>
@@ -28,6 +28,8 @@
   <depend>sensor_msgs</depend>
   <depend>rostest</depend>
   <depend>dynamic_reconfigure</depend>
+  <depend condition="$ROS_PYTHON_VERSION == 2">python-docopt</depend>
+  <depend condition="$ROS_PYTHON_VERSION == 3">python3-docopt</depend>
 
   <test_depend>image_publisher</test_depend>
   <test_depend>ros_pytest</test_depend>

aruco_pose/src/aruco_detect.cpp

@@ -48,7 +48,9 @@
 #include <aruco_pose/Marker.h>
 #include <aruco_pose/MarkerArray.h>
 #include <aruco_pose/DetectorConfig.h>
+#include <aruco_pose/SetMarkers.h>
 
+#include "draw.h"
 #include "utils.h"
 #include <memory>
 #include <functional>
@@ -69,10 +71,13 @@ private:
 	image_transport::CameraSubscriber img_sub_;
 	ros::Publisher markers_pub_, vis_markers_pub_;
 	ros::Subscriber map_markers_sub_;
-	bool estimate_poses_, send_tf_, auto_flip_;
+	ros::ServiceServer set_markers_srv_;
+	bool estimate_poses_, send_tf_, flip_vertical_, auto_flip_, use_map_markers_;
+	bool waiting_for_map_;
 	double length_;
+	ros::Duration transform_timeout_;
 	std::unordered_map<int, double> length_override_;
-	std::string frame_id_prefix_, known_tilt_;
+	std::string frame_id_prefix_, known_vertical_;
 	Mat camera_matrix_, dist_coeffs_;
 	aruco_pose::MarkerArray array_;
 	std::unordered_set<int> map_markers_ids_;
@@ -92,13 +97,17 @@ public:
 		dictionary = nh_priv_.param("dictionary", 2);
 		estimate_poses_ = nh_priv_.param("estimate_poses", true);
 		send_tf_ = nh_priv_.param("send_tf", true);
+		use_map_markers_ = nh_priv_.param("use_map_markers", false);
+		waiting_for_map_ = use_map_markers_;
 		if (estimate_poses_ && !nh_priv_.getParam("length", length_)) {
 			NODELET_FATAL("can't estimate marker's poses as ~length parameter is not defined");
 			ros::shutdown();
 		}
 		readLengthOverride(nh_priv_);
+		transform_timeout_ = ros::Duration(nh_priv_.param("transform_timeout", 0.02));
 
-		known_tilt_ = nh_priv_.param<std::string>("known_tilt", "");
+		known_vertical_ = nh_priv_.param("known_vertical", nh_priv_.param("known_tilt", std::string(""))); // known_tilt is an old name
+		flip_vertical_ = nh_priv_.param<bool>("flip_vertical", false);
 		auto_flip_ = nh_priv_.param("auto_flip", false);
 
 		frame_id_prefix_ = nh_priv_.param<std::string>("frame_id_prefix", "aruco_");
@@ -114,6 +123,8 @@ public:
 		dyn_srv_ = std::make_shared<dynamic_reconfigure::Server<aruco_pose::DetectorConfig>>(nh_priv_);
 		dyn_srv_->setCallback(std::bind(&ArucoDetect::paramCallback, this, std::placeholders::_1, std::placeholders::_2));
 
+		set_markers_srv_ = nh_priv_.advertiseService("set_length_override", &ArucoDetect::setMarkers, this);
+
 		debug_pub_ = it_priv.advertise("debug", 1);
 		markers_pub_ = nh_priv_.advertise<aruco_pose::MarkerArray>("markers", 1);
 		vis_markers_pub_ = nh_priv_.advertise<visualization_msgs::MarkerArray>("visualization", 1);
@@ -127,14 +138,15 @@ private:
 	void imageCallback(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr &cinfo)
 	{
 		if (!enabled_) return;
+		if (waiting_for_map_) return;
 
-		Mat image = cv_bridge::toCvShare(msg, "bgr8")->image;
+		Mat image = cv_bridge::toCvShare(msg)->image;
 
 		vector<int> ids;
 		vector<vector<cv::Point2f>> corners, rejected;
 		vector<cv::Vec3d> rvecs, tvecs;
 		vector<cv::Point3f> obj_points;
-		geometry_msgs::TransformStamped snap_to;
+		geometry_msgs::TransformStamped vertical;
 
 		// Detect markers
 		cv::aruco::detectMarkers(image, dictionary_, corners, ids, parameters_, rejected);
@@ -169,18 +181,20 @@ private:
 					}
 				}
 
-				if (!known_tilt_.empty()) {
+				if (!known_vertical_.empty()) {
 					try {
-						snap_to = tf_buffer_->lookupTransform(msg->header.frame_id, known_tilt_,
+						vertical = tf_buffer_->lookupTransform(msg->header.frame_id, known_vertical_,
-						                                      msg->header.stamp, ros::Duration(0.02));
+						                                       msg->header.stamp, transform_timeout_);
 					} catch (const tf2::TransformException& e) {
-						NODELET_WARN_THROTTLE(5, "can't snap: %s", e.what());
+						NODELET_WARN_THROTTLE(5, "can't retrieve known vertical: %s", e.what());
 					}
 				}
 			}
 
 			array_.markers.reserve(ids.size());
 			aruco_pose::Marker marker;
+			vector<geometry_msgs::TransformStamped> transforms;
+			transforms.reserve(ids.size());
 			geometry_msgs::TransformStamped transform;
 			transform.header.stamp = msg->header.stamp;
 			transform.header.frame_id = msg->header.frame_id;
@@ -193,25 +207,38 @@ private:
 				if (estimate_poses_) {
 					fillPose(marker.pose, rvecs[i], tvecs[i]);
 
-					// snap orientation (if enabled and snap frame available)
+					// apply known vertical (if enabled and vertical frame available)
-					if (!known_tilt_.empty() && !snap_to.header.frame_id.empty()) {
+					if (!known_vertical_.empty() && !vertical.header.frame_id.empty()) {
-						snapOrientation(marker.pose.orientation, snap_to.transform.rotation, auto_flip_);
+						applyVertical(marker.pose.orientation, vertical.transform.rotation, false, auto_flip_);
 					}
 
-					// TODO: check IDs are unique
 					if (send_tf_) {
 						transform.child_frame_id = getChildFrameId(ids[i]);
 
 						// check if such static transform is in the map
 						if (map_markers_ids_.find(ids[i]) == map_markers_ids_.end()) {
+							// check if a markers with that id is already added
+							bool send = true;
+							for (auto &t : transforms) {
+								if (t.child_frame_id == transform.child_frame_id) {
+									send = false;
+									break;
+								}
+							}
+							if (send) {
 								transform.transform.rotation = marker.pose.orientation;
 								fillTranslation(transform.transform.translation, tvecs[i]);
-							br_->sendTransform(transform);
+								transforms.push_back(transform);
+							}
 						}
 					}
 				}
 				array_.markers.push_back(marker);
 			}
+
+			if (send_tf_) {
+				br_->sendTransform(transforms);
+			}
 		}
 
 		markers_pub_.publish(array_);
@@ -238,8 +265,7 @@ private:
 			cv::aruco::drawDetectedMarkers(debug, corners, ids); // draw markers
 			if (estimate_poses_)
 				for (unsigned int i = 0; i < ids.size(); i++)
-					cv::aruco::drawAxis(debug, camera_matrix_, dist_coeffs_,
+					_drawAxis(debug, camera_matrix_, dist_coeffs_, rvecs[i], tvecs[i], getMarkerLength(ids[i]));
-					                    rvecs[i], tvecs[i], getMarkerLength(ids[i]));
 
 			cv_bridge::CvImage out_msg;
 			out_msg.header.frame_id = msg->header.frame_id;
@@ -346,17 +372,47 @@ private:
 		}
 	}
 
+	bool setMarkers(aruco_pose::SetMarkers::Request& req, aruco_pose::SetMarkers::Response& res)
+	{
+		for (auto const& marker : req.markers) {
+			if (marker.id > 999) {
+				res.message = "Invalid marker id: " + std::to_string(marker.id);
+				ROS_ERROR("%s", res.message.c_str());
+				return true;
+			}
+			if (!std::isfinite(marker.length) || marker.length <= 0) {
+				res.message = "Invalid marker " + std::to_string(marker.id) + " length: " + std::to_string(marker.length);
+				ROS_ERROR("%s", res.message.c_str());
+				return true;
+			}
+		}
+
+		for (auto const& marker : req.markers) {
+			length_override_[marker.id] = marker.length;
+		}
+
+		res.success = true;
+		return true;
+	}
+
 	void mapMarkersCallback(const aruco_pose::MarkerArray& msg)
 	{
 		map_markers_ids_.clear();
 		for (auto const& marker : msg.markers) {
 			map_markers_ids_.insert(marker.id);
+			if (use_map_markers_) {
+				if (length_override_.find(marker.id) == length_override_.end()) {
+					length_override_[marker.id] = marker.length;
 				}
 			}
+		}
+		waiting_for_map_ = false;
+	}
 
 	void paramCallback(aruco_pose::DetectorConfig &config, uint32_t level)
 	{
-		enabled_ = config.enabled;
+		enabled_ = config.enabled && config.length > 0;
+		length_ = config.length;
 		parameters_->adaptiveThreshConstant = config.adaptiveThreshConstant;
 		parameters_->adaptiveThreshWinSizeMin = config.adaptiveThreshWinSizeMin;
 		parameters_->adaptiveThreshWinSizeMax = config.adaptiveThreshWinSizeMax;

aruco_pose/src/aruco_map.cpp

@@ -19,11 +19,13 @@
 #include <vector>
 #include <fstream>
 #include <algorithm>
+#include <memory>
 #include <ros/ros.h>
 #include <nodelet/nodelet.h>
 #include <pluginlib/class_list_macros.h>
 #include <image_transport/image_transport.h>
 #include <cv_bridge/cv_bridge.h>
+#include <dynamic_reconfigure/server.h>
 #include <tf/transform_datatypes.h>
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
@@ -41,6 +43,7 @@
 
 #include <aruco_pose/MarkerArray.h>
 #include <aruco_pose/Marker.h>
+#include <aruco_pose/MapConfig.h>
 
 #include <opencv2/opencv.hpp>
 #include <opencv2/aruco.hpp>
@@ -74,10 +77,13 @@ private:
 	tf2_ros::StaticTransformBroadcaster static_br_;
 	tf2_ros::Buffer tf_buffer_;
 	tf2_ros::TransformListener tf_listener_{tf_buffer_};
+	std::shared_ptr<dynamic_reconfigure::Server<aruco_pose::MapConfig>> dyn_srv_;
+	bool enabled_ = true;
+	std::string type_;
 	visualization_msgs::MarkerArray vis_array_;
-	std::string known_tilt_, map_, markers_frame_, markers_parent_frame_;
+	std::string known_vertical_, map_, markers_frame_, markers_parent_frame_;
 	int image_width_, image_height_, image_margin_;
-	bool auto_flip_, image_axis_;
+	bool flip_vertical_, auto_flip_, image_axis_, put_markers_count_to_covariance_;
 
 public:
 	virtual void onInit()
@@ -89,34 +95,35 @@ public:
 
 		// TODO: why image_transport doesn't work here?
 		img_pub_ = nh_priv_.advertise<sensor_msgs::Image>("image", 1, true);
-		markers_pub_ = nh_priv_.advertise<aruco_pose::MarkerArray>("markers", 1, true);
+		markers_pub_ = nh_priv_.advertise<aruco_pose::MarkerArray>("map", 1, true);
 
 		board_ = cv::makePtr<cv::aruco::Board>();
 		board_->dictionary = cv::aruco::getPredefinedDictionary(
 			                 static_cast<cv::aruco::PREDEFINED_DICTIONARY_NAME>(nh_priv_.param("dictionary", 2)));
 		camera_matrix_ = cv::Mat::zeros(3, 3, CV_64F);
 
-		std::string type, map;
+		type_ = nh_priv_.param<std::string>("type", "map");
-		type = nh_priv_.param<std::string>("type", "map");
 		transform_.child_frame_id = nh_priv_.param<std::string>("frame_id", "aruco_map");
-		known_tilt_ = nh_priv_.param<std::string>("known_tilt", "");
+		known_vertical_ = nh_priv_.param("known_vertical", nh_priv_.param("known_tilt", std::string(""))); // known_tilt is an old name
+		flip_vertical_ = nh_priv_.param<bool>("flip_vertical", false);
 		auto_flip_ = nh_priv_.param("auto_flip", false);
 		image_width_ = nh_priv_.param("image_width" , 2000);
 		image_height_ = nh_priv_.param("image_height", 2000);
 		image_margin_ = nh_priv_.param("image_margin", 200);
 		image_axis_ = nh_priv_.param("image_axis", true);
+		put_markers_count_to_covariance_ = nh_priv_.param("put_markers_count_to_covariance", false);
 		markers_parent_frame_ = nh_priv_.param<std::string>("markers/frame_id", transform_.child_frame_id);
 		markers_frame_ = nh_priv_.param<std::string>("markers/child_frame_id_prefix", "");
 
 		// createStripLine();
 
-		if (type == "map") {
+		if (type_ == "map") {
-			param(nh_priv_, "map", map);
+			map_ = nh_priv_.param<std::string>("map" , "");
-			loadMap(map);
+			loadMap(map_);
-		} else if (type == "gridboard") {
+		} else if (type_ == "gridboard") {
 			createGridBoard(nh_priv_);
 		} else {
-			NODELET_FATAL("unknown type: %s", type.c_str());
+			NODELET_FATAL("unknown type: %s", type_.c_str());
 			ros::shutdown();
 		}
 
@@ -124,10 +131,7 @@ public:
 		vis_markers_pub_ = nh_priv_.advertise<visualization_msgs::MarkerArray>("visualization", 1, true);
 		debug_pub_ = it_priv.advertise("debug", 1);
 
-		publishMarkersFrames();
+		publishMap();
-		publishMarkers();
-		publishMapImage();
-		vis_markers_pub_.publish(vis_array_);
 
 		image_sub_.subscribe(nh_, "image_raw", 1);
 		info_sub_.subscribe(nh_, "camera_info", 1);
@@ -136,6 +140,12 @@ 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));
 
+		dyn_srv_ = std::make_shared<dynamic_reconfigure::Server<aruco_pose::MapConfig>>(nh_priv_);
+		dynamic_reconfigure::Server<aruco_pose::MapConfig>::CallbackType cb;
+
+		cb = std::bind(&ArucoMap::paramCallback, this, std::placeholders::_1, std::placeholders::_2);
+		dyn_srv_->setCallback(cb);
+
 		NODELET_INFO("ready");
 	}
 
@@ -143,6 +153,9 @@ public:
 	              const sensor_msgs::CameraInfoConstPtr& cinfo,
 	              const aruco_pose::MarkerArrayConstPtr& markers)
 	{
+		if (!enabled_) return;
+		if (markers->markers.empty()) return; // map not loaded
+
 		int valid = 0;
 		int count = markers->markers.size();
 		std::vector<int> ids;
@@ -166,7 +179,21 @@ public:
 			corners.push_back(marker_corners);
 		}
 
-		if (known_tilt_.empty()) {
+		if (put_markers_count_to_covariance_) {
+			// HACK: pass markers count using covariance field
+			int valid_markers = 0;
+			for (auto const &marker : markers->markers) {
+				for (auto const &board_marker : board_->ids) {
+					if (board_marker == marker.id) {
+						valid_markers++;
+						break;
+					}
+				}
+			}
+			pose_.pose.covariance[0] = valid_markers;
+		}
+
+		if (known_vertical_.empty()) {
 			// simple estimation
 			valid = cv::aruco::estimatePoseBoard(corners, ids, board_, camera_matrix_, dist_coeffs_,
 			                                     rvec, tvec, false);
@@ -180,7 +207,7 @@ public:
 
 		} else {
 			Mat obj_points, img_points;
-			// estimation with "snapping"
+			// estimation with known vertical
 			cv::aruco::getBoardObjectAndImagePoints(board_, corners, ids, obj_points, img_points);
 			if (obj_points.empty()) goto publish_debug;
 
@@ -192,11 +219,11 @@ public:
 
 			fillTransform(transform_.transform, rvec, tvec);
 			try {
-				geometry_msgs::TransformStamped snap_to = tf_buffer_.lookupTransform(markers->header.frame_id,
+				geometry_msgs::TransformStamped vertical = tf_buffer_.lookupTransform(markers->header.frame_id,
-				                                          known_tilt_, markers->header.stamp, ros::Duration(0.02));
+				                                           known_vertical_, markers->header.stamp, ros::Duration(0.02));
-				snapOrientation(transform_.transform.rotation, snap_to.transform.rotation, auto_flip_);
+				applyVertical(transform_.transform.rotation, vertical.transform.rotation, flip_vertical_, auto_flip_);
 			} catch (const tf2::TransformException& e) {
-				NODELET_WARN_THROTTLE(1, "can't snap: %s", e.what());
+				NODELET_WARN_THROTTLE(1, "can't retrieve known vertical: %s", e.what());
 			}
 
 			geometry_msgs::TransformStamped shift;
@@ -268,9 +295,17 @@ publish_debug:
 		std::ifstream f(filename);
 		std::string line;
 
+		clearMarkers();
+
+		if (map_ == "") {
+			NODELET_INFO("No map loaded");
+			return;
+		}
+
 		if (!f.good()) {
-			NODELET_FATAL("%s - %s", strerror(errno), filename.c_str());
+			NODELET_ERROR("%s - %s", strerror(errno), filename.c_str());
-			ros::shutdown();
+			map_ = "";
+			return;
 		}
 
 		while (std::getline(f, line)) {
@@ -296,9 +331,10 @@ publish_debug:
 				s.putback(first);
 			} else {
 				// Probably garbage data; inform user and throw an exception, possibly killing nodelet
-				NODELET_FATAL("Malformed input: %s", line.c_str());
+				NODELET_ERROR("Malformed input: %s", line.c_str());
-				ros::shutdown();
+				map_ = "";
-				throw std::runtime_error("Malformed input");
+				clearMarkers();
+				return;
 			}
 
 			if (!(s >> id >> length >> x >> y)) {
@@ -329,6 +365,14 @@ publish_debug:
 		NODELET_INFO("loading %s complete (%d markers)", filename.c_str(), static_cast<int>(board_->ids.size()));
 	}
 
+	void publishMap()
+	{
+		publishMarkersFrames();
+		publishMarkers();
+		publishMapImage();
+		vis_markers_pub_.publish(vis_array_);
+	}
+
 	void createGridBoard(ros::NodeHandle& nh)
 	{
 		NODELET_INFO("generate gridboard");
@@ -370,6 +414,15 @@ publish_debug:
 		}
 	}
 
+	void clearMarkers()
+	{
+		board_->ids.clear();
+		board_->objPoints.clear();
+		markers_.markers.clear();
+		vis_array_.markers.clear();
+		markers_transforms_.clear();
+	}
+
 	// void createStripLine()
 	// {
 	// 	visualization_msgs::Marker marker;
@@ -466,7 +519,7 @@ publish_debug:
 		vis_marker.pose.position.x = x;
 		vis_marker.pose.position.y = y;
 		vis_marker.pose.position.z = z;
-		tf::quaternionTFToMsg(q, marker.pose.orientation);
+		tf::quaternionTFToMsg(q, vis_marker.pose.orientation);
 		vis_marker.frame_locked = true;
 		vis_array_.markers.push_back(vis_marker);
 
@@ -509,6 +562,22 @@ publish_debug:
 		msg.image = image;
 		img_pub_.publish(msg.toImageMsg());
 	}
+
+	void paramCallback(aruco_pose::MapConfig &config, uint32_t level)
+	{
+		// https://github.com/CopterExpress/clover/commit/2cd334c474e3ed04ef65ca1ba7f08ab535a3dc6d#diff-942723f9452c398ae93f1a91427f9a7b614be5e5871f8a3e590f324d804f0d58R356
+		enabled_ = config.enabled;
+		if (type_ == "map" && config.map != map_) {
+			map_ = config.map;
+			loadMap(map_);
+			publishMap();
+		}
+
+		if (config.image_axis != image_axis_) {
+			image_axis_ = config.image_axis;
+			publishMapImage();
+		}
+	}
 };
 
 PLUGINLIB_EXPORT_CLASS(ArucoMap, nodelet::Nodelet)

aruco_pose/src/genmap.py

@@ -30,7 +30,7 @@ Options:
   -o <filename>  Output map file name in the 'map' subdirectory of aruco_pose package
 
 Example:
-  rosrun aruco_pose genmap.py 0.33 2 4 1 1 0 > $(catkin_find aruco_pose map)/test_map.txt
+  rosrun aruco_pose genmap.py 0.33 2 4 1 1 0 -o test_map.txt
 """
 
 from __future__ import print_function

aruco_pose/src/utils.h

@@ -106,26 +106,25 @@ inline bool isFlipped(tf::Quaternion& q)
 	return (abs(pitch) > M_PI / 2) || (abs(roll) > M_PI / 2);
 }
 
-/* Set roll and pitch from "from" to "to", keeping yaw */
+/* Apply a vertical to an orientation */
-inline void snapOrientation(geometry_msgs::Quaternion& to, const geometry_msgs::Quaternion& from, bool auto_flip = false)
+inline void applyVertical(geometry_msgs::Quaternion& orientation, const geometry_msgs::Quaternion& vertical,
+                          bool flip_vertical = false, bool auto_flip = false) // editorconfig-checker-disable-line
 {
-	tf::Quaternion _from, _to;
+	tf::Quaternion _vertical, _orientation;
-	tf::quaternionMsgToTF(from, _from);
+	tf::quaternionMsgToTF(vertical, _vertical);
-	tf::quaternionMsgToTF(to, _to);
+	tf::quaternionMsgToTF(orientation, _orientation);
 
-	if (auto_flip) {
+	if (flip_vertical || (auto_flip && !isFlipped(_orientation))) {
-		if (!isFlipped(_from)) {
 		static const tf::Quaternion flip = tf::createQuaternionFromRPY(M_PI, 0, 0);
-			_from *= flip; // flip "from"
+		_vertical *= flip; // flip vertical
-		}
 	}
 
-	auto diff = tf::Matrix3x3(_to).transposeTimes(tf::Matrix3x3(_from));
+	auto diff = tf::Matrix3x3(_orientation).transposeTimes(tf::Matrix3x3(_vertical));
 	double _, yaw;
 	diff.getRPY(_, _, yaw);
 	auto q = tf::createQuaternionFromRPY(0, 0, -yaw);
-	_from = _from * q; // set yaw from "to" to "from"
+	_vertical = _vertical * q; // set yaw from orientation to vertical
-	tf::quaternionTFToMsg(_from, to); // set "from" to "to"
+	tf::quaternionTFToMsg(_vertical, orientation); // set vertical to orientation
 }
 
 inline void transformToPose(const geometry_msgs::Transform& transform, geometry_msgs::Pose& pose)

aruco_pose/srv/SetMarkers.srv

@@ -0,0 +1,7 @@
+# * Add or change markers in the map
+# * Change markers' properties, e. g. lengths
+
+Marker[] markers # if length or pose is nan - remove from map
+---
+bool success
+string message

aruco_pose/test/basic.py

@@ -6,7 +6,7 @@ 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
+from visualization_msgs.msg import MarkerArray as VisMarkerArray, Marker as VisMarker
 
 
 @pytest.fixture
@@ -143,7 +143,7 @@ def test_map_image(node):
     assert img.encoding in ('mono8', 'rgb8')
 
 def test_map_markers(node):
-    markers = rospy.wait_for_message('aruco_map/markers', MarkerArray, timeout=5)
+    markers = rospy.wait_for_message('aruco_map/map', MarkerArray, timeout=5)
     assert markers.markers[0].id == 1
     assert markers.markers[1].id == 2
     assert markers.markers[2].id == 3
@@ -199,6 +199,36 @@ def test_map_markers(node):
 
 def test_map_visualization(node):
     vis = rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5)
+    assert len(vis.markers) == 7
+    assert vis.markers[0].header.frame_id == 'aruco_map'
+    assert vis.markers[0].type == VisMarker.CUBE
+    assert vis.markers[0].action == VisMarker.ADD
+    assert vis.markers[0].pose.position.x == 0
+    assert vis.markers[0].pose.position.y == 0
+    assert vis.markers[0].pose.position.z == 0
+    assert vis.markers[0].pose.orientation.x == 0
+    assert vis.markers[0].pose.orientation.y == 0
+    assert vis.markers[0].pose.orientation.z == 0
+    assert vis.markers[0].pose.orientation.w == 1
+    assert vis.markers[0].scale.x == approx(0.33)
+    assert vis.markers[0].scale.y == approx(0.33)
+    assert vis.markers[0].scale.z == approx(0.001)
+    assert vis.markers[1].pose.position.x == 1
+    assert vis.markers[1].pose.position.y == 0
+    assert vis.markers[1].pose.position.z == 0
+    assert vis.markers[1].pose.orientation.x == 0
+    assert vis.markers[1].pose.orientation.y == 0
+    assert vis.markers[1].pose.orientation.z == 0
+    assert vis.markers[1].pose.orientation.w == 1
+    # non-zero yaw marker:
+    assert vis.markers[4].scale.x == approx(0.5)
+    assert vis.markers[4].pose.position.x == approx(0.5)
+    assert vis.markers[4].pose.position.y == 2
+    assert vis.markers[4].pose.position.z == 0
+    assert vis.markers[4].pose.orientation.x == 0
+    assert vis.markers[4].pose.orientation.y == 0
+    assert vis.markers[4].pose.orientation.z == approx(0.5646424733950354)
+    assert vis.markers[4].pose.orientation.w == approx(0.8253356149096783)
 
 def test_map_debug(node):
     img = rospy.wait_for_message('aruco_map/debug', Image, timeout=5)

aruco_pose/test/duplicate.py

@@ -0,0 +1,8 @@
+import pytest
+import subprocess
+
+def test_no_tf_repeated_data():
+    # `/rosout` acts weirdly inside rostest, so using a subprocess
+    cmd = """python -c 'import rospy, tf; rospy.init_node("foo"); listener = tf.TransformListener(); rospy.sleep(2)'"""
+    output = str(subprocess.check_output(cmd, shell=True, stderr=subprocess.STDOUT))
+    assert 'TF_REPEATED_DATA' not in output, 'TF_REPEATED_DATA was logged on duplicate markers'

aruco_pose/test/duplicate.test

@@ -0,0 +1,21 @@
+<launch>
+    <node pkg="image_publisher" type="image_publisher" name="main_camera" args="$(find aruco_pose)/test/duplicate.png">
+        <param name="frame_id" value="main_camera_optical"/>
+        <param name="publish_rate" value="10"/>
+        <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" required="true"/>
+
+    <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="estimate_poses" value="true"/>
+        <param name="send_tf" value="true"/>
+        <param name="cornerRefinementMethod" value="1"/>
+    </node>
+
+	<param name="test_module" value="$(find aruco_pose)/test/duplicate.py"/>
+    <test test-name="aruco_pose_test" pkg="ros_pytest" type="ros_pytest_runner"/>
+</launch>

aruco_pose/test/test_node_failure.py

@@ -2,6 +2,7 @@ import rospy
 import pytest
 
 from visualization_msgs.msg import MarkerArray as VisMarkerArray
+from aruco_pose.msg import MarkerArray
 
 
 @pytest.fixture
@@ -9,5 +10,5 @@ def node():
     return rospy.init_node('aruco_pose_test', anonymous=True)
 
 def test_node_failure(node):
-    with pytest.raises(rospy.exceptions.ROSException):
+    assert rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5).markers == []
-        rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5)
+    assert rospy.wait_for_message('aruco_map/map', MarkerArray, timeout=5).markers == []

book.json

@@ -10,7 +10,7 @@
         "yametrika",
         "anchors",
         "collapsible-menu",
-        "validate-links",
+        "validate-links@https://github.com/okalachev/gitbook-plugin-validate-links.git",
         "bulk-redirect@https://github.com/okalachev/gitbook-plugin-bulk-redirect.git",
         "sitemap@https://github.com/okalachev/plugin-sitemap.git",
         "toolbar@https://github.com/hamishwillee/gitbook-plugin-toolbar.git",

builder/assets/clover.service

@@ -8,5 +8,9 @@ ExecStart=/bin/bash -c ". /home/pi/catkin_ws/devel/setup.sh; \
                       ROS_HOSTNAME=`hostname`.local exec stdbuf -o L roslaunch clover clover.launch --wait --screen --skip-log-check \
                       2> >(tee /tmp/clover.err)"
 
+ExecStartPre=+rm /var/log/clover.log
+StandardOutput=file:/var/log/clover.log
+StandardError=file:/var/log/clover.log
+
 [Install]
 WantedBy=multi-user.target

builder/assets/hardware_setup.sh

@@ -15,52 +15,26 @@
 
 set -e # Exit immidiately on non-zero result
 
-echo_stamp() {
-  # TEMPLATE: echo_stamp <TEXT> <TYPE>
-  # TYPE: SUCCESS, ERROR, INFO
-
-  # More info there https://www.shellhacks.com/ru/bash-colors/
-
-  TEXT="$(date '+[%Y-%m-%d %H:%M:%S]') $1"
-  TEXT="\e[1m$TEXT\e[0m" # BOLD
-
-  case "$2" in
-    SUCCESS)
-    TEXT="\e[32m${TEXT}\e[0m";; # GREEN
-    ERROR)
-    TEXT="\e[31m${TEXT}\e[0m";; # RED
-    *)
-    TEXT="\e[34m${TEXT}\e[0m";; # BLUE
-  esac
-  echo -e ${TEXT}
-}
-
 ##################################################
 # Configure hardware interfaces
 ##################################################
 
-# 1. Enable sshd
+echo "--- Enable sshd"
-echo_stamp "#1 Turn on sshd"
 touch /boot/ssh
 # /usr/bin/raspi-config nonint do_ssh 0
 
-# 2. Enable GPIO
+echo "--- GPIO enabled by default"
-echo_stamp "#2 GPIO enabled by default"
 
-# 3. Enable I2C
+echo "--- Enable I2C"
-echo_stamp "#3 Turn on I2C"
 /usr/bin/raspi-config nonint do_i2c 0
 
-# 4. Enable SPI
+echo "--- Enable SPI"
-echo_stamp "#4 Turn on SPI"
 /usr/bin/raspi-config nonint do_spi 0
 
-# 5. Enable raspicam
+echo  "--- Enable raspicam"
-echo_stamp "#5 Turn on raspicam"
 /usr/bin/raspi-config nonint do_camera 0
 
-# 6. Enable hardware UART
+echo "--- Enable UART"
-echo_stamp "#6 Turn on UART"
 # Temporary solution
 # https://github.com/RPi-Distro/raspi-config/pull/75
 /usr/bin/raspi-config nonint do_serial 1
@@ -72,11 +46,9 @@ systemctl disable hciuart.service
 # https://github.com/RPi-Distro/raspi-config/commit/d6d9ecc0d9cbe4aaa9744ae733b9cb239e79c116
 #/usr/bin/raspi-config nonint do_serial 2
 
-# 7. Enable V4L driver http://robocraft.ru/blog/electronics/3158.html
+echo "--- Enable v4l2 driver"
+# http://robocraft.ru/blog/electronics/3158.html
 #echo "bcm2835-v4l2" >> /etc/modules
-echo_stamp "#7 Turn on v4l2 driver"
 if ! grep -q "^bcm2835-v4l2" /etc/modules;
 then printf "bcm2835-v4l2\n" >> /etc/modules
 fi
-
-echo_stamp "#8 End of configure hardware interfaces"

builder/assets/init_rpi.sh

@@ -15,28 +15,8 @@
 
 set -e # Exit immidiately on non-zero result
 
-echo_stamp() {
-  # TEMPLATE: echo_stamp <TEXT> <TYPE>
-  # TYPE: SUCCESS, ERROR, INFO
-
-  # More info there https://www.shellhacks.com/ru/bash-colors/
-
-  TEXT="$(date '+[%Y-%m-%d %H:%M:%S]') $1"
-  TEXT="\e[1m$TEXT\e[0m" # BOLD
-
-  case "$2" in
-    SUCCESS)
-    TEXT="\e[32m${TEXT}\e[0m";; # GREEN
-    ERROR)
-    TEXT="\e[31m${TEXT}\e[0m";; # RED
-    *)
-    TEXT="\e[34m${TEXT}\e[0m";; # BLUE
-  esac
-  echo -e ${TEXT}
-}
-
 NEW_SSID='clover-'$(head -c 100 /dev/urandom | xxd -ps -c 100 | sed -e "s/[^0-9]//g" | cut -c 1-4)
-echo_stamp "Setting SSID to ${NEW_SSID}"
+echo "--- Setting SSID to ${NEW_SSID}"
 # TODO: Use wpa_cli insted direct file edit
 # FIXME: We rely on raspberrypi-net-mods to copy our file to /etc/wpa_supplicant.
 # This is not very reliable, but seems to fix our rfkill problem.
@@ -57,19 +37,17 @@ network={
 EOF
 
 NEW_HOSTNAME=$(echo ${NEW_SSID} | tr '[:upper:]' '[:lower:]')
-echo_stamp "Setting hostname to $NEW_HOSTNAME"
+echo "--- Setting hostname to $NEW_HOSTNAME"
 hostnamectl set-hostname $NEW_HOSTNAME
 sed -i 's/127\.0\.1\.1.*/127.0.1.1\t'${NEW_HOSTNAME}' '${NEW_HOSTNAME}'.local/g' /etc/hosts
 # .local (mdns) hostname added to make it accesable when wlan and ethernet interfaces are down
 
-echo_stamp "Enable ROS services"
+echo "--- Enable ROS services"
 systemctl enable roscore
 systemctl enable clover
 
-echo_stamp "Harware setup"
+echo "--- Harware setup"
 /root/hardware_setup.sh
 
-echo_stamp "Remove init scripts"
+echo "--- Remove init scripts"
 rm /root/init_rpi.sh /root/hardware_setup.sh
-
-echo_stamp "End of initialization of the image"

builder/assets/install_gitbook.sh

@@ -0,0 +1,9 @@
+#!/usr/bin/env bash
+
+# GitBook CLI is deprecated, its installation is broken.
+# This script fixes it until we stop using GitBook.
+
+export NPM_CONFIG_UNSAFE_PERM=1
+
+npm install gitbook-cli -g
+gitbook fetch 3.2.3 && npm i npm@3.10.10 --prefix=~/.gitbook/versions/3.2.3/ # fixing https://travis-ci.org/github/CopterExpress/clover/jobs/766541125#L932

builder/assets/noetic-rosdep-clover.yaml

@@ -16,3 +16,726 @@ web_video_server:
 ws281x:
   debian:
     buster: [ros-noetic-ws281x]
+catkin:
+  debian:
+    buster: [ros-noetic-catkin]
+genmsg:
+  debian:
+    buster: [ros-noetic-genmsg]
+gencpp:
+  debian:
+    buster: [ros-noetic-gencpp]
+geneus:
+  debian:
+    buster: [ros-noetic-geneus]
+genlisp:
+  debian:
+    buster: [ros-noetic-genlisp]
+gennodejs:
+  debian:
+    buster: [ros-noetic-gennodejs]
+genpy:
+  debian:
+    buster: [ros-noetic-genpy]
+bond_core:
+  debian:
+    buster: [ros-noetic-bond-core]
+cmake_modules:
+  debian:
+    buster: [ros-noetic-cmake-modules]
+class_loader:
+  debian:
+    buster: [ros-noetic-class-loader]
+common_msgs:
+  debian:
+    buster: [ros-noetic-common-msgs]
+common_tutorials:
+  debian:
+    buster: [ros-noetic-common-tutorials]
+cpp_common:
+  debian:
+    buster: [ros-noetic-cpp-common]
+desktop:
+  debian:
+    buster: [ros-noetic-desktop]
+diagnostics:
+  debian:
+    buster: [ros-noetic-diagnostics]
+executive_smach:
+  debian:
+    buster: [ros-noetic-executive-smach]
+geometry:
+  debian:
+    buster: [ros-noetic-geometry]
+geometry_tutorials:
+  debian:
+    buster: [ros-noetic-geometry-tutorials]
+gl_dependency:
+  debian:
+    buster: [ros-noetic-gl-dependency]
+image_common:
+  debian:
+    buster: [ros-noetic-image-common]
+image_pipeline:
+  debian:
+    buster: [ros-noetic-image-pipeline]
+image_transport_plugins:
+  debian:
+    buster: [ros-noetic-image-transport-plugins]
+laser_pipeline:
+  debian:
+    buster: [ros-noetic-laser-pipeline]
+mavlink:
+  debian:
+    buster: [ros-noetic-mavlink]
+media_export:
+  debian:
+    buster: [ros-noetic-media-export]
+message_generation:
+  debian:
+    buster: [ros-noetic-message-generation]
+message_runtime:
+  debian:
+    buster: [ros-noetic-message-runtime]
+mk:
+  debian:
+    buster: [ros-noetic-mk]
+nodelet_core:
+  debian:
+    buster: [ros-noetic-nodelet-core]
+orocos_kdl:
+  debian:
+    buster: [ros-noetic-orocos-kdl]
+perception:
+  debian:
+    buster: [ros-noetic-perception]
+perception_pcl:
+  debian:
+    buster: [ros-noetic-perception-pcl]
+python_orocos_kdl:
+  debian:
+    buster: [ros-noetic-python-orocos-kdl]
+qt_dotgraph:
+  debian:
+    buster: [ros-noetic-qt-dotgraph]
+qt_gui:
+  debian:
+    buster: [ros-noetic-qt-gui]
+qt_gui_py_common:
+  debian:
+    buster: [ros-noetic-qt-gui-py-common]
+qwt_dependency:
+  debian:
+    buster: [ros-noetic-qwt-dependency]
+robot:
+  debian:
+    buster: [ros-noetic-robot]
+ros:
+  debian:
+    buster: [ros-noetic-ros]
+ros_base:
+  debian:
+    buster: [ros-noetic-ros-base]
+ros_comm:
+  debian:
+    buster: [ros-noetic-ros-comm]
+ros_core:
+  debian:
+    buster: [ros-noetic-ros-core]
+ros_environment:
+  debian:
+    buster: [ros-noetic-ros-environment]
+ros_tutorials:
+  debian:
+    buster: [ros-noetic-ros-tutorials]
+rosapi:
+  debian:
+    buster: [ros-noetic-rosapi]
+rosbag_migration_rule:
+  debian:
+    buster: [ros-noetic-rosbag-migration-rule]
+rosbash:
+  debian:
+    buster: [ros-noetic-rosbash]
+rosboost_cfg:
+  debian:
+    buster: [ros-noetic-rosboost-cfg]
+rosbridge_server:
+  debian:
+    buster: [ros-noetic-rosbridge-server]
+rosbridge_suite:
+  debian:
+    buster: [ros-noetic-rosbridge-suite]
+rosbuild:
+  debian:
+    buster: [ros-noetic-rosbuild]
+rosclean:
+  debian:
+    buster: [ros-noetic-rosclean]
+roscpp_core:
+  debian:
+    buster: [ros-noetic-roscpp-core]
+roscpp_traits:
+  debian:
+    buster: [ros-noetic-roscpp-traits]
+roscreate:
+  debian:
+    buster: [ros-noetic-roscreate]
+rosgraph:
+  debian:
+    buster: [ros-noetic-rosgraph]
+roslang:
+  debian:
+    buster: [ros-noetic-roslang]
+roslint:
+  debian:
+    buster: [ros-noetic-roslint]
+roslisp:
+  debian:
+    buster: [ros-noetic-roslisp]
+rosmake:
+  debian:
+    buster: [ros-noetic-rosmake]
+rosmaster:
+  debian:
+    buster: [ros-noetic-rosmaster]
+rospack:
+  debian:
+    buster: [ros-noetic-rospack]
+roslib:
+  debian:
+    buster: [ros-noetic-roslib]
+rosparam:
+  debian:
+    buster: [ros-noetic-rosparam]
+rospy:
+  debian:
+    buster: [ros-noetic-rospy]
+rosserial:
+  debian:
+    buster: [ros-noetic-rosserial]
+rosserial_msgs:
+  debian:
+    buster: [ros-noetic-rosserial-msgs]
+rosserial_python:
+  debian:
+    buster: [ros-noetic-rosserial-python]
+rosservice:
+  debian:
+    buster: [ros-noetic-rosservice]
+rostime:
+  debian:
+    buster: [ros-noetic-rostime]
+roscpp_serialization:
+  debian:
+    buster: [ros-noetic-roscpp-serialization]
+python_qt_binding:
+  debian:
+    buster: [ros-noetic-python-qt-binding]
+roslaunch:
+  debian:
+    buster: [ros-noetic-roslaunch]
+rosunit:
+  debian:
+    buster: [ros-noetic-rosunit]
+angles:
+  debian:
+    buster: [ros-noetic-angles]
+libmavconn:
+  debian:
+    buster: [ros-noetic-libmavconn]
+rosconsole:
+  debian:
+    buster: [ros-noetic-rosconsole]
+pluginlib:
+  debian:
+    buster: [ros-noetic-pluginlib]
+qt_gui_cpp:
+  debian:
+    buster: [ros-noetic-qt-gui-cpp]
+resource_retriever:
+  debian:
+    buster: [ros-noetic-resource-retriever]
+rosconsole_bridge:
+  debian:
+    buster: [ros-noetic-rosconsole-bridge]
+roslz4:
+  debian:
+    buster: [ros-noetic-roslz4]
+rosserial_client:
+  debian:
+    buster: [ros-noetic-rosserial-client]
+rostest:
+  debian:
+    buster: [ros-noetic-rostest]
+rqt_action:
+  debian:
+    buster: [ros-noetic-rqt-action]
+rqt_bag:
+  debian:
+    buster: [ros-noetic-rqt-bag]
+rqt_bag_plugins:
+  debian:
+    buster: [ros-noetic-rqt-bag-plugins]
+rqt_common_plugins:
+  debian:
+    buster: [ros-noetic-rqt-common-plugins]
+rqt_console:
+  debian:
+    buster: [ros-noetic-rqt-console]
+rqt_dep:
+  debian:
+    buster: [ros-noetic-rqt-dep]
+rqt_graph:
+  debian:
+    buster: [ros-noetic-rqt-graph]
+rqt_gui:
+  debian:
+    buster: [ros-noetic-rqt-gui]
+rqt_logger_level:
+  debian:
+    buster: [ros-noetic-rqt-logger-level]
+rqt_moveit:
+  debian:
+    buster: [ros-noetic-rqt-moveit]
+rqt_msg:
+  debian:
+    buster: [ros-noetic-rqt-msg]
+rqt_nav_view:
+  debian:
+    buster: [ros-noetic-rqt-nav-view]
+rqt_plot:
+  debian:
+    buster: [ros-noetic-rqt-plot]
+rqt_pose_view:
+  debian:
+    buster: [ros-noetic-rqt-pose-view]
+rqt_publisher:
+  debian:
+    buster: [ros-noetic-rqt-publisher]
+rqt_py_console:
+  debian:
+    buster: [ros-noetic-rqt-py-console]
+rqt_reconfigure:
+  debian:
+    buster: [ros-noetic-rqt-reconfigure]
+rqt_robot_dashboard:
+  debian:
+    buster: [ros-noetic-rqt-robot-dashboard]
+rqt_robot_monitor:
+  debian:
+    buster: [ros-noetic-rqt-robot-monitor]
+rqt_robot_plugins:
+  debian:
+    buster: [ros-noetic-rqt-robot-plugins]
+rqt_robot_steering:
+  debian:
+    buster: [ros-noetic-rqt-robot-steering]
+rqt_runtime_monitor:
+  debian:
+    buster: [ros-noetic-rqt-runtime-monitor]
+rqt_service_caller:
+  debian:
+    buster: [ros-noetic-rqt-service-caller]
+rqt_shell:
+  debian:
+    buster: [ros-noetic-rqt-shell]
+rqt_srv:
+  debian:
+    buster: [ros-noetic-rqt-srv]
+rqt_tf_tree:
+  debian:
+    buster: [ros-noetic-rqt-tf-tree]
+rqt_top:
+  debian:
+    buster: [ros-noetic-rqt-top]
+rqt_topic:
+  debian:
+    buster: [ros-noetic-rqt-topic]
+rqt_web:
+  debian:
+    buster: [ros-noetic-rqt-web]
+smach:
+  debian:
+    buster: [ros-noetic-smach]
+smclib:
+  debian:
+    buster: [ros-noetic-smclib]
+std_msgs:
+  debian:
+    buster: [ros-noetic-std-msgs]
+actionlib_msgs:
+  debian:
+    buster: [ros-noetic-actionlib-msgs]
+bond:
+  debian:
+    buster: [ros-noetic-bond]
+diagnostic_msgs:
+  debian:
+    buster: [ros-noetic-diagnostic-msgs]
+geometry_msgs:
+  debian:
+    buster: [ros-noetic-geometry-msgs]
+eigen_conversions:
+  debian:
+    buster: [ros-noetic-eigen-conversions]
+kdl_conversions:
+  debian:
+    buster: [ros-noetic-kdl-conversions]
+nav_msgs:
+  debian:
+    buster: [ros-noetic-nav-msgs]
+rosbridge_msgs:
+  debian:
+    buster: [ros-noetic-rosbridge-msgs]
+rosgraph_msgs:
+  debian:
+    buster: [ros-noetic-rosgraph-msgs]
+rosmsg:
+  debian:
+    buster: [ros-noetic-rosmsg]
+rqt_py_common:
+  debian:
+    buster: [ros-noetic-rqt-py-common]
+shape_msgs:
+  debian:
+    buster: [ros-noetic-shape-msgs]
+smach_msgs:
+  debian:
+    buster: [ros-noetic-smach-msgs]
+std_srvs:
+  debian:
+    buster: [ros-noetic-std-srvs]
+tf2_msgs:
+  debian:
+    buster: [ros-noetic-tf2-msgs]
+tf2:
+  debian:
+    buster: [ros-noetic-tf2]
+tf2_eigen:
+  debian:
+    buster: [ros-noetic-tf2-eigen]
+trajectory_msgs:
+  debian:
+    buster: [ros-noetic-trajectory-msgs]
+control_msgs:
+  debian:
+    buster: [ros-noetic-control-msgs]
+urdf_parser_plugin:
+  debian:
+    buster: [ros-noetic-urdf-parser-plugin]
+urdfdom_py:
+  debian:
+    buster: [ros-noetic-urdfdom-py]
+uuid_msgs:
+  debian:
+    buster: [ros-noetic-uuid-msgs]
+geographic_msgs:
+  debian:
+    buster: [ros-noetic-geographic-msgs]
+vision_opencv:
+  debian:
+    buster: [ros-noetic-vision-opencv]
+visualization_msgs:
+  debian:
+    buster: [ros-noetic-visualization-msgs]
+visualization_tutorials:
+  debian:
+    buster: [ros-noetic-visualization-tutorials]
+viz:
+  debian:
+    buster: [ros-noetic-viz]
+webkit_dependency:
+  debian:
+    buster: [ros-noetic-webkit-dependency]
+xmlrpcpp:
+  debian:
+    buster: [ros-noetic-xmlrpcpp]
+roscpp:
+  debian:
+    buster: [ros-noetic-roscpp]
+bondcpp:
+  debian:
+    buster: [ros-noetic-bondcpp]
+bondpy:
+  debian:
+    buster: [ros-noetic-bondpy]
+nodelet:
+  debian:
+    buster: [ros-noetic-nodelet]
+nodelet_tutorial_math:
+  debian:
+    buster: [ros-noetic-nodelet-tutorial-math]
+pluginlib_tutorials:
+  debian:
+    buster: [ros-noetic-pluginlib-tutorials]
+roscpp_tutorials:
+  debian:
+    buster: [ros-noetic-roscpp-tutorials]
+rosout:
+  debian:
+    buster: [ros-noetic-rosout]
+camera_calibration:
+  debian:
+    buster: [ros-noetic-camera-calibration]
+diagnostic_aggregator:
+  debian:
+    buster: [ros-noetic-diagnostic-aggregator]
+diagnostic_updater:
+  debian:
+    buster: [ros-noetic-diagnostic-updater]
+diagnostic_common_diagnostics:
+  debian:
+    buster: [ros-noetic-diagnostic-common-diagnostics]
+dynamic_reconfigure:
+  debian:
+    buster: [ros-noetic-dynamic-reconfigure]
+filters:
+  debian:
+    buster: [ros-noetic-filters]
+joint_state_publisher:
+  debian:
+    buster: [ros-noetic-joint-state-publisher]
+message_filters:
+  debian:
+    buster: [ros-noetic-message-filters]
+rosauth:
+  debian:
+    buster: [ros-noetic-rosauth]
+rosbag_storage:
+  debian:
+    buster: [ros-noetic-rosbag-storage]
+rosnode:
+  debian:
+    buster: [ros-noetic-rosnode]
+rospy_tutorials:
+  debian:
+    buster: [ros-noetic-rospy-tutorials]
+rosshow:
+  debian:
+    buster: [ros-noetic-rosshow]
+rostopic:
+  debian:
+    buster: [ros-noetic-rostopic]
+rqt_gui_cpp:
+  debian:
+    buster: [ros-noetic-rqt-gui-cpp]
+rqt_gui_py:
+  debian:
+    buster: [ros-noetic-rqt-gui-py]
+self_test:
+  debian:
+    buster: [ros-noetic-self-test]
+smach_ros:
+  debian:
+    buster: [ros-noetic-smach-ros]
+tf2_py:
+  debian:
+    buster: [ros-noetic-tf2-py]
+topic_tools:
+  debian:
+    buster: [ros-noetic-topic-tools]
+rosbag:
+  debian:
+    buster: [ros-noetic-rosbag]
+actionlib:
+  debian:
+    buster: [ros-noetic-actionlib]
+actionlib_tutorials:
+  debian:
+    buster: [ros-noetic-actionlib-tutorials]
+diagnostic_analysis:
+  debian:
+    buster: [ros-noetic-diagnostic-analysis]
+nodelet_topic_tools:
+  debian:
+    buster: [ros-noetic-nodelet-topic-tools]
+roswtf:
+  debian:
+    buster: [ros-noetic-roswtf]
+rqt_launch:
+  debian:
+    buster: [ros-noetic-rqt-launch]
+sensor_msgs:
+  debian:
+    buster: [ros-noetic-sensor-msgs]
+camera_calibration_parsers:
+  debian:
+    buster: [ros-noetic-camera-calibration-parsers]
+cv_bridge:
+  debian:
+    buster: [ros-noetic-cv-bridge]
+image_geometry:
+  debian:
+    buster: [ros-noetic-image-geometry]
+image_transport:
+  debian:
+    buster: [ros-noetic-image-transport]
+camera_info_manager:
+  debian:
+    buster: [ros-noetic-camera-info-manager]
+compressed_depth_image_transport:
+  debian:
+    buster: [ros-noetic-compressed-depth-image-transport]
+compressed_image_transport:
+  debian:
+    buster: [ros-noetic-compressed-image-transport]
+cv_camera:
+  debian:
+    buster: [ros-noetic-cv-camera]
+image_proc:
+  debian:
+    buster: [ros-noetic-image-proc]
+image_publisher:
+  debian:
+    buster: [ros-noetic-image-publisher]
+map_msgs:
+  debian:
+    buster: [ros-noetic-map-msgs]
+mavros_msgs:
+  debian:
+    buster: [ros-noetic-mavros-msgs]
+pcl_msgs:
+  debian:
+    buster: [ros-noetic-pcl-msgs]
+pcl_conversions:
+  debian:
+    buster: [ros-noetic-pcl-conversions]
+polled_camera:
+  debian:
+    buster: [ros-noetic-polled-camera]
+rqt_image_view:
+  debian:
+    buster: [ros-noetic-rqt-image-view]
+stereo_msgs:
+  debian:
+    buster: [ros-noetic-stereo-msgs]
+image_view:
+  debian:
+    buster: [ros-noetic-image-view]
+rosbridge_library:
+  debian:
+    buster: [ros-noetic-rosbridge-library]
+stereo_image_proc:
+  debian:
+    buster: [ros-noetic-stereo-image-proc]
+tf2_ros:
+  debian:
+    buster: [ros-noetic-tf2-ros]
+depth_image_proc:
+  debian:
+    buster: [ros-noetic-depth-image-proc]
+mavros:
+  debian:
+    buster: [ros-noetic-mavros]
+tf:
+  debian:
+    buster: [ros-noetic-tf]
+interactive_markers:
+  debian:
+    buster: [ros-noetic-interactive-markers]
+interactive_marker_tutorials:
+  debian:
+    buster: [ros-noetic-interactive-marker-tutorials]
+laser_geometry:
+  debian:
+    buster: [ros-noetic-laser-geometry]
+laser_assembler:
+  debian:
+    buster: [ros-noetic-laser-assembler]
+laser_filters:
+  debian:
+    buster: [ros-noetic-laser-filters]
+pcl_ros:
+  debian:
+    buster: [ros-noetic-pcl-ros]
+tf2_geometry_msgs:
+  debian:
+    buster: [ros-noetic-tf2-geometry-msgs]
+image_rotate:
+  debian:
+    buster: [ros-noetic-image-rotate]
+tf2_kdl:
+  debian:
+    buster: [ros-noetic-tf2-kdl]
+tf2_web_republisher:
+  debian:
+    buster: [ros-noetic-tf2-web-republisher]
+tf_conversions:
+  debian:
+    buster: [ros-noetic-tf-conversions]
+theora_image_transport:
+  debian:
+    buster: [ros-noetic-theora-image-transport]
+turtlesim:
+  debian:
+    buster: [ros-noetic-turtlesim]
+turtle_actionlib:
+  debian:
+    buster: [ros-noetic-turtle-actionlib]
+turtle_tf:
+  debian:
+    buster: [ros-noetic-turtle-tf]
+turtle_tf2:
+  debian:
+    buster: [ros-noetic-turtle-tf2]
+urdf:
+  debian:
+    buster: [ros-noetic-urdf]
+kdl_parser:
+  debian:
+    buster: [ros-noetic-kdl-parser]
+kdl_parser_py:
+  debian:
+    buster: [ros-noetic-kdl-parser-py]
+mavros_extras:
+  debian:
+    buster: [ros-noetic-mavros-extras]
+robot_state_publisher:
+  debian:
+    buster: [ros-noetic-robot-state-publisher]
+rviz:
+  debian:
+    buster: [ros-noetic-rviz]
+librviz_tutorial:
+  debian:
+    buster: [ros-noetic-librviz-tutorial]
+rqt_rviz:
+  debian:
+    buster: [ros-noetic-rqt-rviz]
+rviz_plugin_tutorials:
+  debian:
+    buster: [ros-noetic-rviz-plugin-tutorials]
+rviz_python_tutorial:
+  debian:
+    buster: [ros-noetic-rviz-python-tutorial]
+urdf_tutorial:
+  debian:
+    buster: [ros-noetic-urdf-tutorial]
+usb_cam:
+  debian:
+    buster: [ros-noetic-usb-cam]
+visualization_marker_tutorials:
+  debian:
+    buster: [ros-noetic-visualization-marker-tutorials]
+vl53l1x:
+  debian:
+    buster: [ros-noetic-vl53l1x]
+xacro:
+  debian:
+    buster: [ros-noetic-xacro]
+ddynamic_reconfigure:
+  debian:
+    buster: [ros-noetic-ddynamic-reconfigure]
+librealsense2:
+  debian:
+    buster: [ros-noetic-librealsense2]
+realsense2_camera:
+  debian:
+    buster: [ros-noetic-realsense2-camera]
+realsense2_description:
+  debian:
+    buster: [ros-noetic-realsense2-description]
+geographiclib:
+  debian:
+    buster: [libgeographic-dev]

builder/image-build.sh

@@ -13,7 +13,7 @@
 # copies or substantial portions of the Software.
 #
 
-set -e # Exit immidiately on non-zero result
+set -ex # exit on error, echo commands
 
 # https://www.raspberrypi.org/software/operating-systems/#raspberry-pi-os-32-bit
 SOURCE_IMAGE="https://downloads.raspberrypi.org/raspios_lite_armhf/images/raspios_lite_armhf-2021-05-28/2021-05-07-raspios-buster-armhf-lite.zip"
@@ -22,33 +22,13 @@ export DEBIAN_FRONTEND=${DEBIAN_FRONTEND:='noninteractive'}
 export LANG=${LANG:='C.UTF-8'}
 export LC_ALL=${LC_ALL:='C.UTF-8'}
 
-echo_stamp() {
-  # TEMPLATE: echo_stamp <TEXT> <TYPE>
-  # TYPE: SUCCESS, ERROR, INFO
-
-  # More info there https://www.shellhacks.com/ru/bash-colors/
-
-  TEXT="$(date '+[%Y-%m-%d %H:%M:%S]') $1"
-  TEXT="\e[1m$TEXT\e[0m" # BOLD
-
-  case "$2" in
-    SUCCESS)
-    TEXT="\e[32m${TEXT}\e[0m";; # GREEN
-    ERROR)
-    TEXT="\e[31m${TEXT}\e[0m";; # RED
-    *)
-    TEXT="\e[34m${TEXT}\e[0m";; # BLUE
-  esac
-  echo -e ${TEXT}
-}
-
 BUILDER_DIR="/builder"
 REPO_DIR="${BUILDER_DIR}/repo"
 SCRIPTS_DIR="${REPO_DIR}/builder"
 IMAGES_DIR="${REPO_DIR}/images"
 
-[[ ! -d ${SCRIPTS_DIR} ]] && (echo_stamp "Directory ${SCRIPTS_DIR} doesn't exist" "ERROR"; exit 1)
+[[ ! -d ${SCRIPTS_DIR} ]] && (echo "Error: directory ${SCRIPTS_DIR} doesn't exist"; exit 1)
-[[ ! -d ${IMAGES_DIR} ]] && mkdir ${IMAGES_DIR} && echo_stamp "Directory ${IMAGES_DIR} was created successful" "SUCCESS"
+[[ ! -d ${IMAGES_DIR} ]] && mkdir ${IMAGES_DIR} && echo "Directory ${IMAGES_DIR} was created successful"
 
 if [[ -z ${TRAVIS_TAG} ]]; then IMAGE_VERSION="$(cd ${REPO_DIR}; git log --format=%h -1)"; else IMAGE_VERSION="${TRAVIS_TAG}"; fi
 # IMAGE_VERSION="${TRAVIS_TAG:=$(cd ${REPO_DIR}; git log --format=%h -1)}"
@@ -64,15 +44,15 @@ get_image() {
   local RPI_IMAGE_NAME=$(echo ${RPI_ZIP_NAME} | sed 's/zip/img/')
 
   if [ ! -e "${BUILD_DIR}/${RPI_ZIP_NAME}" ]; then
-    echo_stamp "Downloading original Linux distribution"
+    echo "--- Downloading original Linux distribution"
     wget --progress=dot:giga -O ${BUILD_DIR}/${RPI_ZIP_NAME} $2
-    echo_stamp "Downloading complete" "SUCCESS" \
+    echo "--- Downloading complete" "SUCCESS"
-  else echo_stamp "Linux distribution already donwloaded"; fi
+  else
+    echo "Linux distribution already downloaded"
+  fi
 
-  echo_stamp "Unzipping Linux distribution image" \
+  echo "--- Unzipping Linux distribution image"
-  && unzip -p ${BUILD_DIR}/${RPI_ZIP_NAME} ${RPI_IMAGE_NAME} > $1 \
+  unzip -p ${BUILD_DIR}/${RPI_ZIP_NAME} ${RPI_IMAGE_NAME} > $1
-  && echo_stamp "Unzipping complete" "SUCCESS" \
-  || (echo_stamp "Unzipping was failed!" "ERROR"; exit 1)
 }
 
 get_image ${IMAGE_PATH} ${SOURCE_IMAGE}
@@ -105,8 +85,6 @@ ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/butterf
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/monkey.service' '/lib/systemd/system/'
 # software install
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} exec ${SCRIPTS_DIR}'/image-software.sh'
-# examples
-${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/examples' '/home/pi/'  # TODO: symlink?
 # network setup
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} exec ${SCRIPTS_DIR}'/image-network.sh'
 # avahi setup
@@ -115,15 +93,11 @@ ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/avahi-s
 # If RPi then use a one thread to build a ROS package on RPi, else use all
 [[ $(arch) == 'armv7l' ]] && NUMBER_THREADS=1 || NUMBER_THREADS=$(nproc --all)
 # Clover
-${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/clover.service' '/lib/systemd/system/'
-${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/roscore.service' '/lib/systemd/system/'
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/noetic-rosdep-clover.yaml' '/etc/ros/rosdep/'
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/ros_python_paths' '/etc/sudoers.d/'
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/pigpiod.service' '/lib/systemd/system/'
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/launch.nanorc' '/usr/share/nano/'
 # ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/kinetic-ros-clover.rosinstall' '/home/pi/ros_catkin_ws/'
-# Add PX4 udev rules
-${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${REPO_DIR}'/clover/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

@@ -12,50 +12,29 @@
 # copies or substantial portions of the Software.
 #
 
-set -e # Exit immidiately on non-zero result
+set -ex # exit on error, echo commands
-
-echo_stamp() {
-  # TEMPLATE: echo_stamp <TEXT> <TYPE>
-  # TYPE: SUCCESS, ERROR, INFO
-
-  # More info there https://www.shellhacks.com/ru/bash-colors/
-
-  TEXT="$(date '+[%Y-%m-%d %H:%M:%S]') $1"
-  TEXT="\e[1m$TEXT\e[0m" # BOLD
-
-  case "$2" in
-    SUCCESS)
-    TEXT="\e[32m${TEXT}\e[0m";; # GREEN
-    ERROR)
-    TEXT="\e[31m${TEXT}\e[0m";; # RED
-    *)
-    TEXT="\e[34m${TEXT}\e[0m";; # BLUE
-  esac
-  echo -e ${TEXT}
-}
-
-echo_stamp "Write Clover information"
 
+echo "--- Write Clover information"
 # Clover image version
 echo "$1" >> /etc/clover_version
 # Origin image file name
 echo "${2%.*}" >> /etc/clover_origin
 
-echo_stamp "Write magic script to /etc/rc.local"
+echo "--- Write magic script to /etc/rc.local"
 MAGIC_SCRIPT="sudo /root/init_rpi.sh; sudo sed -i '/sudo \\\/root\\\/init_rpi.sh/d' /etc/rc.local && sudo reboot"
 sed -i "19a${MAGIC_SCRIPT}" /etc/rc.local
 
 # It needs for autosizer.sh & maybe that is correct
-echo_stamp "Change boot partition"
+echo "--- Change boot partition"
 sed -i 's/root=[^ ]*/root=\/dev\/mmcblk0p2/' /boot/cmdline.txt
 sed -i 's/.*  \/boot           vfat    defaults          0       2$/\/dev\/mmcblk0p1  \/boot           vfat    defaults          0       2/' /etc/fstab
 sed -i 's/.*  \/               ext4    defaults,noatime  0       1$/\/dev\/mmcblk0p2  \/               ext4    defaults,noatime  0       1/' /etc/fstab
 
-echo_stamp "Set max space for syslogs"
+echo "--- Set max space for syslogs"
 # https://unix.stackexchange.com/questions/139513/how-to-clear-journalctl
 sed -i 's/#SystemMaxUse=/SystemMaxUse=200M/' /etc/systemd/journald.conf
 
-echo_stamp "Move /etc/ld.so.preload out of the way"
+echo "--- Move /etc/ld.so.preload out of the way"
 mv /etc/ld.so.preload /etc/ld.so.preload.disabled-for-build
 
-echo_stamp "End of init image"
+echo "--- End of init image"

builder/image-network.sh

@@ -12,43 +12,20 @@
 # copies or substantial portions of the Software.
 #
 
-set -e # Exit immidiately on non-zero result
+set -ex # exit on error, echo commands
-
-echo_stamp() {
-  # TEMPLATE: echo_stamp <TEXT> <TYPE>
-  # TYPE: SUCCESS, ERROR, INFO
-
-  # More info there https://www.shellhacks.com/ru/bash-colors/
-
-  TEXT="$(date '+[%Y-%m-%d %H:%M:%S]') $1"
-  TEXT="\e[1m$TEXT\e[0m" # BOLD
-
-  case "$2" in
-    SUCCESS)
-    TEXT="\e[32m${TEXT}\e[0m";; # GREEN
-    ERROR)
-    TEXT="\e[31m${TEXT}\e[0m";; # RED
-    *)
-    TEXT="\e[34m${TEXT}\e[0m";; # BLUE
-  esac
-  echo -e ${TEXT}
-}
-
-echo_stamp "#1 Write STATIC to /etc/dhcpcd.conf"
 
+echo "--- Write static to /etc/dhcpcd.conf"
 cat << EOF >> /etc/dhcpcd.conf
 interface wlan0
 static ip_address=192.168.11.1/24
 EOF
 
-echo_stamp "#2 Set wpa_supplicant country"
+echo "--- Set wpa_supplicant country"
-
 cat << EOF >> /etc/wpa_supplicant/wpa_supplicant.conf
 country=GB
 EOF
 
-echo_stamp "#3 Write dhcp-config to /etc/dnsmasq.conf"
+echo "--- Write dhcp-config to /etc/dnsmasq.conf"
-
 cat << EOF >> /etc/dnsmasq.conf
 interface=wlan0
 address=/clover/coex/192.168.11.1
@@ -59,5 +36,3 @@ bogus-priv
 domain-needed
 quiet-dhcp6
 EOF
-
-echo_stamp "#4 End of network installation"

builder/image-ros.sh

@@ -13,43 +13,17 @@
 # copies or substantial portions of the Software.
 #
 
-set -e # Exit immidiately on non-zero result
+set -ex # exit on error, echo commands
-
-REPO=$1
-REF=$2
-INSTALL_ROS_PACK_SOURCES=$3
-DISCOVER_ROS_PACK=$4
-NUMBER_THREADS=$5
 
 # Current ROS distribution
 ROS_DISTRO=noetic
 
-echo_stamp() {
-  # TEMPLATE: echo_stamp <TEXT> <TYPE>
-  # TYPE: SUCCESS, ERROR, INFO
-
-  # More info there https://www.shellhacks.com/ru/bash-colors/
-
-  TEXT="$(date '+[%Y-%m-%d %H:%M:%S]') $1"
-  TEXT="\e[1m$TEXT\e[0m" # BOLD
-
-  case "$2" in
-    SUCCESS)
-    TEXT="\e[32m${TEXT}\e[0m";; # GREEN
-    ERROR)
-    TEXT="\e[31m${TEXT}\e[0m";; # RED
-    *)
-    TEXT="\e[34m${TEXT}\e[0m";; # BLUE
-  esac
-  echo -e ${TEXT}
-}
-
 # https://gist.github.com/letmaik/caa0f6cc4375cbfcc1ff26bd4530c2a3
 # https://github.com/travis-ci/travis-build/blob/master/lib/travis/build/templates/header.sh
 my_travis_retry() {
   local result=0
   local count=1
-  local max_count=50
+  local max_count=5
   while [ $count -le $max_count ]; do
     [ $result -ne 0 ] && {
       echo -e "\nThe command \"$@\" failed. Retrying, $count of $max_count.\n" >&2
@@ -69,28 +43,28 @@ my_travis_retry() {
 }
 
 # TODO: 'kinetic-rosdep-clover.yaml' should add only if we use our repo?
-echo_stamp "Init rosdep"
+echo "--- Init rosdep"
 my_travis_retry rosdep init
 # FIXME: Re-add this after missing packages are built
-echo "yaml file:///etc/ros/rosdep/${ROS_DISTRO}-rosdep-clover.yaml" >> /etc/ros/rosdep/sources.list.d/20-default.list
+echo "yaml file:///etc/ros/rosdep/${ROS_DISTRO}-rosdep-clover.yaml" >> /etc/ros/rosdep/sources.list.d/10-clover.list
 my_travis_retry rosdep update
 
-echo_stamp "Populate rosdep for ROS user"
+echo "--- Populate rosdep for ROS user"
 my_travis_retry sudo -u pi rosdep update
 
 export ROS_IP='127.0.0.1' # needed for running tests
 
-# echo_stamp "Reconfiguring Clover repository for simplier unshallowing"
+# echo "Reconfiguring Clover repository for simplier unshallowing"
 cd /home/pi/catkin_ws/src/clover
 git config remote.origin.fetch "+refs/heads/*:refs/remotes/origin/*"
 
 # This is sort of a hack to force "custom" packages to be installed - the ones built by COEX, linked against OpenCV 4.2
 # I **wish** OpenCV would not be such a mess, but, well, here we are.
-echo_stamp "Installing OpenCV 4.2-compatible ROS packages"
+echo "--- Installing OpenCV 4.2-compatible ROS packages"
 apt install -y --no-install-recommends \
 ros-${ROS_DISTRO}-compressed-image-transport=1.14.0-0buster \
 ros-${ROS_DISTRO}-cv-bridge=1.15.0-0buster \
-ros-${ROS_DISTRO}-cv-camera=0.5.0-0buster \
+ros-${ROS_DISTRO}-cv-camera=0.5.1-0buster \
 ros-${ROS_DISTRO}-image-publisher=1.15.3-0buster \
 ros-${ROS_DISTRO}-web-video-server=0.2.1-0buster
 apt-mark hold \
@@ -100,10 +74,10 @@ ros-${ROS_DISTRO}-cv-camera \
 ros-${ROS_DISTRO}-image-publisher \
 ros-${ROS_DISTRO}-web-video-server
 
-echo_stamp "Installing libboost-dev" # https://travis-ci.org/github/CopterExpress/clover/jobs/766318908#L6536
+echo "--- Installing libboost-dev" # https://travis-ci.org/github/CopterExpress/clover/jobs/766318908#L6536
 my_travis_retry apt-get install -y --no-install-recommends libboost-dev libboost-all-dev
 
-echo_stamp "Build and install Clover"
+echo "--- Build and install Clover"
 cd /home/pi/catkin_ws
 # Don't try to install gazebo_ros
 my_travis_retry rosdep install -y --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} --os=debian:buster \
@@ -112,24 +86,25 @@ my_travis_retry pip3 install wheel
 my_travis_retry pip3 install -r /home/pi/catkin_ws/src/clover/clover/requirements.txt
 source /opt/ros/${ROS_DISTRO}/setup.bash
 # Don't build simulation plugins for actual drone
-catkin_make -j2 -DCMAKE_BUILD_TYPE=RelWithDebInfo -DCATKIN_BLACKLIST_PACKAGES=clover_gazebo_plugins
+catkin_make -j2 -DCMAKE_BUILD_TYPE=RelWithDebInfo
+source devel/setup.bash
 
-echo_stamp "Install clever package (for backwards compatibility)"
+echo "--- Install clever package (for backwards compatibility)"
 cd /home/pi/catkin_ws/src/clover/builder/assets/clever
 ./setup.py install
 rm -rf build  # remove build artifacts
 
-echo_stamp "Build Clover documentation"
+echo "--- Build Clover documentation"
 cd /home/pi/catkin_ws/src/clover
-NPM_CONFIG_UNSAFE_PERM=true npm install gitbook-cli -g
+builder/assets/install_gitbook.sh
-NPM_CONFIG_UNSAFE_PERM=true gitbook fetch 3.2.3 && npm i npm@3.10.10 --prefix=~/.gitbook/versions/3.2.3/ # fixing https://travis-ci.org/github/CopterExpress/clover/jobs/766541125#L932
+gitbook install
-NPM_CONFIG_UNSAFE_PERM=true gitbook install
 gitbook build
+# replace assets copy to assets symlink to save space
+rm -rf _book/assets && ln -s ../docs/assets _book/assets
 touch node_modules/CATKIN_IGNORE docs/CATKIN_IGNORE _book/CATKIN_IGNORE clover/www/CATKIN_IGNORE apps/CATKIN_IGNORE # ignore documentation files by catkin
 
-echo_stamp "Installing additional ROS packages"
+echo "--- Installing additional ROS packages"
 my_travis_retry apt-get install -y --no-install-recommends \
-    ros-${ROS_DISTRO}-dynamic-reconfigure \
     ros-${ROS_DISTRO}-rosbridge-suite \
     ros-${ROS_DISTRO}-rosserial \
     ros-${ROS_DISTRO}-usb-cam \
@@ -137,24 +112,40 @@ my_travis_retry apt-get install -y --no-install-recommends \
     ros-${ROS_DISTRO}-ws281x \
     ros-${ROS_DISTRO}-rosshow \
     ros-${ROS_DISTRO}-cmake-modules \
-    ros-${ROS_DISTRO}-image-view
+    ros-${ROS_DISTRO}-image-view \
+    ros-${ROS_DISTRO}-nodelet-topic-tools \
+    ros-${ROS_DISTRO}-stereo-msgs
 
 # TODO move GeographicLib datasets to Mavros debian package
-echo_stamp "Install GeographicLib datasets (needed for mavros)" \
+echo "--- 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"
+echo "--- Running tests"
 cd /home/pi/catkin_ws
 # FIXME: Investigate failing tests
 catkin_make run_tests #&& catkin_test_results
 
-echo_stamp "Change permissions for catkin_ws"
+echo "--- Change permissions for catkin_ws"
 chown -Rf pi:pi /home/pi/catkin_ws
 
-echo_stamp "Change permissions for examples"
+echo "--- Update www"
+sudo -u pi sh -c ". devel/setup.sh && rosrun clover www"
+
+echo "--- Make \$HOME/examples symlink"
+ln -s "$(catkin_find clover examples --first-only)" /home/pi
 chown -Rf pi:pi /home/pi/examples
 
-echo_stamp "Setup ROS environment"
+echo "--- Make systemd services symlinks"
+ln -s /home/pi/catkin_ws/src/clover/builder/assets/clover.service /lib/systemd/system/
+ln -s /home/pi/catkin_ws/src/clover/builder/assets/roscore.service /lib/systemd/system/
+# validate
+[ -f /lib/systemd/system/clover.service ]
+[ -f /lib/systemd/system/roscore.service ]
+
+echo "--- Make udev rules symlink"
+ln -s "$(catkin_find clover udev --first-only)"/* /lib/udev/rules.d/
+
+echo "--- Setup ROS environment"
 cat << EOF >> /home/pi/.bashrc
 LANG='C.UTF-8'
 LC_ALL='C.UTF-8'
@@ -163,12 +154,5 @@ source /opt/ros/${ROS_DISTRO}/setup.bash
 source /home/pi/catkin_ws/devel/setup.bash
 EOF
 
-#echo_stamp "Removing local apt mirror"
+echo "--- Clean apt cache"
-# Restore original sources.list
-#mv /var/sources.list.bak /etc/apt/sources.list
-# Clean apt cache
 apt-get clean -qq > /dev/null
-# Remove local mirror repository key
-#apt-key del COEX-MIRROR
-
-echo_stamp "END of ROS INSTALLATION"

builder/image-software.sh

@@ -12,27 +12,7 @@
 # copies or substantial portions of the Software.
 #
 
-set -e # Exit immidiately on non-zero result
+set -ex # exit on error, echo commands
-
-echo_stamp() {
-  # TEMPLATE: echo_stamp <TEXT> <TYPE>
-  # TYPE: SUCCESS, ERROR, INFO
-
-  # More info there https://www.shellhacks.com/ru/bash-colors/
-
-  TEXT="$(date '+[%Y-%m-%d %H:%M:%S]') $1"
-  TEXT="\e[1m${TEXT}\e[0m" # BOLD
-
-  case "$2" in
-    SUCCESS)
-    TEXT="\e[32m${TEXT}\e[0m";; # GREEN
-    ERROR)
-    TEXT="\e[31m${TEXT}\e[0m";; # RED
-    *)
-    TEXT="\e[34m${TEXT}\e[0m";; # BLUE
-  esac
-  echo -e ${TEXT}
-}
 
 # https://gist.github.com/letmaik/caa0f6cc4375cbfcc1ff26bd4530c2a3
 # https://github.com/travis-ci/travis-build/blob/master/lib/travis/build/templates/header.sh
@@ -57,11 +37,10 @@ my_travis_retry() {
   return $result
 }
 
-echo_stamp "Increase apt retries"
+echo "--- Increase apt retries"
-
 echo "APT::Acquire::Retries \"3\";" > /etc/apt/apt.conf.d/80-retries
 
-echo_stamp "Install apt keys & repos"
+echo "--- Install apt keys & repos"
 
 # TODO: This STDOUT consist 'OK'
 apt-get update \
@@ -73,14 +52,14 @@ echo "deb http://packages.ros.org/ros/ubuntu buster main" > /etc/apt/sources.lis
 wget -O - 'http://packages.coex.tech/key.asc' | apt-key add -
 echo 'deb http://packages.coex.tech buster main' >> /etc/apt/sources.list
 
-echo_stamp "Update apt cache"
+echo "--- Update apt cache"
 
 # TODO: FIX ERROR: /usr/bin/apt-key: 596: /usr/bin/apt-key: cannot create /dev/null: Permission denied
 apt-get update
 # && apt upgrade -y
 
 # Let's retry fetching those packages several times, just in case
-echo_stamp "Software installing"
+echo "--- Install software"
 my_travis_retry apt-get install --no-install-recommends -y cmake-data=3.13.4-1 cmake=3.13.4-1 # FIXME: using older CMake due to https://travis-ci.org/github/CopterExpress/clover/jobs/764367665#L6984
 my_travis_retry apt-get install --no-install-recommends -y \
 unzip \
@@ -121,7 +100,7 @@ python3-opencv
 sed -i "s/updates_available//" /usr/share/byobu/status/status
 # sed -i "s/updates_available//" /home/pi/.byobu/status
 
-echo_stamp "Installing pip"
+echo "--- Installing pip"
 curl https://bootstrap.pypa.io/get-pip.py -o get-pip.py
 curl https://bootstrap.pypa.io/pip/2.7/get-pip.py -o get-pip2.py
 python3 get-pip.py
@@ -130,28 +109,30 @@ rm get-pip.py get-pip2.py
 #my_travis_retry pip install --upgrade pip
 #my_travis_retry pip3 install --upgrade pip
 
-echo_stamp "Make sure both pip and pip3 are installed"
+echo "--- Make sure both pip and pip3 are installed"
 pip --version
 pip3 --version
 
-echo_stamp "Install and enable Butterfly (web terminal)"
+echo "--- Install and enable Butterfly (web terminal)"
-echo_stamp "Workaround for tornado >= 6.0 breaking butterfly"
+echo "Workaround for tornado >= 6.0 breaking butterfly"
 export CRYPTOGRAPHY_DONT_BUILD_RUST=1
+my_travis_retry pip3 install cryptography==3.4.6 # https://stackoverflow.com/a/68472128/6850197
+my_travis_retry pip3 install pyOpenSSL==20.0.1
 my_travis_retry pip3 install tornado==5.1.1
 my_travis_retry pip3 install butterfly
 my_travis_retry pip3 install butterfly[systemd]
 systemctl enable butterfly.socket
 
-echo_stamp "Install ws281x library"
+echo "--- Install ws281x library"
 my_travis_retry pip3 install --prefer-binary rpi_ws281x
 
-echo_stamp "Setup Monkey"
+echo "--- Setup Monkey"
 mv /etc/monkey/sites/default /etc/monkey/sites/default.orig
 mv /root/monkey /etc/monkey/sites/default
 sed -i 's/SymLink Off/SymLink On/' /etc/monkey/monkey.conf
 systemctl enable monkey.service
 
-echo_stamp "Install Node.js"
+echo "--- Install Node.js"
 cd /home/pi
 wget --no-verbose https://nodejs.org/dist/v10.15.0/node-v10.15.0-linux-armv6l.tar.gz
 tar -xzf node-v10.15.0-linux-armv6l.tar.gz
@@ -159,28 +140,26 @@ cp -R node-v10.15.0-linux-armv6l/* /usr/local/
 rm -rf node-v10.15.0-linux-armv6l/
 rm node-v10.15.0-linux-armv6l.tar.gz
 
-echo_stamp "Installing ptvsd"
+echo "--- Installing ptvsd"
 my_travis_retry pip install ptvsd
 my_travis_retry pip3 install ptvsd
 
-echo_stamp "Installing pyzbar"
+echo "--- Installing pyzbar"
 my_travis_retry pip install pyzbar
 my_travis_retry pip3 install pyzbar
 
-echo_stamp "Add .vimrc"
+echo "--- Add .vimrc"
 cat << EOF > /home/pi/.vimrc
 set mouse-=a
 syntax on
 autocmd BufNewFile,BufRead *.launch set syntax=xml
 EOF
 
-echo_stamp "Change default keyboard layout to US"
+echo "--- Change default keyboard layout to US"
 sed -i 's/XKBLAYOUT="gb"/XKBLAYOUT="us"/g' /etc/default/keyboard
 
-echo_stamp "Attempting to kill dirmngr"
+echo "--- Attempting to kill dirmngr"
 gpgconf --kill dirmngr
 # dirmngr is only used by apt-key, so we can safely kill it.
 # We ignore pkill's exit value as well.
 pkill -9 -f dirmngr || true
-
-echo_stamp "End of software installation"

builder/image-validate.sh

@@ -12,9 +12,10 @@
 # copies or substantial portions of the Software.
 #
 
-set -ex
+set -ex # exit on error, echo commands
 
-echo "Run image tests"
+
+echo "--- Run image tests"
 
 export ROS_DISTRO='noetic'
 export ROS_IP='127.0.0.1'
@@ -31,5 +32,13 @@ cd /home/pi/catkin_ws/src/clover/builder/test/
 
 systemctl stop roscore
 
-echo "Move /etc/ld.so.preload back to its original position"
+# check documented packages available
+apt-cache show gst-rtsp-launch
+apt-cache show openvpn
+
+echo "--- Move /etc/ld.so.preload back to its original position"
 mv /etc/ld.so.preload.disabled-for-build /etc/ld.so.preload
+
+echo "--- Largest packages installed"
+sudo -E sh -c 'apt-get install -y debian-goodies'
+dpigs -H -n 100

builder/test/tests.py

@@ -2,6 +2,7 @@
 
 # validate all required modules installed
 
+import os
 import rospy
 from geometry_msgs.msg import PoseStamped
 from sensor_msgs.msg import Range, BatteryState
@@ -22,6 +23,7 @@ from clover.srv import GetTelemetry, Navigate, NavigateGlobal, SetPosition, SetV
 from led_msgs.srv import SetLEDs
 from led_msgs.msg import LEDStateArray, LEDState
 from aruco_pose.msg import Marker, MarkerArray, Point2D
+from clover import long_callback
 
 import dynamic_reconfigure.client
 
@@ -31,9 +33,15 @@ import tf2_geometry_msgs
 import VL53L1X
 import pymavlink
 from pymavlink import mavutil
-import rpi_ws281x
+from image_geometry import PinholeCameraModel, StereoCameraModel
-import pigpio
 # from espeak import espeak
 from pyzbar import pyzbar
+import docopt
+import geopy
+import flask
 
 print(cv2.getBuildInformation())
+
+if not os.environ.get('VM'):
+    import rpi_ws281x
+    import pigpio

builder/test/tests.sh

@@ -1,21 +1,15 @@
 #!/usr/bin/env bash
 
-set -ex
+set -ex # exit on error, echo commands
 
 # TODO: validate versions
 
 # validate required software is installed
 
-python --version
 python2 --version
 python3 --version
-ipython --version
 ipython3 --version
 
-# ptvsd does not have a stand-alone binary
-python -m ptvsd --version
-python3 -m ptvsd --version
-
 node -v
 npm -v
 
@@ -25,37 +19,77 @@ lsof -v
 git --version
 vim --version
 pip --version
-pip2 --version
 pip3 --version
 tcpdump --version
 monkey --version
+# espeak --version
+systemctl --version
+
+if [ -z $VM ]; then
+	# rpi only software
+	python --version
+	ipython --version
+	pip2 --version
+	# `python` is python2 for now
+	[[ $(python -c 'import sys;print(sys.version_info.major)') == "2" ]]
+
+	# ptvsd does not have a stand-alone binary
+	python -m ptvsd --version
+	python3 -m ptvsd --version
+
 	pigpiod -v
 	i2cdetect -V
 	butterfly -h
-# espeak --version
 	mjpg_streamer --version
+fi
 
 # ros stuff
 
 roscore -h
 rosversion clover
 rosversion aruco_pose
-rosversion vl53l1x
 rosversion mavros
 rosversion mavros_extras
 rosversion ws281x
 rosversion led_msgs
 rosversion dynamic_reconfigure
 rosversion tf2_web_republisher
-rosversion compressed_image_transport
+rosversion rosbridge_server
-rosversion rosbridge_suite
-rosversion rosserial
 rosversion usb_cam
 rosversion cv_camera
 rosversion web_video_server
-rosversion rosshow
 rosversion nodelet
 rosversion image_view
 
+[[ $(rosversion ws281x) == "0.0.13" ]]
+
+if [ -z $VM ]; then
+	rosversion compressed_image_transport
+	rosversion rosshow
+	rosversion vl53l1x
+	rosversion rosserial
+	[[ $(rosversion cv_camera) == "0.5.1" ]] # patched version with init fix
+fi
+
+# determine user home directory
+[ $VM ] && H="/home/clover" || H="/home/pi"
+
+# test basic ros tool work
+source $H/catkin_ws/devel/setup.bash
+roscd
+rosrun
+rosmsg
+rossrv
+rosnode || [ $? -eq 64 ] # usage output code is 64
+rostopic || [ $? -eq 64 ]
+rosservice || [ $? -eq 64 ]
+rosparam
+roslaunch -h
+
 # validate examples are present
-[[ $(ls /home/pi/examples/*) ]]
+[[ $(ls $H/examples/*) ]]
+
+# validate web tools present
+[ -d $H/.ros/www ]
+[ "$(readlink $H/.ros/www/clover)" = "$H/catkin_ws/src/clover/clover/www" ]
+[ "$(readlink $H/.ros/www/clover_blocks)" = "$H/catkin_ws/src/clover/clover_blocks/www" ]

check_assets_size.py

@@ -18,7 +18,7 @@ EXCLUDE = 'rviz.png', 'ssid.png', 'sitl_docker_demo.png', 'qgc-params.png', 'but
     'qgc-battery.png', 'qgc-radio.png', 'qgc-cal-acc.png', 'qgc-esc.png', 'qgc-cal-compass.png', \
     'qgc.png', 'qgc-parameters.png', 'clever4-front-white-large.png', 'qgc-modes.png', \
     'qgc-requires-setup.png', 'clever4-front-white.png', 'clever4-kit-white.png', '26_1.png', 'battery_holder.stl', \
-    'camera_case.stl', 'camera_mount.stl'
+    'camera_case.stl', 'camera_mount.stl', 'grip_right.stl', 'grip_left.stl'
 
 code = 0
 

check_unused_assets.py

@@ -5,7 +5,7 @@ import sys
 import subprocess
 
 EXCLUDE = ('clever4-front-white.png', 'clever4-front-white-large.png', '.DS_Store',
-    'clever4-front-black-large.png','clover42-black.png')
+    'clever4-front-black-large.png','clover42-black.png', 'clover42-main-margin.png')
 
 code = 0
 

clover/CMakeLists.txt

@@ -17,6 +17,7 @@ find_package(catkin REQUIRED COMPONENTS
   message_generation
   geometry_msgs
   sensor_msgs
+  led_msgs
   geographic_msgs
   tf
   tf2
@@ -24,10 +25,13 @@ find_package(catkin REQUIRED COMPONENTS
   tf2_ros
   image_transport
   cv_bridge
+  dynamic_reconfigure
 )
 
 list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/cmake")
 
+# https://github.com/mavlink/mavros/blob/7f1a8/mavros/CMakeLists.txt#L42
+set(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH};/usr/share/cmake/geographiclib")
 find_package(GeographicLib REQUIRED)
 
 # Workaround for OpenCV 3/4 support
@@ -51,7 +55,7 @@ find_package(OpenCV ${_opencv_version} REQUIRED
 ## Uncomment this if the package has a setup.py. This macro ensures
 ## modules and global scripts declared therein get installed
 ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
-# catkin_python_setup()
+catkin_python_setup()
 
 ################################################
 ## Declare ROS messages, services and actions ##
@@ -78,11 +82,10 @@ find_package(OpenCV ${_opencv_version} REQUIRED
 ##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
 
 ## Generate messages in the 'msg' folder
-# add_message_files(
+add_message_files(
-#   FILES
+  FILES
-#   Message1.msg
+  State.msg
-#   Message2.msg
+)
-# )
 
 ## Generate services in the 'srv' folder
 add_service_files(
@@ -90,6 +93,9 @@ add_service_files(
   GetTelemetry.srv
   Navigate.srv
   NavigateGlobal.srv
+  SetAltitude.srv
+  SetYaw.srv
+  SetYawRate.srv
   SetPosition.srv
   SetVelocity.srv
   SetAttitude.srv
@@ -126,10 +132,9 @@ generate_messages(
 ##     and list every .cfg file to be processed
 
 ## Generate dynamic reconfigure parameters in the 'cfg' folder
-# generate_dynamic_reconfigure_options(
+generate_dynamic_reconfigure_options(
-#   cfg/DynReconf1.cfg
+  cfg/Flow.cfg
-#   cfg/DynReconf2.cfg
+)
-# )
 
 ###################################
 ## catkin specific configuration ##
@@ -211,6 +216,8 @@ add_dependencies(clover_led ${PROJECT_NAME}_generate_messages_cpp)
 
 add_dependencies(shell ${PROJECT_NAME}_generate_messages_cpp)
 
+add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencfg)
+
 ## Rename C++ executable without prefix
 ## The above recommended prefix causes long target names, the following renames the
 ## target back to the shorter version for ease of user use
@@ -241,12 +248,12 @@ target_link_libraries(${PROJECT_NAME}
 #   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 # )
 
-## Mark executables and/or libraries for installation
+# Mark executables and/or libraries for installation
-# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
+install(TARGETS simple_offboard clover_rc camera_markers vpe_publisher clover_led shell clover
-#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
+)
 
 ## Mark cpp header files for installation
 # install(DIRECTORY include/${PROJECT_NAME}/
@@ -266,13 +273,21 @@ catkin_install_python(PROGRAMS src/selfcheck.py
   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
 
+install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+
+install(DIRECTORY examples DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+
+install(DIRECTORY www DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+
+# TODO: install www
+
 # 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
+    udev/99-px4fmu.rules
     DESTINATION /lib/udev/rules.d
   )
 else()
@@ -295,4 +310,5 @@ endif()
 if (CATKIN_ENABLE_TESTING)
   find_package(rostest REQUIRED)
   add_rostest(test/basic.test)
+  add_rostest(test/offboard.test)
 endif()

clover/README.md

@@ -36,7 +36,7 @@ curl https://raw.githubusercontent.com/mavlink/mavros/master/mavros/scripts/inst
 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/clover/clover/config
+cd ~/catkin_ws/src/clover/clover/udev
 sudo cp 99-px4fmu.rules /lib/udev/rules.d
 ```
 

clover/cfg/Flow.cfg

@@ -0,0 +1,10 @@
+#!/usr/bin/env python
+PACKAGE = "clover"
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+gen.add("enabled", bool_t, 0, "if optical flow enabled", True)
+
+exit(gen.generate(PACKAGE, "clover", "Flow"))

clover/cmake/FindGeographicLib.cmake

@@ -1,18 +0,0 @@
-# taken from: https://github.com/mavlink/mavros/blob/master/libmavconn/cmake/Modules/FindGeographicLib.cmake
-
-# Look for GeographicLib
-#
-# Set
-#  GEOGRAPHICLIB_FOUND = TRUE
-#  GeographicLib_INCLUDE_DIRS = /usr/local/include
-#  GeographicLib_LIBRARIES = /usr/local/lib/libGeographic.so
-#  GeographicLib_LIBRARY_DIRS = /usr/local/lib
-
-find_path (GeographicLib_INCLUDE_DIRS NAMES GeographicLib/Config.h)
-
-find_library (GeographicLib_LIBRARIES NAMES Geographic)
-
-include (FindPackageHandleStandardArgs)
-find_package_handle_standard_args (GeographicLib DEFAULT_MSG
-  GeographicLib_LIBRARIES GeographicLib_INCLUDE_DIRS)
-mark_as_advanced (GeographicLib_LIBRARIES GeographicLib_INCLUDE_DIRS)

clover/config/99-px4fmu.rules

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

clover/examples/camera.py

@@ -0,0 +1,64 @@
+# Information: https://clover.coex.tech/camera
+
+# Example on basic working with the camera and image processing:
+
+# - cuts out a central square from the camera image;
+# - publishes this cropped image to the topic `/cv/center`;
+# - computes the average color of it;
+# - prints its name to the console.
+
+import rospy
+import cv2
+from sensor_msgs.msg import Image
+from cv_bridge import CvBridge
+from clover import long_callback
+
+rospy.init_node('cv')
+bridge = CvBridge()
+
+printed_color = None
+center_pub = rospy.Publisher('~center', Image, queue_size=1)
+
+def get_color_name(h):
+    if h < 15: return 'red'
+    elif h < 30: return 'orange'
+    elif h < 60: return 'yellow'
+    elif h < 90: return 'green'
+    elif h < 120: return 'cyan'
+    elif h < 150: return 'blue'
+    elif h < 170: return 'magenta'
+    else: return 'red'
+
+
+@long_callback
+def image_callback(msg):
+    img = bridge.imgmsg_to_cv2(msg, 'bgr8')
+
+    # convert to HSV to work with color hue
+    img_hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
+
+    # cut out a central square
+    w = img.shape[1]
+    h = img.shape[0]
+    r = 20
+    center = img_hsv[h // 2 - r:h // 2 + r, w // 2 - r:w // 2 + r]
+
+    # compute and print the average hue
+    mean_hue = center[:, :, 0].mean()
+    color = get_color_name(mean_hue)
+    global printed_color
+    if color != printed_color:
+        print(color)
+        printed_color = color
+
+    # publish the cropped image
+    center = cv2.cvtColor(center, cv2.COLOR_HSV2BGR)
+    center_pub.publish(bridge.cv2_to_imgmsg(center, 'bgr8'))
+
+# process every frame:
+image_sub = rospy.Subscriber('main_camera/image_raw', Image, image_callback, queue_size=1)
+
+# process 5 frames per second:
+# image_sub = rospy.Subscriber('main_camera/image_raw_throttled', Image, image_callback, queue_size=1)
+
+rospy.spin()

clover/examples/flight.py

@@ -15,17 +15,17 @@ set_attitude = rospy.ServiceProxy('set_attitude', srv.SetAttitude)
 set_rates = rospy.ServiceProxy('set_rates', srv.SetRates)
 land = rospy.ServiceProxy('land', Trigger)
 
-# Take off and hover 1 m above the ground
+print('Take off and hover 1 m above the ground')
 navigate(x=0, y=0, z=1, frame_id='body', auto_arm=True)
 
-# Wait for 3 seconds
+# Wait for 5 seconds
-rospy.sleep(3)
+rospy.sleep(5)
 
-# Fly forward 1 m
+print('Fly forward 1 m')
 navigate(x=1, y=0, z=0, frame_id='body')
 
-# Wait for 3 seconds
+# Wait for 5 seconds
-rospy.sleep(3)
+rospy.sleep(5)
 
-# Perform landing
+print('Perform landing')
 land()

clover/examples/flight_marker.py

@@ -15,23 +15,23 @@ set_attitude = rospy.ServiceProxy('set_attitude', srv.SetAttitude)
 set_rates = rospy.ServiceProxy('set_rates', srv.SetRates)
 land = rospy.ServiceProxy('land', Trigger)
 
-# Take off and hover 1 m above the ground
+print('Take off and hover 1 m above the ground')
 navigate(x=0, y=0, z=1, frame_id='body', auto_arm=True)
 
-# Wait for 3 seconds
+# Wait for 5 seconds
-rospy.sleep(3)
+rospy.sleep(5)
 
-# Fly 1 meter above ArUco marker 0
+print('Fly 1 meter above ArUco marker 0')
 navigate(x=0, y=0, z=1, frame_id='aruco_0')
 
-# Wait for 3 seconds
+# Wait for 5 seconds
-rospy.sleep(3)
+rospy.sleep(5)
 
-# Fly to x=1 y=1 z=1 relative to ArUco markers map
+print('Fly to x=1 y=1 z=1 relative to ArUco markers map')
 navigate(x=1, y=1, z=1, frame_id='aruco_map')
 
-# Wait for 3 seconds
+# Wait for 5 seconds
-rospy.sleep(3)
+rospy.sleep(5)
 
-# Perform landing
+print('Perform landing')
 land()

clover/examples/gps.py

@@ -0,0 +1,47 @@
+# Information: https://clover.coex.tech/en/simple_offboard.html#navigateglobal
+
+import rospy
+from clover import srv
+from std_srvs.srv import Trigger
+import math
+
+rospy.init_node('flight')
+
+get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry)
+navigate = rospy.ServiceProxy('navigate', srv.Navigate)
+navigate_global = rospy.ServiceProxy('navigate_global', srv.NavigateGlobal)
+set_position = rospy.ServiceProxy('set_position', srv.SetPosition)
+set_velocity = rospy.ServiceProxy('set_velocity', srv.SetVelocity)
+set_attitude = rospy.ServiceProxy('set_attitude', srv.SetAttitude)
+set_rates = rospy.ServiceProxy('set_rates', srv.SetRates)
+land = rospy.ServiceProxy('land', Trigger)
+
+# https://clover.coex.tech/en/snippets.html#wait_arrival
+def wait_arrival(tolerance=0.2):
+    while not rospy.is_shutdown():
+        telem = get_telemetry(frame_id='navigate_target')
+        if math.sqrt(telem.x ** 2 + telem.y ** 2 + telem.z ** 2) < tolerance:
+            break
+        rospy.sleep(0.2)
+
+start = get_telemetry()
+
+if math.isnan(start.lat):
+    raise Exception('No global position, install and configure GPS sensor: https://clover.coex.tech/gps')
+
+print('Start point global position: lat={}, lon={}'.format(start.lat, start.lon))
+
+print('Take off 3 meters')
+navigate(x=0, y=0, z=3, frame_id='body', auto_arm=True)
+wait_arrival()
+
+print('Fly 1 arcsecond to the North (approx. 30 meters)')
+navigate_global(lat=start.lat+1.0/60/60, lon=start.lon, z=start.z+3, yaw=math.inf, speed=5)
+wait_arrival()
+
+print('Fly to home position')
+navigate_global(lat=start.lat, lon=start.lon, z=start.z+3, yaw=math.inf, speed=5)
+wait_arrival()
+
+print('Land')
+land()

clover/examples/navigate_wait.py

@@ -16,11 +16,8 @@ set_attitude = rospy.ServiceProxy('set_attitude', srv.SetAttitude)
 set_rates = rospy.ServiceProxy('set_rates', srv.SetRates)
 land = rospy.ServiceProxy('land', Trigger)
 
-def navigate_wait(x=0, y=0, z=0, yaw=float('nan'), yaw_rate=0, speed=0.5, \
+def navigate_wait(x=0, y=0, z=0, yaw=math.nan, speed=0.5, frame_id='body', tolerance=0.2, auto_arm=False):
-        frame_id='body', tolerance=0.2, auto_arm=False):
+    res = navigate(x=x, y=y, z=z, yaw=yaw, speed=speed, frame_id=frame_id, auto_arm=auto_arm)
-
-    res = navigate(x=x, y=y, z=z, yaw=yaw, yaw_rate=yaw_rate, speed=speed, \
-        frame_id=frame_id, auto_arm=auto_arm)
 
     if not res.success:
         return res
@@ -31,11 +28,11 @@ def navigate_wait(x=0, y=0, z=0, yaw=float('nan'), yaw_rate=0, speed=0.5, \
             return res
         rospy.sleep(0.2)
 
-# Take off 1 meter
+print('Take off 1 meter')
 navigate_wait(z=1, frame_id='body', auto_arm=True)
 
-# Fly forward 1 m
+print('Fly forward 1 m')
 navigate_wait(x=1, frame_id='body')
 
-# Land
+print('Land')
 land()

clover/examples/red_circle.py

@@ -0,0 +1,91 @@
+# This example makes the drone find and follow the red circle.
+# To test in the simulator, place 'Red Circle' model on the floor.
+# More information: https://clover.coex.tech/red_circle
+
+# Input topic: main_camera/image_raw (camera image)
+# Output topics:
+#   cv/mask (red color mask)
+#   cv/red_circle (position of the center of the red circle in 3D space)
+
+import rospy
+import cv2
+import numpy as np
+from math import nan
+from sensor_msgs.msg import Image, CameraInfo
+from geometry_msgs.msg import PointStamped, Point
+from cv_bridge import CvBridge
+from clover import long_callback, srv
+import tf2_ros
+import tf2_geometry_msgs
+import image_geometry
+
+rospy.init_node('cv', disable_signals=True) # disable signals to allow interrupting with ctrl+c
+
+get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry)
+set_position = rospy.ServiceProxy('set_position', srv.SetPosition)
+
+bridge = CvBridge()
+
+tf_buffer = tf2_ros.Buffer()
+tf_listener = tf2_ros.TransformListener(tf_buffer)
+
+mask_pub = rospy.Publisher('~mask', Image, queue_size=1)
+point_pub = rospy.Publisher('~red_circle', PointStamped, queue_size=1)
+
+# read camera info
+camera_model = image_geometry.PinholeCameraModel()
+camera_model.fromCameraInfo(rospy.wait_for_message('main_camera/camera_info', CameraInfo))
+
+
+def img_xy_to_point(xy, dist):
+    xy_rect = camera_model.rectifyPoint(xy)
+    ray = camera_model.projectPixelTo3dRay(xy_rect)
+    return Point(x=ray[0] * dist, y=ray[1] * dist, z=dist)
+
+def get_center_of_mass(mask):
+    M = cv2.moments(mask)
+    if M['m00'] == 0:
+        return None
+    return M['m10'] // M['m00'], M['m01'] // M['m00']
+
+follow_red_circle = False
+
+@long_callback
+def image_callback(msg):
+    img = bridge.imgmsg_to_cv2(msg, 'bgr8')
+    img_hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
+
+    # we need to use two ranges for red color
+    mask1 = cv2.inRange(img_hsv, (0, 150, 150), (15, 255, 255))
+    mask2 = cv2.inRange(img_hsv, (160, 150, 150), (180, 255, 255))
+
+    # combine two masks using bitwise OR
+    mask = cv2.bitwise_or(mask1, mask2)
+
+    # publish the mask
+    if mask_pub.get_num_connections() > 0:
+        mask_pub.publish(bridge.cv2_to_imgmsg(mask, 'mono8'))
+
+    # calculate x and y of the circle
+    xy = get_center_of_mass(mask)
+    if xy is None:
+        return
+
+    # calculate and publish the position of the circle in 3D space
+    altitude = get_telemetry('terrain').z
+    xy3d = img_xy_to_point(xy, altitude)
+    target = PointStamped(header=msg.header, point=xy3d)
+    point_pub.publish(target)
+
+    if follow_red_circle:
+        # follow the target
+        setpoint = tf_buffer.transform(target, 'map', timeout=rospy.Duration(0.2))
+        set_position(x=setpoint.point.x, y=setpoint.point.y, z=nan, yaw=nan, frame_id=setpoint.header.frame_id)
+
+# process each camera frame:
+image_sub = rospy.Subscriber('main_camera/image_raw', Image, image_callback, queue_size=1)
+
+rospy.loginfo('Hit enter to follow the red circle')
+input()
+follow_red_circle = True
+rospy.spin()

clover/examples/subscriber.py

@@ -0,0 +1,15 @@
+# Information: https://clover.coex.tech/en/laser.html
+
+import rospy
+from sensor_msgs.msg import Range
+
+rospy.init_node('process_rangefinder')
+
+def range_callback(msg):
+    # Process data from the rangefinder
+    print('Rangefinder distance:', msg.range)
+
+# Subscribe to laser rangefinder data
+rospy.Subscriber('rangefinder/range', Range, range_callback)
+
+rospy.spin()

clover/launch/aruco.launch

@@ -3,21 +3,27 @@
     <arg name="aruco_map" default="false"/>
     <arg name="aruco_vpe" default="false"/>
     <arg name="placement" default="floor"/> <!-- markers placement: floor, ceiling, unknown -->
-    <arg name="length" default="0.33"/> <!-- not-in-map markers length, m -->
+    <arg name="length" default="0.22"/> <!-- not-in-map markers length, m -->
     <arg name="map" default="map.txt"/> <!-- markers map file name -->
 
     <!-- For additional help go to https://clover.coex.tech/aruco -->
 
+    <arg name="force_init" default="false"/>
+    <arg name="disable" default="false"/> <!-- only force init -->
+
     <!-- aruco_detect: detect aruco markers, estimate poses -->
-    <node name="aruco_detect" pkg="nodelet" if="$(arg aruco_detect)" type="nodelet" args="load aruco_pose/aruco_detect main_camera_nodelet_manager" output="screen" clear_params="true" respawn="true">
+    <node name="aruco_detect" pkg="nodelet" if="$(eval aruco_detect and not disable)" type="nodelet" args="load aruco_pose/aruco_detect main_camera_nodelet_manager" output="screen" clear_params="true" respawn="true">
         <remap from="image_raw" to="main_camera/image_raw"/>
         <remap from="camera_info" to="main_camera/camera_info"/>
-        <remap from="map_markers" to="aruco_map/markers" if="$(arg aruco_map)"/>
+        <remap from="map_markers" to="aruco_map/map"/>
+        <param name="dictionary" value="2"/> <!-- DICT_4X4_250 -->
         <param name="estimate_poses" value="true"/>
         <param name="send_tf" value="true"/>
-        <param name="known_tilt" value="map" if="$(eval placement == 'floor')"/>
+        <param name="use_map_markers" value="true"/>
-        <param name="known_tilt" value="map_flipped" if="$(eval placement == 'ceiling')"/>
+        <param name="known_vertical" value="map" if="$(eval placement == 'floor' or placement == 'ceiling')"/>
+        <param name="flip_vertical" value="true" if="$(eval placement == 'ceiling')"/>
         <param name="length" value="$(arg length)"/>
+        <param name="transform_timeout" value="0.1"/>
         <!-- aruco detector parameters -->
         <param name="cornerRefinementMethod" value="2"/> <!-- contour refinement -->
         <param name="minMarkerPerimeterRate" value="0.075"/> <!-- 0.075 for 320x240, 0.0375 for 640x480 -->
@@ -26,13 +32,13 @@
     </node>
 
     <!-- aruco_map: estimate aruco map pose -->
-    <node name="aruco_map" pkg="nodelet" type="nodelet" if="$(arg aruco_map)" args="load aruco_pose/aruco_map main_camera_nodelet_manager" output="screen" clear_params="true" respawn="true">
+    <node name="aruco_map" pkg="nodelet" type="nodelet" if="$(eval aruco_map and not disable)" args="load aruco_pose/aruco_map main_camera_nodelet_manager" output="screen" clear_params="true" respawn="true">
         <remap from="image_raw" to="main_camera/image_raw"/>
         <remap from="camera_info" to="main_camera/camera_info"/>
         <remap from="markers" to="aruco_detect/markers"/>
         <param name="map" value="$(find aruco_pose)/map/$(arg map)"/>
-        <param name="known_tilt" value="map" if="$(eval placement == 'floor')"/>
+        <param name="known_vertical" value="map" if="$(eval placement == 'floor' or placement == 'ceiling')"/>
-        <param name="known_tilt" value="map_flipped" if="$(eval placement == 'ceiling')"/>
+        <param name="flip_vertical" value="true" if="$(eval placement == 'ceiling')"/>
         <param name="image_axis" value="true"/>
         <param name="frame_id" value="aruco_map_detected" if="$(arg aruco_vpe)"/>
         <param name="frame_id" value="aruco_map" unless="$(arg aruco_vpe)"/>
@@ -41,11 +47,11 @@
     </node>
 
     <!-- vpe publisher from aruco markers -->
-    <node name="vpe_publisher" pkg="clover" type="vpe_publisher" if="$(arg aruco_vpe)" output="screen" clear_params="true">
+    <node name="vpe_publisher" pkg="clover" type="vpe_publisher" if="$(eval aruco_vpe or force_init)" output="screen" clear_params="true">
-        <remap from="~pose_cov" to="aruco_map/pose"/>
+        <remap from="~pose_cov" to="aruco_map/pose" if="$(arg aruco_vpe)"/>
         <remap from="~vpe" to="mavros/vision_pose/pose"/>
-        <param name="frame_id" value="aruco_map_detected"/>
+        <param name="frame_id" value="aruco_map_detected" if="$(arg aruco_vpe)"/>
-        <param name="publish_zero" value="true"/>
+        <param name="force_init" value="$(arg force_init)"/>
         <param name="offset_frame_id" value="aruco_map"/>
     </node>
 </launch>

clover/launch/clover.launch

@@ -12,6 +12,7 @@
     <arg name="led" default="true"/>
     <arg name="blocks" default="false"/>
     <arg name="rc" default="false"/>
+    <arg name="force_init" default="true"/> <!-- force estimator to init by publishing zero pose -->
 
     <arg name="simulator" default="false"/> <!-- flag that we are operating on a simulated drone -->
 
@@ -33,7 +34,10 @@
     </node>
 
     <!-- aruco markers -->
-    <include file="$(find clover)/launch/aruco.launch" if="$(arg aruco)"/>
+    <include file="$(find clover)/launch/aruco.launch" if="$(eval aruco or force_init)">
+        <arg name="force_init" value="$(arg force_init)"/>
+        <arg name="disable" value="$(eval not aruco)"/>
+    </include>
 
     <!-- optical flow -->
     <node pkg="nodelet" type="nodelet" name="optical_flow" args="load clover/optical_flow main_camera_nodelet_manager" if="$(arg optical_flow)" clear_params="true" output="screen" respawn="true">
@@ -41,16 +45,13 @@
         <remap from="camera_info" to="main_camera/camera_info"/>
         <param name="calc_flow_gyro" value="true"/>
         <param name="roi_rad" value="0.8"/>
+        <param name="disable_on_vpe" value="true"/>
     </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"/>
-
     <!-- simplified offboard control -->
     <node name="simple_offboard" pkg="clover" type="simple_offboard" output="screen" clear_params="true">
-        <param name="reference_frames/body" value="map"/>
-        <param name="reference_frames/base_link" value="map"/>
-        <param name="reference_frames/navigate_target" value="map"/>
         <param name="reference_frames/main_camera_optical" value="map"/>
+        <param name="terrain_frame_mode" value="range"/>
     </node>
 
     <!-- main camera -->
@@ -71,6 +72,9 @@
         <param name="pass_statuses" type="yaml" value="[0, 6, 7, 11]"/>
     </node>
 
+    <!-- rangefinder's frame -->
+    <node pkg="tf2_ros" type="static_transform_publisher" name="rangefinder_frame" args="0 0 -0.05 0 1.5707963268 0 base_link rangefinder" if="$(arg rangefinder_vl53l1x)"/>
+
     <!-- led strip -->
     <include file="$(find clover)/launch/led.launch" if="$(arg led)">
         <arg name="simulator" value="$(arg simulator)"/>
@@ -85,8 +89,4 @@
         <param name="use_fake_gcs" value="false"/>
     </node>
 
-    <!-- Update static directory -->
-    <node pkg="roswww_static" name="roswww_static" type="main.py" clear_params="true">
-        <param name="default_package" value="clover"/>
-    </node>
 </launch>

clover/launch/led.launch

@@ -21,7 +21,8 @@
     </node>
 
     <!-- high level led effects control, events notification with leds -->
-    <node pkg="clover" name="led_effect" type="led" ns="led" clear_params="true" output="screen" if="$(arg led_effect)">
+    <node pkg="clover" name="led_effect" type="led" clear_params="true" output="screen" if="$(arg led_effect)">
+        <param name="led" value="led"/>
         <param name="blink_rate" value="2"/>
         <param name="fade_period" value="0.5"/>
         <param name="rainbow_period" value="5"/>

clover/launch/main_camera.launch

@@ -4,6 +4,9 @@
     <arg name="direction_z" default="down"/> <!-- direction the camera points: down, up -->
     <arg name="direction_y" default="backward"/> <!-- direction the camera cable points: backward, forward -->
     <arg name="device" default="/dev/video0"/> <!-- v4l2 device -->
+    <arg name="throttled_topic" default="true"/> <!-- enable throttled image topic -->
+    <arg name="throttled_topic_rate" default="5.0"/> <!-- throttled image topic rate -->
+    <arg name="rectify" default="false"/> <!-- enable rectification -->
     <arg name="simulator" default="false"/>
 
     <node if="$(eval direction_z == 'down' and direction_y == 'backward')" pkg="tf2_ros" type="static_transform_publisher" name="main_camera_frame" args="0.05 0 -0.07 -1.5707963 0 3.1415926 base_link main_camera_optical"/>
@@ -43,4 +46,15 @@
     <node pkg="clover" type="camera_markers" ns="main_camera" name="main_camera_markers">
         <param name="scale" value="3.0"/>
     </node>
+
+    <!-- image topic throttled -->
+    <node pkg="topic_tools" name="main_camera_throttle" type="throttle" ns="main_camera"
+        args="messages image_raw $(arg throttled_topic_rate) image_raw_throttled" if="$(arg throttled_topic)"/>
+
+    <!-- rectified image topic -->
+    <node pkg="nodelet" type="nodelet" name="rectify" args="load image_proc/rectify main_camera_nodelet_manager" if="$(arg rectify)">
+        <remap from="image_mono" to="main_camera/image_raw"/>
+        <remap from="camera_info" to="main_camera/camera_info"/>
+        <remap from="image_rect" to="main_camera/image_rect"/>
+    </node>
 </launch>

clover/launch/mavros.launch

@@ -1,5 +1,5 @@
 <launch>
-    <arg name="fcu_conn" default="usb"/> <!-- options: usb, uart, tcp, udp, sitl -->
+    <arg name="fcu_conn" default="usb"/> <!-- options: usb, uart, tcp, udp, sitl, hitl -->
     <arg name="fcu_ip" default="127.0.0.1"/>
     <arg name="fcu_sys_id" default="1"/>
     <arg name="gcs_bridge" default="tcp"/>
@@ -23,6 +23,9 @@
         <!-- sitl since PX4 1.9.0 -->
         <param name="fcu_url" value="udp://@$(arg fcu_ip):14580" if="$(eval fcu_conn == 'sitl')"/>
 
+        <!-- hitl connection (to gazebo_mavlink_interface plugin) -->
+        <param name="fcu_url" value="udp://$(arg fcu_ip):14540@" if="$(eval fcu_conn == 'hitl')"/>
+
         <!-- set target_system_id -->
         <param name="target_system_id" value="$(arg fcu_sys_id)" />
 
@@ -39,7 +42,7 @@
         <rosparam command="load" file="$(find clover)/launch/mavros_config.yaml"/>
 
         <!-- remap rangefinder -->
-        <remap from="mavros/distance_sensor/rangefinder_sub" to="rangefinder/range"/>
+        <remap from="mavros/distance_sensor/rangefinder_sub" to="$(arg distance_sensor_remap)" if="$(eval bool(distance_sensor_remap))"/>
 
         <rosparam param="plugin_whitelist">
             - altitude
@@ -74,9 +77,6 @@
         covariance: 1  # cm
     </rosparam>
 
-    <!-- Rangefinders frame -->
-    <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="visualization" pkg="mavros_extras" type="visualization" if="$(arg viz)">
         <remap to="mavros/local_position/pose" from="local_position"/>

clover/launch/mavros_config.yaml

@@ -1,11 +1,11 @@
 # Config file for mavros
 # Based on https://raw.githubusercontent.com/mavlink/mavros/master/mavros/launch/px4_config.yaml
 
-startup_px4_usb_quirk: true
+startup_px4_usb_quirk: false
 
 conn:
-  heartbeat_rate: 1.0    # send hertbeat rate in Hertz
+  heartbeat_rate: 1.0    # send heartbeat rate in Hertz
-  timeout: 10.0          # hertbeat timeout in seconds
+  timeout: 10.0          # heartbeat timeout in seconds
   timesync_rate: 10.0    # TIMESYNC rate in Hertz (feature disabled if 0.0)
   system_time_rate: 1.0  # send system time to FCU rate in Hertz (disabled if 0.0)
 
@@ -13,6 +13,7 @@ time:
   time_ref_source: "fcu"  # time_reference source
   timesync_mode: MAVLINK
   timesync_avg_alpha: 0.6 # timesync averaging factor
+  publish_sim_time: false # don't publish /clock
 
 global_position:
   frame_id: "map"             # origin frame
@@ -77,6 +78,9 @@ distance_sensor:
       field_of_view: 0.5
   rangefinder_sub:
       subscriber: true
+      id: 1
+      orientation: PITCH_270
+      covariance: 1  # cm
 
 # fake_gps
 fake_gps:

clover/launch/simulator.launch

@@ -0,0 +1,4 @@
+<launch>
+    <!-- shortcut for running the simulation (`roslaunch clover simulator.launch`) -->
+    <include file="$(find clover_simulation)/launch/simulator.launch"/>
+</launch>

clover/msg/State.msg

@@ -0,0 +1,38 @@
+uint8 MODE_NONE = 0
+uint8 MODE_NAVIGATE = 1
+uint8 MODE_NAVIGATE_GLOBAL = 2
+uint8 MODE_POSITION = 3
+uint8 MODE_VELOCITY = 4
+uint8 MODE_ATTITUDE = 5
+uint8 MODE_RATES = 6
+
+uint8 YAW_MODE_YAW = 0
+uint8 YAW_MODE_YAW_RATE = 1
+uint8 YAW_MODE_YAW_TOWARDS = 2
+
+# type of offboard control
+uint8 mode
+uint8 yaw_mode
+
+# targets
+float32 x
+float32 y
+float32 z
+float32 speed
+float32 lat
+float32 lon
+float32 vx
+float32 vy
+float32 vz
+float32 roll
+float32 pitch
+float32 yaw
+float32 roll_rate
+float32 pitch_rate
+float32 yaw_rate
+float32 thrust
+
+# frames of reference
+string xy_frame_id
+string z_frame_id
+string yaw_frame_id

clover/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>clover</name>
-  <version>0.21.1</version>
+  <version>0.24.0</version>
   <description>The Clover package</description>
 
   <maintainer email="okalachev@gmail.com">Oleg Kalachev</maintainer>
@@ -37,11 +37,15 @@
   <depend>rosbridge_server</depend>
   <depend>web_video_server</depend>
   <depend>tf2_web_republisher</depend>
+  <depend>libxml2</depend>
+  <depend>libxslt</depend>
   <depend condition="$ROS_PYTHON_VERSION == 2">python-lxml</depend>
   <depend condition="$ROS_PYTHON_VERSION == 3">python3-lxml</depend>
+  <depend>dynamic_reconfigure</depend>
+  <depend>image_proc</depend>
+  <depend>image_geometry</depend>
   <exec_depend>python-pymavlink</exec_depend>
-  <!-- Use test_depend for packages you need only for testing: -->
+  <test_depend>ros_pytest</test_depend>
-  <!--   <test_depend>gtest</test_depend> -->
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>

clover/requirements.txt

@@ -1,5 +1,4 @@
-flask==1.1.1
+flask
-docopt==0.6.2
+geopy
-geopy==1.11.0
+smbus2
-smbus2==0.3.0
+VL53L1X
-VL53L1X==0.0.5

clover/setup.py

@@ -0,0 +1,11 @@
+## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD
+
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+# fetch values from package.xml
+setup_args = generate_distutils_setup(
+    packages=['clover'],
+    package_dir={'': 'src'})
+
+setup(**setup_args)

clover/src/autotest/autotest_aruco.py

@@ -0,0 +1,90 @@
+#!/usr/bin/env python3
+
+import rospy
+import math
+import signal
+import sys
+import dynamic_reconfigure.client
+from clover import srv
+from std_srvs.srv import Trigger
+from sensor_msgs.msg import Range
+from aruco_pose.msg import MarkerArray
+from util import handle_response
+
+rospy.init_node('autotest_aruco', disable_signals=True) # disable signals to allow interrupting with ctrl+c
+
+try:
+    flow_client = dynamic_reconfigure.client.Client('optical_flow', timeout=2)
+except rospy.ROSException:
+    flow_client = None
+    print('Cannot configure optical flow, skip')
+
+get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry)
+navigate = handle_response(rospy.ServiceProxy('navigate', srv.Navigate))
+land = handle_response(rospy.ServiceProxy('land', Trigger))
+
+def interrupt(sig, frame):
+    print('\nInterrupted, landing...')
+    land()
+    sys.exit(0)
+
+signal.signal(signal.SIGINT, interrupt)
+
+def print_current_map_position():
+    telem = get_telemetry()
+    dist = rospy.wait_for_message('rangefinder/range', Range).range
+    print('Map position:\tx={:.1f}\ty={:.1f}\tz={:.1f}\tyaw={:.1f}\tdist={:.2f}'.format(telem.x, telem.y, telem.z, telem.yaw, dist))
+
+def navigate_wait(x=0, y=0, z=0, yaw=math.nan, speed=0.5, frame_id='body', tolerance=0.2, auto_arm=False):
+    res = navigate(x=x, y=y, z=z, yaw=yaw, speed=speed, frame_id=frame_id, auto_arm=auto_arm)
+
+    if not res.success:
+        return res
+
+    while not rospy.is_shutdown():
+        telem = get_telemetry(frame_id='navigate_target')
+        if math.sqrt(telem.x ** 2 + telem.y ** 2 + telem.z ** 2) < tolerance:
+            return res
+        rospy.sleep(0.2)
+
+markers = rospy.wait_for_message('aruco_map/map', MarkerArray, timeout=3)
+left = min(marker.pose.position.x for marker in markers.markers)
+bottom = min(marker.pose.position.y for marker in markers.markers)
+width = max(marker.pose.position.x for marker in markers.markers)
+height = max(marker.pose.position.y for marker in markers.markers)
+center_x = left + width / 2
+center_y = bottom + height / 2
+
+print('Map rect: %g %g - %g %g' % (left, bottom, width, height))
+
+input('Take off and hover 1 m [enter] ')
+navigate_wait(x=0, y=0, z=1, frame_id='body', auto_arm=True)
+print_current_map_position()
+
+input('Go to corner %g %g 1.5 speed 1 [enter] ' % (width, height))
+navigate_wait(x=width, y=height, z=1.5, speed=1, frame_id='aruco_map')
+print_current_map_position()
+
+input('Go to center %g %g 1.5 speed 5 [enter] ' % (center_x, center_y))
+navigate_wait(x=center_x, y=center_y, z=1.5, speed=5, frame_id='aruco_map')
+print_current_map_position()
+
+if flow_client:
+    input('Disable optical flow and keep hovering [enter] ')
+    flow_client.update_configuration({'enabled': False})
+    rospy.sleep(5)
+
+    input('Enable optical flow back [enter] ')
+    flow_client.update_configuration({'enabled': True})
+
+input('Go to side 1 %g 2 heading top [enter] ' % (center_y))
+navigate_wait(x=1, y=center_y, z=2, yaw=1.57, frame_id='aruco_map')
+print_current_map_position()
+
+marker_id = markers.markers[0].id
+input('Go to marker %d z=1.5 [enter] ' % marker_id)
+navigate_wait(x=0, y=0, z=1.5, yaw=0, frame_id='aruco_%d' % marker_id)
+print_current_map_position()
+
+input('Perform landing [enter] ')
+land()

clover/src/autotest/autotest_flight.py

@@ -0,0 +1,99 @@
+#!/usr/bin/env python3
+
+import rospy
+import math
+from math import nan, inf
+import signal
+import sys
+from clover import srv
+from std_srvs.srv import Trigger
+from sensor_msgs.msg import Range
+from util import handle_response
+
+rospy.init_node('autotest_flight', disable_signals=True) # disable signals to allow interrupting with ctrl+c
+
+get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry)
+navigate = handle_response(rospy.ServiceProxy('navigate', srv.Navigate))
+navigate_global = handle_response(rospy.ServiceProxy('navigate_global', srv.NavigateGlobal))
+set_yaw = handle_response(rospy.ServiceProxy('set_yaw', srv.SetYaw))
+set_yaw_rate = handle_response(rospy.ServiceProxy('set_yaw_rate', srv.SetYawRate))
+set_position = handle_response(rospy.ServiceProxy('set_position', srv.SetPosition))
+set_velocity = handle_response(rospy.ServiceProxy('set_velocity', srv.SetVelocity))
+set_attitude = handle_response(rospy.ServiceProxy('set_attitude', srv.SetAttitude))
+set_rates = handle_response(rospy.ServiceProxy('set_rates', srv.SetRates))
+land = handle_response(rospy.ServiceProxy('land', Trigger))
+
+def interrupt(sig, frame):
+    print('\nInterrupted, landing...')
+    land()
+    sys.exit(0)
+
+signal.signal(signal.SIGINT, interrupt)
+
+def navigate_wait(x=0, y=0, z=0, yaw=nan, speed=0.5, frame_id='body', tolerance=0.2, auto_arm=False):
+    res = navigate(x=x, y=y, z=z, yaw=yaw, speed=speed, frame_id=frame_id, auto_arm=auto_arm)
+
+    if not res.success:
+        return res
+
+    while not rospy.is_shutdown():
+        telem = get_telemetry(frame_id='navigate_target')
+        if math.sqrt(telem.x ** 2 + telem.y ** 2 + telem.z ** 2) < tolerance:
+            return res
+        rospy.sleep(0.2)
+
+def print_distance():
+    dist = rospy.wait_for_message('rangefinder/range', Range).range
+    print('Distance: {:.2f}'.format(dist))
+
+input('Take off and hover 1 m [enter] ')
+navigate_wait(z=1, frame_id='body', auto_arm=True)
+print_distance()
+start = get_telemetry()
+
+input('Fly forward 2 m [enter] ')
+navigate_wait(x=2, frame_id='navigate_target')
+print_distance()
+
+input('Climb 0.5 m [enter] ')
+navigate_wait(z=0.5, frame_id='navigate_target')
+print_distance()
+
+input('Rotate left 90° [enter] ')
+navigate(yaw=math.pi / 2, frame_id='navigate_target')
+rospy.sleep(3)
+
+input('Use set_velocity to fly forward 2 m speed 1 [enter]')
+set_velocity(vx=1, vy=0.0, vz=0, frame_id='body')
+rospy.sleep(2)
+set_position(frame_id='body')
+
+input('Rotate right 90° using set_yaw [enter] ')
+set_yaw(yaw=-math.pi / 2, frame_id='navigate_target')
+rospy.sleep(3)
+
+input('Use set_attitude to fly backwards [enter]')
+set_attitude(roll=0, pitch=-0.3, yaw=0, thrust=0.5, frame_id='body')
+rospy.sleep(0.3)
+set_position(frame_id='body')
+
+input('Use set_attitude to fly right [enter]')
+set_attitude(roll=0.3, pitch=0, yaw=0, thrust=0.5, frame_id='body')
+rospy.sleep(0.5)
+set_position(frame_id='body')
+
+input('Use set_rates to fly right [enter]')
+set_rates(roll_rate=1.2, thrust=0.5)
+rospy.sleep(0.4)
+set_position(frame_id='body')
+
+input('Rotate 360° to the right using set_yaw_rate [enter]')
+set_yaw_rate(yaw_rate=-1)
+rospy.sleep(2 * math.pi)
+set_position(frame_id='body')
+
+input('Return to start point heading forward [enter]')
+navigate_wait(x=start.x, y=start.y, z=start.z, yaw=inf, speed=1, frame_id='map')
+
+input('Land [enter]')
+land()

clover/src/autotest/autotest_led.py

@@ -0,0 +1,72 @@
+#!/usr/bin/env python3
+
+import rospy
+import functools
+from clover.srv import SetLEDEffect
+from led_msgs.srv import SetLEDs
+from led_msgs.msg import LEDStateArray, LEDState
+from util import handle_response
+
+rospy.init_node('autotest_led', disable_signals=True)
+
+set_leds = handle_response(rospy.ServiceProxy('led/set_leds', SetLEDs))
+set_effect = handle_response(rospy.ServiceProxy('led/set_effect', SetLEDEffect))
+
+led_count = len(rospy.wait_for_message('led/state', LEDStateArray, timeout=10).leds)
+print('LED count =', led_count)
+
+print('== Testing effects ==')
+
+input('Fill red [enter] ')
+set_effect(r=255, g=0, b=0)
+
+input('Fill green [enter] ')
+set_effect(r=0, g=100, b=0)
+
+input('Blink white [enter] ')
+set_effect(effect='blink', r=255, g=255, b=255)
+rospy.sleep(3)
+
+input('Blink fast violet [enter] ')
+set_effect(effect='blink_fast', r=220, g=20, b=250)
+rospy.sleep(3)
+
+input('Fade to blue [enter] ')
+set_effect(effect='fade', r=0, g=0, b=255)
+
+input('Wipe to yellow [enter] ')
+set_effect(effect='wipe', r=255, g=255, b=40)
+
+input('Flash red [enter] ')
+set_effect(effect='flash', r=255, g=0, b=0)
+rospy.sleep(1)
+
+input('Rainbow [enter] ')
+set_effect(effect='rainbow')
+rospy.sleep(4)
+
+input('Rainbow fill [enter] ')
+set_effect(effect='rainbow_fill')
+rospy.sleep(4)
+
+input('Turn off [enter] ')
+set_effect()
+
+print('== Testing low-level control ==')
+
+input('Fill orange [enter] ')
+set_leds(leds=[LEDState(index=i, r=245, g=155, b=0) for i in range(led_count)])
+
+input('Fill blue gradient [enter] ')
+set_leds(leds=[LEDState(index=i, r=0, g=20, b=int(255 * i / led_count)) for i in range(led_count)])
+
+input('Animate green dot [enter] ')
+set_effect()
+for i in range(led_count):
+    if i > 0:
+        set_leds(leds=[LEDState(index=i - 1, r=0, g=0, b=0)])
+    set_leds(leds=[LEDState(index=i, r=0, g=255, b=0)])
+    rospy.sleep(0.05)
+
+input('Turn off [enter] ')
+set_effect()

clover/src/autotest/util.py

@@ -0,0 +1,11 @@
+import functools
+
+# decorator to handle response and print error message
+def handle_response(fn):
+    @functools.wraps(fn)
+    def wrapper(*args, **kwargs):
+        res = fn(*args, **kwargs)
+        if not res.success:
+            print('\033[91mError:\033[0m {}'.format(res.message))
+        return res
+    return wrapper

clover/src/clover/__init__.py

@@ -0,0 +1,35 @@
+import rospy
+from threading import Thread, Event
+
+def long_callback(fn):
+    """
+    Decorator fixing a rospy issue for long-running topic callbacks, primarily
+    for image processing.
+
+    See: https://github.com/ros/ros_comm/issues/1901.
+
+    Usage example:
+
+    @long_callback
+    def image_callback(msg):
+        # perform image processing
+        # ...
+
+    rospy.Subscriber('main_camera/image_raw', Image, image_callback, queue_size=1)
+    """
+    e = Event()
+
+    def thread():
+        while not rospy.is_shutdown():
+            e.wait()
+            e.clear()
+            fn(thread.current_msg)
+
+    thread.current_msg = None
+    Thread(target=thread, daemon=True).start()
+
+    def wrapper(msg):
+        thread.current_msg = msg
+        e.set()
+
+    return wrapper

clover/src/led.cpp

@@ -31,7 +31,6 @@ ros::Time start_time;
 double blink_rate, blink_fast_rate, flash_delay, fade_period, wipe_period, rainbow_period;
 double low_battery_threshold;
 std::vector<std::string> error_ignore;
-bool blink_state;
 led_msgs::SetLEDs set_leds;
 led_msgs::LEDStateArray state, start_state;
 ros::ServiceClient set_leds_srv;
@@ -87,9 +86,8 @@ void proceed(const ros::TimerEvent& event)
 	set_leds.request.leds.resize(led_count);
 
 	if (current_effect.effect == "blink" || current_effect.effect == "blink_fast") {
-		blink_state = !blink_state;
+		// enable on odd counter
-		// toggle all leds
+		if (counter % 2 != 0) {
-		if (blink_state) {
 			fill(current_effect.r, current_effect.g, current_effect.b);
 		} else {
 			fill(0, 0, 0);
@@ -222,6 +220,7 @@ bool setEffect(clover::SetLEDEffect::Request& req, clover::SetLEDEffect::Respons
 	counter = 0;
 	start_state = state;
 	start_time = ros::Time::now();
+	proceed({ .current_real = start_time });
 
 	return true;
 }
@@ -310,18 +309,22 @@ int main(int argc, char **argv)
 	nh_priv.param("notify/low_battery/threshold", low_battery_threshold, 3.7);
 	nh_priv.param("notify/error/ignore", error_ignore, {});
 
-	ros::service::waitForService("set_leds"); // cannot work without set_leds service
+	std::string led; // led namespace
-	set_leds_srv = nh.serviceClient<led_msgs::SetLEDs>("set_leds", true);
+	nh_priv.param("led", led, std::string("led"));
+	if (!led.empty()) led += "/";
+
+	ros::service::waitForService(led + "set_leds"); // cannot work without set_leds service
+	set_leds_srv = nh.serviceClient<led_msgs::SetLEDs>(led + "set_leds", true);
 
 	// wait for leds count info
-	handleState(*ros::topic::waitForMessage<led_msgs::LEDStateArray>("state", nh));
+	handleState(*ros::topic::waitForMessage<led_msgs::LEDStateArray>(led + "state", nh));
 
-	auto state_sub = nh.subscribe("state", 1, &handleState);
+	auto state_sub = nh.subscribe(led + "state", 1, &handleState);
 
-	auto set_effect = nh.advertiseService("set_effect", &setEffect);
+	auto set_effect = nh.advertiseService(led + "set_effect", &setEffect);
 
-	auto mavros_state_sub = nh.subscribe("/mavros/state", 1, &handleMavrosState);
+	auto mavros_state_sub = nh.subscribe("mavros/state", 1, &handleMavrosState);
-	auto battery_sub = nh.subscribe("/mavros/battery", 1, &handleBattery);
+	auto battery_sub = nh.subscribe("mavros/battery", 1, &handleBattery);
 	auto rosout_sub = nh.subscribe("/rosout_agg", 1, &handleLog);
 
 	timer = nh.createTimer(ros::Duration(0), &proceed, false, false);

clover/src/optical_flow.cpp

@@ -22,11 +22,14 @@
 #include <tf2/utils.h>
 #include <tf2_ros/transform_listener.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <dynamic_reconfigure/server.h>
 #include <mavros_msgs/OpticalFlowRad.h>
 #include <sensor_msgs/Imu.h>
+#include <geometry_msgs/PoseStamped.h>
 #include <geometry_msgs/Vector3Stamped.h>
 #include <geometry_msgs/PointStamped.h>
 #include <geometry_msgs/TwistStamped.h>
+#include <clover/FlowConfig.h>
 
 using cv::Mat;
 
@@ -38,6 +41,7 @@ public:
 	{}
 
 private:
+	bool enabled_;
 	ros::Publisher flow_pub_, velo_pub_, shift_pub_;
 	ros::Time prev_stamp_;
 	std::string fcu_frame_id_, local_frame_id_;
@@ -53,6 +57,11 @@ private:
 	std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
 	std::unique_ptr<tf2_ros::TransformListener> tf_listener_;
 	bool calc_flow_gyro_;
+	float flow_gyro_default_;
+	bool disable_on_vpe_;
+	ros::Subscriber vpe_sub_;
+	ros::Time last_vpe_time_;
+	std::shared_ptr<dynamic_reconfigure::Server<clover::FlowConfig>> dyn_srv_;
 
 	void onInit()
 	{
@@ -69,21 +78,30 @@ private:
 		roi_px_ = nh_priv.param("roi", 128);
 		roi_rad_ = nh_priv.param("roi_rad", 0.0);
 		calc_flow_gyro_ = nh_priv.param("calc_flow_gyro", false);
+		flow_gyro_default_ = nh_priv.param("flow_gyro_default", NAN);
 
 		img_pub_ = it_priv.advertise("debug", 1);
 		flow_pub_ = nh.advertise<mavros_msgs::OpticalFlowRad>("mavros/px4flow/raw/send", 1);
 		velo_pub_ = nh_priv.advertise<geometry_msgs::TwistStamped>("angular_velocity", 1);
 		shift_pub_ = nh_priv.advertise<geometry_msgs::Vector3Stamped>("shift", 1);
 
-		flow_.integrated_xgyro = NAN; // no IMU available
-		flow_.integrated_ygyro = NAN;
-		flow_.integrated_zgyro = NAN;
 		flow_.time_delta_distance_us = 0;
 		flow_.distance = -1; // no distance sensor available
 		flow_.temperature = 0;
 
 		img_sub_ = it.subscribeCamera("image_raw", 1, &OpticalFlow::flow, this);
 
+		disable_on_vpe_ = nh_priv.param("disable_on_vpe", false);
+		if (disable_on_vpe_) {
+			vpe_sub_ = nh.subscribe("mavros/vision_pose/pose", 1, &OpticalFlow::vpeCallback, this);
+		}
+
+		dyn_srv_ = std::make_shared<dynamic_reconfigure::Server<clover::FlowConfig>>(nh_priv);
+		dynamic_reconfigure::Server<clover::FlowConfig>::CallbackType cb;
+
+		cb = std::bind(&OpticalFlow::paramCallback, this, std::placeholders::_1, std::placeholders::_2);
+		dyn_srv_->setCallback(cb);
+
 		NODELET_INFO("Optical Flow initialized");
 	}
 
@@ -110,6 +128,14 @@ private:
 
 	void flow(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cinfo)
 	{
+		if (!enabled_) return;
+
+		if (disable_on_vpe_ &&
+			!last_vpe_time_.isZero() &&
+			(msg->header.stamp - last_vpe_time_).toSec() < 0.1) {
+			return;
+		}
+
 		parseCameraInfo(cinfo);
 
 		auto img = cv_bridge::toCvShare(msg, "mono8")->image;
@@ -143,7 +169,7 @@ private:
 
 		img.convertTo(curr_, CV_32F);
 
-		if (prev_.empty()) {
+		if (prev_.empty() || (msg->header.stamp - prev_stamp_).toSec() > 0.1) { // outdated previous frame
 			prev_ = curr_.clone();
 			prev_stamp_ = msg->header.stamp;
 			cv::createHanningWindow(hann_, curr_.size(), CV_32F);
@@ -179,7 +205,7 @@ private:
 			double flow_x = atan2(points_undist[0].x, focal_length_x);
 			double flow_y = atan2(points_undist[0].y, focal_length_y);
 
-			// // Convert to FCU frame
+			// Convert to FCU frame
 			geometry_msgs::Vector3Stamped flow_camera, flow_fcu;
 			flow_camera.header.frame_id = msg->header.frame_id;
 			flow_camera.header.stamp = msg->header.stamp;
@@ -196,6 +222,11 @@ private:
 			ros::Duration integration_time = msg->header.stamp - prev_stamp_;
 			uint32_t integration_time_us = integration_time.toSec() * 1.0e6;
 
+			// Calculate flow gyro
+			flow_.integrated_xgyro = flow_gyro_default_;
+			flow_.integrated_ygyro = flow_gyro_default_;
+			flow_.integrated_zgyro = flow_gyro_default_;
+
 			if (calc_flow_gyro_) {
 				try {
 					auto flow_gyro_camera = calcFlowGyro(msg->header.frame_id, prev_stamp_, msg->header.stamp);
@@ -205,9 +236,7 @@ private:
 					flow_.integrated_ygyro = flow_gyro_fcu.vector.y;
 					flow_.integrated_zgyro = flow_gyro_fcu.vector.z;
 				} catch (const tf2::TransformException& e) {
-					// Invalidate previous frame
+					// Transform not available, keep NANs in flow gyro
-					prev_.release();
-					goto publish_debug;
 				}
 			}
 
@@ -222,6 +251,14 @@ private:
 			prev_ = curr_.clone();
 			prev_stamp_ = msg->header.stamp;
 
+			// Publish estimated angular velocity
+			geometry_msgs::TwistStamped velo;
+			velo.header.stamp = msg->header.stamp;
+			velo.header.frame_id = fcu_frame_id_;
+			velo.twist.angular.x = flow_fcu.vector.x / integration_time.toSec();
+			velo.twist.angular.y = flow_fcu.vector.y / integration_time.toSec();
+			velo_pub_.publish(velo);
+
 publish_debug:
 			// Publish debug image
 			if (img_pub_.getNumSubscribers() > 0) {
@@ -234,14 +271,6 @@ publish_debug:
 				out_msg.image = img;
 				img_pub_.publish(out_msg.toImageMsg());
 			}
-
-			// Publish estimated angular velocity
-			geometry_msgs::TwistStamped velo;
-			velo.header.stamp = msg->header.stamp;
-			velo.header.frame_id = fcu_frame_id_;
-			velo.twist.angular.x = flow_fcu.vector.x / integration_time.toSec();
-			velo.twist.angular.y = flow_fcu.vector.y / integration_time.toSec();
-			velo_pub_.publish(velo);
 		}
 	}
 
@@ -262,6 +291,18 @@ publish_debug:
 
 		return flow;
 	}
+
+	void paramCallback(clover::FlowConfig &config, uint32_t level)
+	{
+		enabled_ = config.enabled;
+		if (!enabled_) {
+			prev_ = Mat(); // clear previous frame
+		}
+	}
+
+	void vpeCallback(const geometry_msgs::PoseStamped& vpe) {
+		last_vpe_time_ = vpe.header.stamp;
+	}
 };
 
 PLUGINLIB_EXPORT_CLASS(OpticalFlow, nodelet::Nodelet)

clover/src/rc.cpp

@@ -91,7 +91,7 @@ private:
 	void fakeGCSThread()
 	{
 		// Awful workaround for fixing PX4 not sending STATUSTEXTs
-		// if there is no GCS hearbeats.
+		// if there is no GCS heartbeats.
 		// TODO: use timer
 		// TODO: remove, when PX4 get this fixed.
 		ros::Publisher mavlink_pub = nh.advertise<mavros_msgs::Mavlink>("mavlink/to", 1);

clover/src/selfcheck.py

@@ -9,13 +9,14 @@
 # The above copyright notice and this permission notice shall be included in all
 # copies or substantial portions of the Software.
 
-import os
+import os, sys
 import math
 import subprocess
 import re
 from collections import OrderedDict
 import traceback
-from threading import Event
+import threading
+from threading import Event, Thread, Lock
 import numpy
 import rospy
 import tf2_ros
@@ -27,67 +28,86 @@ from mavros_msgs.msg import State, OpticalFlowRad, Mavlink
 from mavros_msgs.srv import ParamGet
 from geometry_msgs.msg import PoseStamped, TwistStamped, PoseWithCovarianceStamped, Vector3Stamped
 from visualization_msgs.msg import MarkerArray as VisualizationMarkerArray
+from diagnostic_msgs.msg import DiagnosticArray
 import tf.transformations as t
 from aruco_pose.msg import MarkerArray
 from mavros import mavlink
-
+import locale
-
-# TODO: check attitude is present
-# TODO: disk free space
-# TODO: map, base_link, body
-# TODO: rc service
-# TODO: perform commander check, ekf2 status on PX4
-# TODO: check if FCU params setter succeed
-# TODO: selfcheck ROS service (with blacklists for checks)
 
 
 rospy.init_node('selfcheck')
 
+os.environ['ROSCONSOLE_FORMAT']='${message}'
+
+# use user's locale to convert numbers, etc
+locale.setlocale(locale.LC_ALL, '')
 
 tf_buffer = tf2_ros.Buffer()
 tf_listener = tf2_ros.TransformListener(tf_buffer)
 
 
-failures = []
+thread_local = threading.local()
-infos = []
+reports_lock = Lock()
-current_check = None
+
+
+# formatting colors
+if sys.stdout.isatty():
+    GREY = '\033[90m'
+    GREEN = '\033[92m'
+    RED = '\033[31m'
+    END = '\033[0m'
+else:
+    GREY = GREEN = RED = END = ''
 
 
 def failure(text, *args):
     msg = text % args
-    rospy.logwarn('%s: %s', current_check, msg)
+    thread_local.reports += [{'failure': msg}]
-    failures.append(msg)
 
 
 def info(text, *args):
     msg = text % args
-    rospy.loginfo('%s: %s', current_check, msg)
+    thread_local.reports += [{'info': msg}]
-    infos.append(msg)
 
 
 def check(name):
     def inner(fn):
         def wrapper(*args, **kwargs):
-            failures[:] = []
+            start = rospy.get_time()
-            infos[:] = []
+            thread_local.reports = []
-            global current_check
-            current_check = name
             try:
                 fn(*args, **kwargs)
             except Exception as e:
                 traceback.print_exc()
                 rospy.logerr('%s: exception occurred', name)
-                return
+            with reports_lock:
-            if not failures and not infos:
+                for report in thread_local.reports:
-                rospy.loginfo('%s: OK', name)
+                    if 'failure' in report:
+                        rospy.logerr('%s: %s', name, report['failure'])
+                    elif 'info' in report:
+                        rospy.loginfo(GREY + name + END + ': ' + report['info'])
+                if not thread_local.reports:
+                    rospy.loginfo(GREY + name + END + ': ' + GREEN + 'OK' + END)
+                if rospy.get_param('~time', False):
+                    rospy.loginfo('%s: %.1f sec', name, rospy.get_time() - start)
         return wrapper
     return inner
 
 
+def ff(value, precision=2):
+    # safely format float or int
+    if value is None:
+        return RED + '???' + END
+    if isinstance(value, float):
+        return ('{:.' + str(precision + 1) + '}').format(value)
+    elif isinstance(value, int):
+        return str(value)
+
+
 param_get = rospy.ServiceProxy('mavros/param/get', ParamGet)
 
 
-def get_param(name):
+def get_param(name, default=None, strict=True):
     try:
         res = param_get(param_id=name)
     except rospy.ServiceException as e:
@@ -95,13 +115,19 @@ def get_param(name):
         return None
 
     if not res.success:
+        if strict:
             failure('unable to retrieve PX4 parameter %s', name)
+        return default
     else:
         if res.value.integer != 0:
             return res.value.integer
         return res.value.real
 
 
+def get_paramf(name, precision=2):
+    return ff(get_param(name), precision)
+
+
 recv_event = Event()
 link = mavutil.mavlink.MAVLink('', 255, 1)
 mavlink_pub = rospy.Publisher('mavlink/to', Mavlink, queue_size=1)
@@ -146,6 +172,24 @@ def mavlink_exec(cmd, timeout=3.0):
     return mavlink_recv
 
 
+def read_diagnostics(name, key):
+    e = Event()
+    def cb(msg):
+        for status in msg.status:
+            if status.name.lower() == name.lower():
+                for value in status.values:
+                    if value.key.lower() == key.lower():
+                        cb.value = value.value
+                        e.set()
+                        return
+
+    cb.value = None
+    sub = rospy.Subscriber('/diagnostics', DiagnosticArray, cb)
+    e.wait(1.0)  # wait to read all the diagnostics from nodes publishing them
+    sub.unregister()
+    return cb.value
+
+
 BOARD_ROTATIONS = {
     0: 'no rotation',
     1: 'yaw 45°',
@@ -191,31 +235,36 @@ def check_fcu():
         state = rospy.wait_for_message('mavros/state', State, timeout=3)
         if not state.connected:
             failure('no connection to the FCU (check wiring)')
+            info('fcu_url = %s', rospy.get_param('mavros/fcu_url', '?'))
             return
 
+        if not is_process_running('px4', exact=True): # can't use px4 console in SITL
+            clover_tag = re.compile(r'-cl[oe]ver\.\d+$')
+            clover_fw = False
+
             # 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.*)$')
+            for line in version_str.split('\n'):
-        is_clover_firmware = False
+                if line.startswith('FW version: '):
-        for ver_line in version_str.split('\n'):
+                    info(line[len('FW version: '):])
-            match = r.search(ver_line)
+                elif line.startswith('FW git tag: '): # only Clover's firmware
-            if match is not None:
+                    tag = line[len('FW git tag: '):]
-                field, version = match.groups()
+                    clover_fw = clover_tag.search(tag)
-                info('firmware %s: %s' % (field, version))
+                    info(tag)
-                if 'clover' in version or 'clever' in version:
+                elif line.startswith('HW arch: '):
-                    is_clover_firmware = True
+                    info(line[len('HW arch: '):])
 
-        if not is_clover_firmware:
+            if not clover_fw:
-            failure('not running Clover PX4 firmware, https://clover.coex.tech/firmware')
+                info('not Clover PX4 firmware, check https://clover.coex.tech/firmware')
 
         est = get_param('SYS_MC_EST_GROUP')
         if est == 1:
             info('selected estimator: LPE')
-            fuse = get_param('LPE_FUSION')
+            fuse = int(get_param('LPE_FUSION'))
             if fuse & (1 << 4):
                 info('LPE_FUSION: land detector fusion is enabled')
             else:
@@ -247,6 +296,7 @@ def check_fcu():
         if cbrk_usb_chk != 197848:
             failure('set parameter CBRK_USB_CHK to 197848 for flying with USB connected')
 
+        if not is_process_running('px4', exact=True): # skip battery check in SITL
             try:
                 battery = rospy.wait_for_message('mavros/battery', BatteryState, timeout=3)
                 if not battery.cell_voltage:
@@ -260,8 +310,21 @@ def check_fcu():
             except rospy.ROSException:
                 failure('no battery state')
 
+        # time sync check
+        try:
+            info('time sync offset: %.2f s', float(read_diagnostics('mavros: Time Sync', 'Estimated time offset (s)')))
+        except:
+            failure('cannot read time sync offset')
+
     except rospy.ROSException:
-        failure('no MAVROS state (check wiring)')
+        failure('no MAVROS state')
+        fcu_url = rospy.get_param('mavros/fcu_url', '?')
+        if fcu_url == '/dev/px4fmu':
+            if not os.path.exists('/lib/udev/rules.d/99-px4fmu.rules'):
+                info('udev rules are not installed, install udev rules or change usb_device to /dev/ttyACM0 in mavros.launch')
+            else:
+                info('udev did\'t recognize px4fmu device, check wiring or change usb_device to /dev/ttyACM0 in mavros.launch')
+        info('fcu_url = %s', rospy.get_param('mavros/fcu_url', '?'))
 
 
 def describe_direction(v):
@@ -328,7 +391,7 @@ def is_process_running(binary, exact=False, full=False):
         if exact:
             args.append('-x')  # match exactly with the command name
         if full:
-            args.append('-f')  # use full process name to match
+            args.append('-f')  # use full command line (including arguments) to match
         args.append(binary)
         subprocess.check_output(args)
         return True
@@ -338,19 +401,24 @@ def is_process_running(binary, exact=False, full=False):
 
 @check('ArUco markers')
 def check_aruco():
+    markers = None
+
     if is_process_running('aruco_detect', full=True):
         try:
-            info('aruco_detect/length = %g m', rospy.get_param('aruco_detect/length'))
+            info('aruco_detect/length = %g m', rospy.get_param('aruco_detect/length', '?'))
         except KeyError:
             failure('aruco_detect/length parameter is not set')
-        known_tilt = rospy.get_param('aruco_detect/known_tilt', '')
+        known_vertical = rospy.get_param('aruco_detect/known_vertical', '')
-        if known_tilt == 'map':
+        flip_vertical = rospy.get_param('aruco_detect/flip_vertical', False)
-            known_tilt += ' (ALL markers are on the floor)'
+        description = ''
-        elif known_tilt == 'map_flipped':
+        if known_vertical == 'map' and not flip_vertical:
-            known_tilt += ' (ALL markers are on the ceiling)'
+            description = ' (all markers are on the floor)'
-        info('aruco_detector/known_tilt = %s', known_tilt)
+        elif known_vertical == 'map' and flip_vertical:
+            description = ' (all markers are on the ceiling)'
+        info('aruco_detect/known_vertical = %s', known_vertical)
+        info('aruco_detect/flip_vertical = %s%s', flip_vertical, description)
         try:
-            rospy.wait_for_message('aruco_detect/markers', MarkerArray, timeout=1)
+            markers = rospy.wait_for_message('aruco_detect/markers', MarkerArray, timeout=0.8)
         except rospy.ROSException:
             failure('no markers detection')
             return
@@ -359,42 +427,61 @@ def check_aruco():
         return
 
     if is_process_running('aruco_map', full=True):
-        known_tilt = rospy.get_param('aruco_map/known_tilt', '')
+        known_vertical = rospy.get_param('aruco_map/known_vertical', '')
-        if known_tilt == 'map':
+        flip_vertical = rospy.get_param('aruco_map/flip_vertical', False)
-            known_tilt += ' (marker\'s map is on the floor)'
+        description = ''
-        elif known_tilt == 'map_flipped':
+        if known_vertical == 'map' and not flip_vertical:
-            known_tilt += ' (marker\'s map is on the ceiling)'
+            description += ' (markers map is on the floor)'
-        info('aruco_map/known_tilt = %s', known_tilt)
+        elif known_vertical == 'map' and flip_vertical:
+            description += ' (markers map is on the ceiling)'
+        info('aruco_map/known_vertical = %s', known_vertical)
+        info('aruco_map/flip_vertical = %s%s', flip_vertical, description)
 
         try:
-            visualization = rospy.wait_for_message('aruco_map/visualization', VisualizationMarkerArray, timeout=1)
+            visualization = rospy.wait_for_message('aruco_map/visualization', VisualizationMarkerArray, timeout=0.8)
             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)
+            rospy.wait_for_message('aruco_map/pose', PoseWithCovarianceStamped, timeout=0.8)
         except rospy.ROSException:
+            if not markers:
+                info('no map detection as no markers detection')
+            elif not markers.markers:
+                info('no map detection as no markers detected')
+            else:
                 failure('no map detection')
     else:
         info('aruco_map is not running')
 
 
+def is_on_the_floor():
+    try:
+        dist = rospy.wait_for_message('rangefinder/range', Range, timeout=1)
+        return dist.range < 0.3
+    except rospy.ROSException:
+        return False
+
+
 @check('Vision position estimate')
 def check_vpe():
     vis = None
     try:
-        vis = rospy.wait_for_message('mavros/vision_pose/pose', PoseStamped, timeout=1)
+        vis = rospy.wait_for_message('mavros/vision_pose/pose', PoseStamped, timeout=0.8)
     except rospy.ROSException:
         try:
-            vis = rospy.wait_for_message('mavros/mocap/pose', PoseStamped, timeout=1)
+            vis = rospy.wait_for_message('mavros/mocap/pose', PoseStamped, timeout=0.8)
         except rospy.ROSException:
-            failure('no VPE or MoCap messages')
+            if not is_process_running('vpe_publisher', full=True):
-            # check if vpe_publisher is running
+                info('no vision position estimate, vpe_publisher is not running')
-            try:
+            elif rospy.get_param('aruco_map/known_vertical', '') == 'map' \
-                subprocess.check_output(['pgrep', '-x', 'vpe_publisher'])
+                   and rospy.get_param('aruco_map/flip_vertical', False):
-            except subprocess.CalledProcessError:
+                failure('no vision position estimate, markers are on the ceiling')
-                return  # it's not running, skip following checks
+            elif is_on_the_floor():
+                info('no vision position estimate, the drone is on the floor')
+            else:
+                failure('no vision position estimate')
 
     # check PX4 settings
     est = get_param('SYS_MC_EST_GROUP')
@@ -406,26 +493,37 @@ def check_vpe():
         if vision_yaw_w == 0:
             failure('vision yaw weight is zero, change ATT_W_EXT_HDG parameter')
         else:
-            info('Vision yaw weight: %.2f', vision_yaw_w)
+            info('vision yaw weight: %s', ff(vision_yaw_w))
-        fuse = get_param('LPE_FUSION')
+        fuse = int(get_param('LPE_FUSION'))
         if not fuse & (1 << 2):
             failure('vision position fusion is disabled, change LPE_FUSION parameter')
         delay = get_param('LPE_VIS_DELAY')
         if delay != 0:
-            failure('LPE_VIS_DELAY parameter is %s, but it should be zero', delay)
+            failure('LPE_VIS_DELAY = %s, but it should be zero', delay)
-        info('LPE_VIS_XY is %.2f m, LPE_VIS_Z is %.2f m', get_param('LPE_VIS_XY'), get_param('LPE_VIS_Z'))
+        info('LPE_VIS_XY = %s m, LPE_VIS_Z = %s m', get_paramf('LPE_VIS_XY'), get_paramf('LPE_VIS_Z'))
     elif est == 2:
-        fuse = get_param('EKF2_AID_MASK')
+        ev_ctrl = get_param('EKF2_EV_CTRL', strict=False)
+        if ev_ctrl is not None: # PX4 after v1.14
+            ev_ctrl = int(ev_ctrl)
+            if not ev_ctrl & (1 << 0):
+                failure('vision horizontal position fusion is disabled, change EKF2_EV_CTRL parameter')
+            if not ev_ctrl & (1 << 1):
+                failure('vision vertical position fusion is disabled, change EKF2_EV_CTRL parameter')
+            if not ev_ctrl & (1 << 3):
+                failure('vision yaw fusion is disabled, change EKF2_EV_CTRL parameter')
+        else: # PX4 before v1.14
+            fuse = int(get_param('EKF2_AID_MASK'))
             if not fuse & (1 << 3):
                 failure('vision position fusion is disabled, change EKF2_AID_MASK parameter')
             if not fuse & (1 << 4):
                 failure('vision yaw fusion is disabled, change EKF2_AID_MASK parameter')
+
         delay = get_param('EKF2_EV_DELAY')
         if delay != 0:
-            failure('EKF2_EV_DELAY is %.2f, but it should be zero', delay)
+            failure('EKF2_EV_DELAY = %.2f, but it should be zero', delay)
-        info('EKF2_EVA_NOISE is %.3f, EKF2_EVP_NOISE is %.3f',
+        info('EKF2_EVA_NOISE = %s, EKF2_EVP_NOISE = %s',
-            get_param('EKF2_EVA_NOISE'),
+            get_paramf('EKF2_EVA_NOISE', 3),
-            get_param('EKF2_EVP_NOISE'))
+            get_paramf('EKF2_EVP_NOISE', 3))
 
     if not vis:
         return
@@ -483,6 +581,12 @@ def check_local_position():
             failure('roll is %.2f deg; place copter horizontally or redo level horizon calib',
                     math.degrees(roll))
 
+        if not tf_buffer.can_transform('base_link', pose.header.frame_id, rospy.get_rostime(), rospy.Duration(0.5)):
+            failure('can\'t transform from %s to base_link (timeout 0.5 s): is TF enabled?', pose.header.frame_id)
+
+        if not tf_buffer.can_transform('body', pose.header.frame_id, rospy.get_rostime(), rospy.Duration(0.5)):
+            failure('can\'t transform from %s to body (timeout 0.5 s)', pose.header.frame_id)
+
     except rospy.ROSException:
         failure('no local position')
 
@@ -517,13 +621,25 @@ def check_velocity():
 @check('Global position (GPS)')
 def check_global_position():
     try:
-        rospy.wait_for_message('mavros/global_position/global', NavSatFix, timeout=1)
+        rospy.wait_for_message('mavros/global_position/global', NavSatFix, timeout=0.8)
     except rospy.ROSException:
         info('no global position')
+        if get_param('SYS_MC_EST_GROUP') == 2:
+            gps_ctrl = get_param('EKF2_GPS_CTRL', strict=False)
+            if gps_ctrl is not None: # PX4 after v1.14
+                if int(gps_ctrl) & (1 << 0):
+                    failure('GPS fusion enabled may suppress vision position aiding, change EKF2_GPS_CTRL')
+            else: # PX4 before v1.14
+                if int(get_param('EKF2_AID_MASK', 0)) & (1 << 0):
+                    failure('GPS fusion enabled may suppress vision position aiding, change EKF2_AID_MASK')
 
 
 @check('Optical flow')
 def check_optical_flow():
+    if not is_process_running('optical_flow', full=True):
+        info('optical_flow is not running')
+        return
+
     # TODO:check FPS!
     try:
         rospy.wait_for_message('mavros/px4flow/raw/send', OpticalFlowRad, timeout=0.5)
@@ -531,40 +647,49 @@ def check_optical_flow():
         # check PX4 settings
         rot = get_param('SENS_FLOW_ROT')
         if rot != 0:
-            failure('SENS_FLOW_ROT parameter is %s, but it should be zero', rot)
+            failure('SENS_FLOW_ROT = %s, but it should be zero', rot)
         est = get_param('SYS_MC_EST_GROUP')
         if est == 1:
-            fuse = get_param('LPE_FUSION')
+            fuse = int(get_param('LPE_FUSION'))
             if not fuse & (1 << 1):
                 failure('optical flow fusion is disabled, change LPE_FUSION parameter')
             if not fuse & (1 << 1):
                 failure('flow gyro compensation is disabled, change LPE_FUSION parameter')
-            scale = get_param('LPE_FLW_SCALE')
+            scale = get_param('LPE_FLW_SCALE', 1)
             if not numpy.isclose(scale, 1.0):
-                failure('LPE_FLW_SCALE parameter is %.2f, but it should be 1.0', scale)
+                failure('LPE_FLW_SCALE = %.2f, but it should be 1.0', scale)
 
-            info('LPE_FLW_QMIN is %s, LPE_FLW_R is %.4f, LPE_FLW_RR is %.4f, SENS_FLOW_MINHGT is %.3f, SENS_FLOW_MAXHGT is %.3f',
+            info('LPE_FLW_QMIN = %s, LPE_FLW_R = %s, LPE_FLW_RR = %s',
-                          get_param('LPE_FLW_QMIN'),
+                          get_paramf('LPE_FLW_QMIN'),
-                          get_param('LPE_FLW_R'),
+                          get_paramf('LPE_FLW_R', 4),
-                          get_param('LPE_FLW_RR'),
+                          get_paramf('LPE_FLW_RR', 4))
-                          get_param('SENS_FLOW_MINHGT'),
-                          get_param('SENS_FLOW_MAXHGT'))
         elif est == 2:
-            fuse = get_param('EKF2_AID_MASK')
+            of_ctrl = get_param('EKF2_OF_CTRL', strict=False)
+            if of_ctrl is not None: # PX4 after v1.14
+                if of_ctrl == 0:
+                    failure('optical flow fusion is disabled, change EKF2_OF_CTRL')
+            else: # PX4 before v1.14
+                fuse = int(get_param('EKF2_AID_MASK', 0))
                 if not fuse & (1 << 1):
                     failure('optical flow fusion is disabled, change EKF2_AID_MASK parameter')
-            delay = get_param('EKF2_OF_DELAY')
+            delay = get_param('EKF2_OF_DELAY', 0)
             if delay != 0:
-                failure('EKF2_OF_DELAY is %.2f, but it should be zero', delay)
+                failure('EKF2_OF_DELAY = %.2f, but it should be zero', delay)
-            info('EKF2_OF_QMIN is %s, EKF2_OF_N_MIN is %.4f, EKF2_OF_N_MAX is %.4f, SENS_FLOW_MINHGT is %.3f, SENS_FLOW_MAXHGT is %.3f',
+            info('EKF2_OF_QMIN = %s, EKF2_OF_N_MIN = %s, EKF2_OF_N_MAX = %s',
-                          get_param('EKF2_OF_QMIN'),
+                          get_paramf('EKF2_OF_QMIN'),
-                          get_param('EKF2_OF_N_MIN'),
+                          get_paramf('EKF2_OF_N_MIN', 4),
-                          get_param('EKF2_OF_N_MAX'),
+                          get_paramf('EKF2_OF_N_MAX', 4))
-                          get_param('SENS_FLOW_MINHGT'),
+        info('SENS_FLOW_MINHGT = %s, SENS_FLOW_MAXHGT = %s', get_paramf('SENS_FLOW_MINHGT', 3), get_paramf('SENS_FLOW_MAXHGT', 3))
-                          get_param('SENS_FLOW_MAXHGT'))
 
     except rospy.ROSException:
-        failure('no optical flow data (from Raspberry)')
+        if rospy.get_param('optical_flow/disable_on_vpe', False):
+            try:
+                rospy.wait_for_message('mavros/vision_pose/pose', PoseStamped, timeout=1)
+                info('no optical flow as disable_on_vpe is true')
+            except:
+                failure('no optical flow on RPi, disable_on_vpe is true, but no vision pose also')
+        else:
+            failure('no optical flow on RPi')
 
 
 @check('Rangefinder')
@@ -588,19 +713,22 @@ def check_rangefinder():
 
     est = get_param('SYS_MC_EST_GROUP')
     if est == 1:
-        fuse = get_param('LPE_FUSION')
+        fuse = int(get_param('LPE_FUSION', 0))
         if not fuse & (1 << 5):
             info('"pub agl as lpos down" in LPE_FUSION is disabled, NOT operating over flat surface')
         else:
             info('"pub agl as lpos down" in LPE_FUSION is enabled, operating over flat surface')
 
     elif est == 2:
+        hgt = get_param('EKF2_HGT_REF', strict=False)
+        if hgt is None: # PX4 before v1.14
             hgt = get_param('EKF2_HGT_MODE')
         if hgt != 2:
             info('EKF2_HGT_MODE != Range sensor, NOT operating over flat surface')
         else:
             info('EKF2_HGT_MODE = Range sensor, operating over flat surface')
-        aid = get_param('EKF2_RNG_AID')
+        aid = get_param('EKF2_RNG_AID', strict=False)
+        if aid is not None: # PX4 before v1.14
             if aid != 1:
                 info('EKF2_RNG_AID != 1, range sensor aiding disabled')
             else:
@@ -609,16 +737,20 @@ def check_rangefinder():
 
 @check('Boot duration')
 def check_boot_duration():
+    if not os.path.exists('/etc/clover_version'):
+        info('skip check')
+        return # Don't check not on Clover's image
+
     output = subprocess.check_output('systemd-analyze').decode()
     r = re.compile(r'([\d\.]+)s\s*$', flags=re.MULTILINE)
     duration = float(r.search(output).groups()[0])
-    if duration > 15:
+    if duration > 20:
         failure('long Raspbian boot duration: %ss (systemd-analyze for analyzing)', duration)
 
 
 @check('CPU usage')
 def check_cpu_usage():
-    WHITELIST = 'nodelet',
+    WHITELIST = 'nodelet', 'gzclient', 'gzserver', 'selfcheck.py'
     CMD = "top -n 1 -b -i | tail -n +8 | awk '{ printf(\"%-8s\\t%-8s\\t%-8s\\n\", $1, $9, $12); }'"
     output = subprocess.check_output(CMD, shell=True).decode()
     processes = output.split('\n')
@@ -627,13 +759,16 @@ def check_cpu_usage():
             continue
         pid, cpu, cmd = process.split('\t')
 
-        if cmd.strip() not in WHITELIST and float(cpu) > 30:
+        if cmd.strip() not in WHITELIST and locale.atof(cpu) > 30:
             failure('high CPU usage (%s%%) detected: %s (PID %s)',
                     cpu.strip(), cmd.strip(), pid.strip())
 
 
 @check('clover.service')
 def check_clover_service():
+    if not os.path.exists('/etc/clover_version'):
+        return # Don't check not on Clover's image
+
     try:
         output = subprocess.check_output('systemctl show -p ActiveState --value clover.service'.split(),
                                          stderr=subprocess.STDOUT).decode()
@@ -646,13 +781,22 @@ def check_clover_service():
     elif 'failed' in output:
         failure('service failed to run, check your launch-files')
 
-    r = re.compile(r'^(.*)\[(FATAL|ERROR)\] \[\d+.\d+\]: (.*?)(\x1b(.*))?$')
+    BLACKLIST = 'Unexpected command 520', 'Time jump detected', 'different index:'
+
+    r = re.compile(r'^(.*)\[(FATAL|ERROR| WARN)\] \[\d+.\d+\]: (.*?)(\x1b(.*))?$')
     error_count = OrderedDict()
     try:
         for line in open('/tmp/clover.err', 'r'):
+            skip = False
+            for substr in BLACKLIST:
+                if substr in line:
+                    skip = True
+            if skip:
+                continue
+
             node_error = r.search(line)
             if node_error:
-                msg = node_error.groups()[1] + ': ' + node_error.groups()[2]
+                msg = node_error.groups()[1].strip() + ': ' + node_error.groups()[2]
                 if msg in error_count:
                     error_count[msg] += 1
                 else:
@@ -674,12 +818,19 @@ def check_clover_service():
 def check_image():
     try:
         info('version: %s', open('/etc/clover_version').read().strip())
+    except IOError:
+        try:
+            info('VM version: %s', open('/etc/clover_vm_version').read().strip())
         except IOError:
             info('no /etc/clover_version file, not the Clover image?')
 
 
 @check('Preflight status')
 def check_preflight_status():
+    if is_process_running('px4', exact=True):
+        info('can\'t check in SITL')
+        return
+
     # Make sure the console is available to us
     mavlink_exec('\n')
     cmdr_output = mavlink_exec('commander check')
@@ -701,6 +852,10 @@ def check_preflight_status():
 
 @check('Network')
 def check_network():
+    if not os.path.exists('/etc/clover_version'):
+        # TODO:
+        return # Don't check not on Clover's image
+
     ros_hostname = os.environ.get('ROS_HOSTNAME', '').strip()
 
     if not ros_hostname:
@@ -723,6 +878,14 @@ def check_network():
 
 @check('RPi health')
 def check_rpi_health():
+    try:
+        import shutil
+        total, used, free = shutil.disk_usage('/')
+        if free < 1024 * 1024 * 1024:
+            failure('disk space is less than 1 GB; consider removing logs (~/.ros/log/)')
+    except Exception as e:
+        info('could not check the disk free space: %s', str(e))
+
     # `vcgencmd get_throttled` output codes taken from
     # https://github.com/raspberrypi/documentation/blob/JamesH65-patch-vcgencmd-vcdbg-docs/raspbian/applications/vcgencmd.md#get_throttled
     # TODO: support more base platforms?
@@ -753,7 +916,7 @@ def check_rpi_health():
         # with some of the FLAGs OR'ed together
         output = subprocess.check_output(['vcgencmd', 'get_throttled']).decode()
     except OSError:
-        failure('could not call vcgencmd binary; not a Raspberry Pi?')
+        info('could not call vcgencmd binary; not a Raspberry Pi?')
         return
 
     throttle_mask = int(output.split('=')[1], base=16)
@@ -770,26 +933,47 @@ def check_board():
         info('could not open /proc/device-tree/model, not a Raspberry Pi?')
 
 
+def parallel_for(fns):
+    threads = []
+    for fn in fns:
+        thread = Thread(target=fn)
+        thread.start()
+        threads.append(thread)
+    for thread in threads:
+        thread.join()
+
+
+def consequentially_for(fns):
+    for fn in fns:
+        fn()
+
+
 def selfcheck():
-    check_image()
+    checks = [
-    check_board()
+        check_image,
-    check_clover_service()
+        check_board,
-    check_network()
+        check_clover_service,
-    check_fcu()
+        check_network,
-    check_imu()
+        check_fcu,
-    check_local_position()
+        check_imu,
-    check_velocity()
+        check_local_position,
-    check_global_position()
+        check_velocity,
-    check_preflight_status()
+        check_global_position,
-    check_main_camera()
+        check_preflight_status,
-    check_aruco()
+        check_main_camera,
-    check_simpleoffboard()
+        check_aruco,
-    check_optical_flow()
+        check_simpleoffboard,
-    check_vpe()
+        check_optical_flow,
-    check_rangefinder()
+        check_vpe,
-    check_rpi_health()
+        check_rangefinder,
-    check_cpu_usage()
+        check_rpi_health,
-    check_boot_duration()
+        check_cpu_usage,
+        check_boot_duration,
+    ]
+    if rospy.get_param('~parallel', False):
+        parallel_for(checks)
+    else:
+        consequentially_for(checks)
 
 
 if __name__ == '__main__':

clover/src/simple_offboard.cpp

@@ -23,12 +23,14 @@
 #include <tf2_ros/static_transform_broadcaster.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 #include <std_srvs/Trigger.h>
+#include <geometry_msgs/PointStamped.h>
 #include <geometry_msgs/PoseStamped.h>
 #include <geometry_msgs/TwistStamped.h>
 #include <geometry_msgs/Vector3Stamped.h>
 #include <geometry_msgs/QuaternionStamped.h>
 #include <sensor_msgs/NavSatFix.h>
 #include <sensor_msgs/BatteryState.h>
+#include <sensor_msgs/Range.h>
 #include <mavros_msgs/CommandBool.h>
 #include <mavros_msgs/SetMode.h>
 #include <mavros_msgs/PositionTarget.h>
@@ -37,14 +39,19 @@
 #include <mavros_msgs/State.h>
 #include <mavros_msgs/StatusText.h>
 #include <mavros_msgs/ManualControl.h>
+#include <mavros_msgs/Altitude.h>
 
 #include <clover/GetTelemetry.h>
 #include <clover/Navigate.h>
 #include <clover/NavigateGlobal.h>
+#include <clover/SetAltitude.h>
+#include <clover/SetYaw.h>
+#include <clover/SetYawRate.h>
 #include <clover/SetPosition.h>
 #include <clover/SetVelocity.h>
 #include <clover/SetAttitude.h>
 #include <clover/SetRates.h>
+#include <clover/State.h>
 
 using std::string;
 using std::isnan;
@@ -54,6 +61,7 @@ using namespace clover;
 using mavros_msgs::PositionTarget;
 using mavros_msgs::AttitudeTarget;
 using mavros_msgs::Thrust;
+using mavros_msgs::Altitude;
 
 // tf2
 tf2_ros::Buffer tf_buffer;
@@ -61,6 +69,7 @@ std::shared_ptr<tf2_ros::TransformBroadcaster> transform_broadcaster;
 std::shared_ptr<tf2_ros::StaticTransformBroadcaster> static_transform_broadcaster;
 
 // Parameters
+string mavros;
 string local_frame;
 string fcu_frame;
 ros::Duration transform_timeout;
@@ -78,35 +87,43 @@ float default_speed;
 bool auto_release;
 bool land_only_in_offboard, nav_from_sp, check_kill_switch;
 std::map<string, string> reference_frames;
+string terrain_frame_mode;
 
 // Publishers
-ros::Publisher attitude_pub, attitude_raw_pub, position_pub, position_raw_pub, rates_pub, thrust_pub;
+ros::Publisher attitude_pub, attitude_raw_pub, position_pub, position_raw_pub, rates_pub, thrust_pub, state_pub;
 
 // Service clients
 ros::ServiceClient arming, set_mode;
 
 // Containers
 ros::Timer setpoint_timer;
-tf::Quaternion tq;
 PoseStamped position_msg;
 PositionTarget position_raw_msg;
-AttitudeTarget att_raw_msg;
+//TwistStamped rates_msg;
-Thrust thrust_msg;
-TwistStamped rates_msg;
 TransformStamped target, setpoint;
 geometry_msgs::TransformStamped body;
+geometry_msgs::TransformStamped terrain;
 
 // State
 PoseStamped nav_start;
-PoseStamped setpoint_position, setpoint_position_transformed;
+PointStamped setpoint_position;
-Vector3Stamped setpoint_velocity, setpoint_velocity_transformed;
+PointStamped setpoint_altitude;
-QuaternionStamped setpoint_attitude, setpoint_attitude_transformed;
+Vector3Stamped setpoint_velocity;
-float setpoint_yaw_rate;
+float setpoint_yaw, setpoint_roll, setpoint_pitch;
+Vector3 setpoint_rates;
+string yaw_frame_id;
+float setpoint_thrust;
 float nav_speed;
+float setpoint_lat = NAN, setpoint_lon = NAN;
 bool busy = false;
 bool wait_armed = false;
 bool nav_from_sp_flag = false;
 
+// Last published
+PoseStamped setpoint_pose_local;
+Vector3Stamped setpoint_velocity_local;
+float yaw_local;
+
 enum setpoint_type_t {
 	NONE,
 	NAVIGATE,
@@ -114,7 +131,10 @@ enum setpoint_type_t {
 	POSITION,
 	VELOCITY,
 	ATTITUDE,
-	RATES
+	RATES,
+	_ALTITUDE,
+	_YAW,
+	_YAW_RATE,
 };
 
 enum setpoint_type_t setpoint_type = NONE;
@@ -149,6 +169,9 @@ void handleState(const mavros_msgs::State& s)
 inline void publishBodyFrame()
 {
 	if (body.child_frame_id.empty()) return;
+	if (!body.header.stamp.isZero() && body.header.stamp == local_position.header.stamp) {
+		return; // avoid TF_REPEATED_DATA warnings
+	}
 
 	tf::Quaternion q;
 	q.setRPY(0, 0, tf::getYaw(local_position.pose.orientation));
@@ -166,7 +189,7 @@ void handleLocalPosition(const PoseStamped& pose)
 {
 	local_position = pose;
 	publishBodyFrame();
-	// TODO: terrain?, home?
+	// TODO: home?
 }
 
 // wait for transform without interrupting publishing setpoints
@@ -181,9 +204,33 @@ inline bool waitTransform(const string& target, const string& source,
 		ros::spinOnce();
 		r.sleep();
 	}
+	return false;
 }
 
-#define TIMEOUT(msg, timeout) (ros::Time::now() - msg.header.stamp > timeout)
+void publishTerrain(const double distance, const ros::Time& stamp)
+{
+	if (!waitTransform(local_frame, body.child_frame_id, stamp, ros::Duration(0.1))) return;
+
+	auto t = tf_buffer.lookupTransform(local_frame, body.child_frame_id, stamp);
+	t.child_frame_id = terrain.child_frame_id;
+	t.transform.translation.z -= distance;
+	static_transform_broadcaster->sendTransform(t);
+}
+
+void handleAltitude(const Altitude& alt)
+{
+	if (!std::isfinite(alt.bottom_clearance)) return;
+	publishTerrain(alt.bottom_clearance, alt.header.stamp);
+}
+
+void handleRange(const Range& range)
+{
+	if (!std::isfinite(range.range)) return;
+	// TODO: check it's facing down
+	publishTerrain(range.range, range.header.stamp);
+}
+
+#define TIMEOUT(msg, timeout) (msg.header.stamp.isZero() || (ros::Time::now() - msg.header.stamp > timeout))
 
 bool getTelemetry(GetTelemetry::Request& req, GetTelemetry::Response& res)
 {
@@ -202,11 +249,11 @@ bool getTelemetry(GetTelemetry::Request& req, GetTelemetry::Response& res)
 	res.vx = NAN;
 	res.vy = NAN;
 	res.vz = NAN;
-	res.pitch = NAN;
 	res.roll = NAN;
+	res.pitch = NAN;
 	res.yaw = NAN;
-	res.pitch_rate = NAN;
 	res.roll_rate = NAN;
+	res.pitch_rate = NAN;
 	res.yaw_rate = NAN;
 	res.voltage = NAN;
 	res.cell_voltage = NAN;
@@ -336,20 +383,20 @@ inline float getDistance(const Point& from, const Point& to)
 	return hypot(from.x - to.x, from.y - to.y, from.z - to.z);
 }
 
-void getNavigateSetpoint(const ros::Time& stamp, float speed, Point& nav_setpoint)
+void getNavigateSetpoint(const ros::Time& stamp, const float speed, Point& nav_setpoint)
 {
 	if (wait_armed) {
 		// don't start navigating if we're waiting arming
 		nav_start.header.stamp = stamp;
 	}
 
-	float distance = getDistance(nav_start.pose.position, setpoint_position_transformed.pose.position);
+	float distance = getDistance(nav_start.pose.position, setpoint_pose_local.pose.position);
 	float time = distance / speed;
 	float passed = std::min((stamp - nav_start.header.stamp).toSec() / time, 1.0);
 
-	nav_setpoint.x = nav_start.pose.position.x + (setpoint_position_transformed.pose.position.x - nav_start.pose.position.x) * passed;
+	nav_setpoint.x = nav_start.pose.position.x + (setpoint_pose_local.pose.position.x - nav_start.pose.position.x) * passed;
-	nav_setpoint.y = nav_start.pose.position.y + (setpoint_position_transformed.pose.position.y - nav_start.pose.position.y) * passed;
+	nav_setpoint.y = nav_start.pose.position.y + (setpoint_pose_local.pose.position.y - nav_start.pose.position.y) * passed;
-	nav_setpoint.z = nav_start.pose.position.z + (setpoint_position_transformed.pose.position.z - nav_start.pose.position.z) * passed;
+	nav_setpoint.z = nav_start.pose.position.z + (setpoint_pose_local.pose.position.z - nav_start.pose.position.z) * passed;
 }
 
 PoseStamped globalToLocal(double lat, double lon)
@@ -380,44 +427,101 @@ PoseStamped globalToLocal(double lat, double lon)
 	return pose;
 }
 
+// publish navigate_target frame
+void publishTarget(ros::Time stamp, bool _static = false)
+{
+	bool single_frame = (setpoint_position.header.frame_id == setpoint_altitude.header.frame_id);
+
+	// handle yaw for target frame
+	if (setpoint_yaw_type == YAW || setpoint_yaw_type == YAW_RATE) { // use last set yaw for yaw_rate
+		if (setpoint_altitude.header.frame_id == yaw_frame_id) {
+			target.transform.rotation = tf::createQuaternionMsgFromYaw(setpoint_yaw);
+		} else {
+			single_frame = false;
+			target.transform.rotation = tf::createQuaternionMsgFromYaw(yaw_local);
+		}
+	} else if (setpoint_yaw_type == TOWARDS) {
+		single_frame = false;
+		target.transform.rotation = tf::createQuaternionMsgFromYaw(yaw_local);
+	}
+
+	if (_static && single_frame) {
+		// publish at user's command, if all frames are the same
+		target.header.frame_id = setpoint_position.header.frame_id;
+		target.header.stamp = stamp;
+		target.transform.translation.x = setpoint_position.point.x;
+		target.transform.translation.y = setpoint_position.point.y;
+		target.transform.translation.z = setpoint_position.point.z;
+
+	} else if (!_static) {
+		// publish at each iteration, if frames are different
+		target.header = setpoint_pose_local.header;
+		target.transform.translation.x = setpoint_pose_local.pose.position.x;
+		target.transform.translation.y = setpoint_pose_local.pose.position.y;
+		target.transform.translation.z = setpoint_pose_local.pose.position.z;
+	}
+
+	static_transform_broadcaster->sendTransform(target);
+}
+
 void publish(const ros::Time stamp)
 {
 	if (setpoint_type == NONE) return;
 
 	position_raw_msg.header.stamp = stamp;
-	thrust_msg.header.stamp = stamp;
-	rates_msg.header.stamp = stamp;
 
-	try {
+	// transform position
-		// transform position and/or yaw
+	if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == POSITION) {
-		if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == POSITION || setpoint_type == VELOCITY || setpoint_type == ATTITUDE) {
 		setpoint_position.header.stamp = stamp;
-			tf_buffer.transform(setpoint_position, setpoint_position_transformed, local_frame, ros::Duration(0.05));
+		setpoint_altitude.header.stamp = stamp;
+		// transform xy
+		try {
+			auto xy = tf_buffer.transform(setpoint_position, local_frame, ros::Duration(0.05));
+			setpoint_pose_local.header = xy.header;
+			setpoint_pose_local.pose.position.x = xy.point.x;
+			setpoint_pose_local.pose.position.y = xy.point.y;
+		} catch (tf2::TransformException& ex) {
+			// can't transform xy, use last known
+			ROS_WARN_THROTTLE(10, "can't transform: %s", ex.what());
+		}
+		// transform altitude
+		try {
+			setpoint_pose_local.pose.position.z = tf_buffer.transform(setpoint_altitude, local_frame, ros::Duration(0.05)).point.z;
+		} catch (tf2::TransformException& ex) {
+			// can't transform altitude, use last known
+			ROS_WARN_THROTTLE(10, "can't transform: %s", ex.what());
+		}
 	}
 
-		// transform velocity
+	// transform yaw
-		if (setpoint_type == VELOCITY) {
+	if (setpoint_yaw_type == YAW) {
-			setpoint_velocity.header.stamp = stamp;
+		try {
-			tf_buffer.transform(setpoint_velocity, setpoint_velocity_transformed, local_frame, ros::Duration(0.05));
+			QuaternionStamped q;
-		}
+			q.header.stamp = stamp;
-
+			q.header.frame_id = yaw_frame_id;
-	} catch (const tf2::TransformException& e) {
+			q.quaternion = tf::createQuaternionMsgFromYaw(setpoint_yaw);
-		ROS_WARN_THROTTLE(10, "can't transform");
+			yaw_local = tf2::getYaw(tf_buffer.transform(q, local_frame, ros::Duration(0.05)).quaternion);
+		} catch (tf2::TransformException& ex) {
+			// can't transform yaw, use last known
+			ROS_WARN_THROTTLE(10, "can't transform: %s", ex.what());
+		}
 	}
 
+	// compute navigate setpoint
 	if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL) {
-		position_msg.pose.orientation = setpoint_position_transformed.pose.orientation; // copy yaw
 		getNavigateSetpoint(stamp, nav_speed, position_msg.pose.position);
 
 		if (setpoint_yaw_type == TOWARDS) {
-			double yaw_towards = atan2(position_msg.pose.position.y - nav_start.pose.position.y,
+			yaw_local = atan2(position_msg.pose.position.y - nav_start.pose.position.y,
 			                  position_msg.pose.position.x - nav_start.pose.position.x);
-			position_msg.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, yaw_towards);
 		}
+
+		position_msg.pose.orientation = tf::createQuaternionMsgFromYaw(yaw_local);
 	}
 
 	if (setpoint_type == POSITION) {
-		position_msg = setpoint_position_transformed;
+		position_msg = setpoint_pose_local;
+		position_msg.pose.orientation = tf::createQuaternionMsgFromYaw(yaw_local);
 	}
 
 	if (setpoint_type == POSITION || setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL) {
@@ -434,13 +538,17 @@ void publish(const ros::Time stamp)
 			                             PositionTarget::IGNORE_AFY +
 			                             PositionTarget::IGNORE_AFZ +
 			                             PositionTarget::IGNORE_YAW;
-			position_raw_msg.yaw_rate = setpoint_yaw_rate;
+			position_raw_msg.yaw_rate = setpoint_rates.z;
 			position_raw_msg.position = position_msg.pose.position;
 			position_raw_pub.publish(position_raw_msg);
 		}
 
 		// publish setpoint frame
 		if (!setpoint.child_frame_id.empty()) {
+			if (setpoint.header.stamp >= position_msg.header.stamp) {
+				return; // avoid TF_REPEATED_DATA warnings
+			}
+
 			setpoint.transform.translation.x = position_msg.pose.position.x;
 			setpoint.transform.translation.y = position_msg.pose.position.y;
 			setpoint.transform.translation.z = position_msg.pose.position.z;
@@ -449,9 +557,22 @@ void publish(const ros::Time stamp)
 			setpoint.header.stamp = position_msg.header.stamp;
 			transform_broadcaster->sendTransform(setpoint);
 		}
+
+		// publish dynamic target frame
+		publishTarget(stamp);
 	}
 
 	if (setpoint_type == VELOCITY) {
+		// transform velocity to local frame
+		setpoint_velocity.header.stamp = stamp;
+		try {
+			setpoint_velocity_local = tf_buffer.transform(setpoint_velocity, local_frame, ros::Duration(0.05));
+		} catch (tf2::TransformException& ex) {
+			// can't transform velocity, use last known
+			ROS_WARN_THROTTLE(10, "can't transform: %s", ex.what());
+		}
+
+		// publish velocity
 		position_raw_msg.type_mask = PositionTarget::IGNORE_PX +
 		                             PositionTarget::IGNORE_PY +
 		                             PositionTarget::IGNORE_PZ +
@@ -459,14 +580,22 @@ void publish(const ros::Time stamp)
 		                             PositionTarget::IGNORE_AFY +
 		                             PositionTarget::IGNORE_AFZ;
 		position_raw_msg.type_mask += setpoint_yaw_type == YAW ? PositionTarget::IGNORE_YAW_RATE : PositionTarget::IGNORE_YAW;
-		position_raw_msg.velocity = setpoint_velocity_transformed.vector;
+		position_raw_msg.velocity = setpoint_velocity_local.vector;
-		position_raw_msg.yaw = tf2::getYaw(setpoint_position_transformed.pose.orientation);
+		position_raw_msg.yaw = yaw_local;
-		position_raw_msg.yaw_rate = setpoint_yaw_rate;
+		position_raw_msg.yaw_rate = setpoint_rates.z;
 		position_raw_pub.publish(position_raw_msg);
 	}
 
 	if (setpoint_type == ATTITUDE) {
-		attitude_pub.publish(setpoint_position_transformed);
+		PoseStamped msg;
+		msg.header.stamp = stamp;
+		msg.header.frame_id = local_frame;
+		msg.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(setpoint_roll, setpoint_pitch, yaw_local);
+		attitude_pub.publish(msg);
+
+		Thrust thrust_msg;
+		thrust_msg.header.stamp = stamp;
+		thrust_msg.thrust = setpoint_thrust;
 		thrust_pub.publish(thrust_msg);
 	}
 
@@ -475,11 +604,12 @@ void publish(const ros::Time stamp)
 		// thrust_pub.publish(thrust_msg);
 		// mavros rates topics waits for rates in local frame
 		// use rates in body frame for simplicity
+		AttitudeTarget att_raw_msg;
 		att_raw_msg.header.stamp = stamp;
 		att_raw_msg.header.frame_id = fcu_frame;
 		att_raw_msg.type_mask = AttitudeTarget::IGNORE_ATTITUDE;
-		att_raw_msg.body_rate = rates_msg.twist.angular;
+		att_raw_msg.body_rate = setpoint_rates;
-		att_raw_msg.thrust = thrust_msg.thrust;
+		att_raw_msg.thrust = setpoint_thrust;
 		attitude_raw_pub.publish(att_raw_msg);
 	}
 }
@@ -519,10 +649,59 @@ inline void checkState()
 		throw std::runtime_error("No connection to FCU, https://clover.coex.tech/connection");
 }
 
+void publishState()
+{
+	clover::State msg;
+	msg.mode = setpoint_type;
+	msg.yaw_mode = setpoint_yaw_type;
+
+	if (setpoint_position.header.frame_id.empty()) {
+		msg.x = NAN;
+		msg.y = NAN;
+		msg.z = NAN;
+	} else {
+		msg.x = setpoint_position.point.x;
+		msg.y = setpoint_position.point.y;
+		msg.z = setpoint_altitude.point.z;
+	}
+
+	msg.speed = nav_speed;
+	msg.lat = setpoint_lat;
+	msg.lon = setpoint_lon;
+	msg.vx = setpoint_velocity.vector.x;
+	msg.vy = setpoint_velocity.vector.y;
+	msg.vz = setpoint_velocity.vector.z;
+	msg.roll = setpoint_roll;
+	msg.pitch = setpoint_pitch;
+	msg.yaw = !yaw_frame_id.empty() ? setpoint_yaw : NAN;
+
+	msg.roll_rate = setpoint_rates.x;
+	msg.pitch_rate = setpoint_rates.y;
+	msg.yaw_rate = setpoint_rates.z;
+	msg.thrust = setpoint_thrust;
+
+	if (setpoint_type == VELOCITY) {
+		msg.xy_frame_id = setpoint_velocity.header.frame_id;
+		msg.z_frame_id = setpoint_velocity.header.frame_id;
+	} else {
+		msg.xy_frame_id = setpoint_position.header.frame_id;
+		msg.z_frame_id = setpoint_altitude.header.frame_id;
+	}
+	msg.yaw_frame_id = yaw_frame_id;
+
+	state_pub.publish(msg);
+}
+
+inline float safe(float value) {
+	return std::isfinite(value) ? value : 0;
+}
+
 #define ENSURE_FINITE(var) { if (!std::isfinite(var)) throw std::runtime_error(#var " argument cannot be NaN or Inf"); }
 
+#define ENSURE_NON_INF(var) { if (std::isinf(var)) throw std::runtime_error(#var " argument cannot be 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, // editorconfig-checker-disable-line
+           float roll, float pitch, float yaw, float roll_rate, float pitch_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
 {
@@ -549,69 +728,40 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl
 		auto search = reference_frames.find(frame_id);
 		const string& reference_frame = search == reference_frames.end() ? frame_id : search->second;
 
-		// Serve "partial" commands
+		ENSURE_NON_INF(x);
+		ENSURE_NON_INF(y);
+		ENSURE_NON_INF(z);
+		ENSURE_NON_INF(speed); // TODO: allow inf
+		ENSURE_NON_INF(vx);
+		ENSURE_NON_INF(vy);
+		ENSURE_NON_INF(vz);
+		ENSURE_NON_INF(roll);
+		ENSURE_NON_INF(pitch);
+		ENSURE_NON_INF(roll_rate);
+		ENSURE_NON_INF(pitch_rate);
+		ENSURE_NON_INF(yaw_rate);
+		ENSURE_NON_INF(thrust);
 
-		if (!auto_arm && std::isfinite(yaw) &&
+		if (sp_type == NAVIGATE_GLOBAL) {
-		    isnan(x) && isnan(y) && isnan(z) && isnan(vx) && isnan(vy) && isnan(vz) &&
-		    isnan(pitch) && isnan(roll) && isnan(thrust) &&
-		    isnan(lat) && isnan(lon)) {
-			// change only the yaw
-			if (setpoint_type == POSITION || setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == VELOCITY) {
-				if (!waitTransform(setpoint_position.header.frame_id, frame_id, stamp, transform_timeout))
-					throw std::runtime_error("Can't transform from " + frame_id + " to " + setpoint_position.header.frame_id);
-
-				message = "Changing yaw only";
-
-				QuaternionStamped q;
-				q.header.frame_id = frame_id;
-				q.header.stamp = stamp;
-				q.quaternion = tf::createQuaternionMsgFromYaw(yaw); // TODO: pitch=0, roll=0 is not totally correct
-				setpoint_position.pose.orientation = tf_buffer.transform(q, setpoint_position.header.frame_id).quaternion;
-				setpoint_yaw_type = YAW;
-				goto publish_setpoint;
-			} else {
-				throw std::runtime_error("Setting yaw is possible only when position or velocity setpoints active");
-			}
-		}
-
-		if (!auto_arm && std::isfinite(yaw_rate) &&
-		    isnan(x) && isnan(y) && isnan(z) && isnan(vx) && isnan(vy) && isnan(vz) &&
-		    isnan(pitch) && isnan(roll) && isnan(yaw) && isnan(thrust) &&
-		    isnan(lat) && isnan(lon)) {
-			// change only the yaw rate
-			if (setpoint_type == POSITION || setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == VELOCITY) {
-				message = "Changing yaw rate only";
-
-				setpoint_yaw_type = YAW_RATE;
-				setpoint_yaw_rate = yaw_rate;
-				goto publish_setpoint;
-			} else {
-				throw std::runtime_error("Setting yaw rate is possible only when position or velocity setpoints active");
-			}
-		}
-
-		// Serve normal commands
-
-		if (sp_type == NAVIGATE || sp_type == POSITION) {
-			ENSURE_FINITE(x);
-			ENSURE_FINITE(y);
-			ENSURE_FINITE(z);
-		} else if (sp_type == NAVIGATE_GLOBAL) {
 			ENSURE_FINITE(lat);
 			ENSURE_FINITE(lon);
-			ENSURE_FINITE(z);
+		}
-		} else if (sp_type == VELOCITY) {
+
-			ENSURE_FINITE(vx);
+		if (isfinite(x) != isfinite(y)) {
-			ENSURE_FINITE(vy);
+			throw std::runtime_error("x and y can be set only together");
-			ENSURE_FINITE(vz);
+		}
-		} else if (sp_type == ATTITUDE) {
+
-			ENSURE_FINITE(pitch);
+		if (isfinite(yaw_rate)) {
-			ENSURE_FINITE(roll);
+			if (sp_type > RATES && setpoint_type == ATTITUDE) {
-			ENSURE_FINITE(thrust);
+				throw std::runtime_error("Yaw rate cannot be set in attitude mode.");
-		} else if (sp_type == RATES) {
+			}
-			ENSURE_FINITE(pitch_rate);
+		}
-			ENSURE_FINITE(roll_rate);
+
-			ENSURE_FINITE(thrust);
+		// set_altitude
+		if (sp_type == _ALTITUDE) {
+			if (setpoint_type == VELOCITY || setpoint_type == ATTITUDE || setpoint_type == RATES) {
+				throw std::runtime_error("Altitude cannot be set in velocity, attitude or rates mode.");
+			}
 		}
 
 		if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL) {
@@ -625,20 +775,13 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl
 				speed = default_speed;
 		}
 
-		if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == POSITION || sp_type == VELOCITY) {
-			if (yaw_rate != 0 && !std::isnan(yaw))
-				throw std::runtime_error("Yaw value should be NaN for setting yaw rate");
-
-			if (std::isnan(yaw_rate) && std::isnan(yaw))
-				throw std::runtime_error("Both yaw and yaw_rate cannot be NaN");
-		}
-
 		if (sp_type == NAVIGATE_GLOBAL) {
 			if (TIMEOUT(global_position, global_position_timeout))
 				throw std::runtime_error("No global position");
 		}
 
-		if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == POSITION || sp_type == VELOCITY || sp_type == ATTITUDE) {
+		// if any value need to be transformed to reference frame
+		if (isfinite(x) || isfinite(y) || isfinite(z) || isfinite(vx) || isfinite(vy) || isfinite(vz) || isfinite(yaw)) {
 			// make sure transform from frame_id to reference frame available
 			if (!waitTransform(reference_frame, frame_id, stamp, transform_timeout))
 				throw std::runtime_error("Can't transform from " + frame_id + " to " + reference_frame);
@@ -655,15 +798,27 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl
 			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;
+			setpoint_lat = lat;
+			setpoint_lon = lon;
 		}
 
 		// Everything fine - switch setpoint type
+		if (sp_type <= RATES) {
 			setpoint_type = sp_type;
+		}
 
-		if (sp_type != NAVIGATE && sp_type != NAVIGATE_GLOBAL) {
+		if (setpoint_type != NAVIGATE && setpoint_type != NAVIGATE_GLOBAL) {
 			nav_from_sp_flag = false;
 		}
 
+		bool to_auto_arm = auto_arm && (state.mode != "OFFBOARD" || !state.armed);
+		if (to_auto_arm || setpoint_type == VELOCITY || setpoint_type == ATTITUDE || setpoint_type == RATES) {
+			// invalidate position setpoint
+			setpoint_position.header.frame_id = "";
+			setpoint_altitude.header.frame_id = "";
+			yaw_frame_id = "";
+		}
+
 		if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL) {
 			// starting point
 			if (nav_from_sp && nav_from_sp_flag) {
@@ -672,84 +827,139 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl
 			} else {
 				nav_start = local_position;
 			}
+
+			if (!isnan(speed)) {
 				nav_speed = speed;
+			}
+
 			nav_from_sp_flag = true;
 		}
 
-		// if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == POSITION || sp_type == VELOCITY) {
+		// handle position
-		// 	if (std::isnan(yaw) && yaw_rate == 0) {
+		if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == POSITION) {
-		// 		// keep yaw unchanged
-		//		// TODO: this is incorrect, because we need yaw in desired frame
-		// 		yaw = tf2::getYaw(local_position.pose.orientation);
-		// 	}
-		// }
 
-		if (sp_type == POSITION || sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == VELOCITY || sp_type == ATTITUDE) {
+			PointStamped desired;
-			// destination point and/or yaw
+			desired.header.frame_id = frame_id;
-			PoseStamped ps;
+			desired.header.stamp = stamp;
-			ps.header.frame_id = frame_id;
+			desired.point.x = safe(x);
-			ps.header.stamp = stamp;
+			desired.point.y = safe(y);
-			ps.pose.position.x = x;
+			desired.point.z = safe(z);
-			ps.pose.position.y = y;
-			ps.pose.position.z = z;
-			ps.pose.orientation.w = 1.0; // Ensure quaternion is always valid
 
-			if (std::isnan(yaw)) {
+			// transform to reference frame
-				setpoint_yaw_type = YAW_RATE;
+			desired = tf_buffer.transform(desired, reference_frame);
-				setpoint_yaw_rate = yaw_rate;
+
-			} else if (std::isinf(yaw) && yaw > 0) {
+			// set horizontal position
+			if (isfinite(x) && isfinite(y)) {
+				setpoint_position = desired;
+			} else if (setpoint_position.header.frame_id.empty()) {
+				 // TODO: use transform for current stamp
+				setpoint_position.header = local_position.header;
+				setpoint_position.point = local_position.pose.position;
+			}
+
+			// set altitude
+			if (isfinite(z)) {
+				setpoint_altitude = desired;
+			} else if (setpoint_altitude.header.frame_id.empty()) {
+				setpoint_altitude.header = local_position.header;
+				setpoint_altitude.point = local_position.pose.position;
+			}
+		}
+
+		// handle velocity
+		if (sp_type == VELOCITY) {
+			// TODO: allow setting different modes by altitude and xy
+			Vector3Stamped desired;
+			desired.header.frame_id = frame_id;
+			desired.header.stamp = stamp;
+			desired.vector.x = safe(vx);
+			desired.vector.y = safe(vy);
+			desired.vector.z = safe(vz);
+
+			// transform to reference frame
+			desired = tf_buffer.transform(desired, reference_frame);
+			setpoint_velocity.header = desired.header;
+
+			// set horizontal velocity
+			if (isfinite(vx) && isfinite(vy)) {
+				setpoint_velocity.vector.x = desired.vector.x;
+				setpoint_velocity.vector.y = desired.vector.y;
+			}
+
+			// set vertical velocity
+			if (isfinite(vz)) {
+				setpoint_velocity.vector.z = desired.vector.z;
+			}
+		}
+
+		// handle yaw
+		if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == POSITION || sp_type == VELOCITY || sp_type == ATTITUDE || sp_type == _YAW) {
+			if (isfinite(yaw)) {
+				setpoint_yaw_type = YAW;
+				QuaternionStamped desired;
+				desired.header.frame_id = frame_id;
+				desired.header.stamp = stamp;
+				desired.quaternion = tf::createQuaternionMsgFromYaw(yaw);
+
+				// transform to reference frame
+				desired = tf_buffer.transform(desired, reference_frame);
+				setpoint_yaw = tf2::getYaw(desired.quaternion);
+				yaw_frame_id = reference_frame;
+
+			} else if (isinf(yaw) && yaw > 0) {
 				// yaw towards
 				setpoint_yaw_type = TOWARDS;
-				yaw = 0;
+
-				setpoint_yaw_rate = 0;
+			} else if (yaw_frame_id.empty() || sp_type == _YAW) {
-			} else {
+				// yaw is nan and not set previously OR set_yaw(yaw=nan) was called
 				setpoint_yaw_type = YAW;
-				setpoint_yaw_rate = 0;
+				setpoint_yaw = tf2::getYaw(local_position.pose.orientation); // set yaw to current yaw
-				ps.pose.orientation = tf::createQuaternionMsgFromYaw(yaw);
+				yaw_frame_id = local_position.header.frame_id;
+			}
 		}
 
-			tf_buffer.transform(ps, setpoint_position, reference_frame);
+		// handle roll
+		if (isfinite(roll)) {
+			setpoint_roll = roll;
 		}
 
-		if (sp_type == VELOCITY) {
+		// handle pitch
-			Vector3Stamped vel;
+		if (isfinite(pitch)) {
-			vel.header.frame_id = frame_id;
+			setpoint_pitch = pitch;
-			vel.header.stamp = stamp;
-			vel.vector.x = vx;
-			vel.vector.y = vy;
-			vel.vector.z = vz;
-			tf_buffer.transform(vel, setpoint_velocity, reference_frame);
 		}
 
-		if (sp_type == ATTITUDE || sp_type == RATES) {
+		// handle yaw rate
-			thrust_msg.thrust = thrust;
+		if (isfinite(yaw_rate)) {
+			setpoint_yaw_type = YAW_RATE;
+			setpoint_rates.z = yaw_rate;
 		}
 
-		if (sp_type == RATES) {
+		// handle pitch rate
-			rates_msg.twist.angular.x = roll_rate;
+		if (isfinite(roll_rate)) {
-			rates_msg.twist.angular.y = pitch_rate;
+			setpoint_rates.x = roll_rate;
-			rates_msg.twist.angular.z = yaw_rate;
+		}
+
+		// handle roll rate
+		if (isfinite(pitch_rate)) {
+			setpoint_rates.y = pitch_rate;
+		}
+
+		// handle thrust
+		if (isfinite(thrust)) {
+			setpoint_thrust = thrust;
 		}
 
 		wait_armed = auto_arm;
 
-publish_setpoint:
 		publish(stamp); // calculate initial transformed messages first
 		setpoint_timer.start();
 
-		// publish target frame
-		if (!target.child_frame_id.empty()) {
 		if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == POSITION) {
-				target.header.frame_id = setpoint_position.header.frame_id;
+			publishTarget(stamp, true);
-				target.header.stamp = stamp;
-				target.transform.translation.x = setpoint_position.pose.position.x;
-				target.transform.translation.y = setpoint_position.pose.position.y;
-				target.transform.translation.z = setpoint_position.pose.position.z;
-				target.transform.rotation = setpoint_position.pose.orientation;
-				static_transform_broadcaster->sendTransform(target);
-			}
 		}
 
+		publishState();
+
 		if (auto_arm) {
 			offboardAndArm();
 			wait_armed = false;
@@ -774,27 +984,39 @@ publish_setpoint:
 }
 
 bool navigate(Navigate::Request& req, Navigate::Response& res) {
-	return serve(NAVIGATE, req.x, req.y, req.z, NAN, NAN, NAN, NAN, NAN, req.yaw, NAN, NAN, req.yaw_rate, NAN, NAN, NAN, req.speed, req.frame_id, req.auto_arm, res.success, res.message);
+	return serve(NAVIGATE, req.x, req.y, req.z, NAN, NAN, NAN, NAN, NAN, req.yaw, NAN, NAN, NAN, NAN, NAN, NAN, req.speed, req.frame_id, req.auto_arm, res.success, res.message);
 }
 
 bool navigateGlobal(NavigateGlobal::Request& req, NavigateGlobal::Response& res) {
-	return serve(NAVIGATE_GLOBAL, NAN, NAN, req.z, NAN, NAN, NAN, NAN, NAN, req.yaw, NAN, NAN, req.yaw_rate, req.lat, req.lon, NAN, req.speed, req.frame_id, req.auto_arm, res.success, res.message);
+	return serve(NAVIGATE_GLOBAL, NAN, NAN, req.z, NAN, NAN, NAN, NAN, NAN, req.yaw, NAN, NAN, NAN, req.lat, req.lon, NAN, req.speed, req.frame_id, req.auto_arm, res.success, res.message);
+}
+
+bool setAltitude(SetAltitude::Request& req, SetAltitude::Response& res) {
+	return serve(_ALTITUDE, NAN, NAN, req.z, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, req.frame_id, false, res.success, res.message);
+}
+
+bool setYaw(SetYaw::Request& req, SetYaw::Response& res) {
+	return serve(_YAW, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, req.yaw, NAN, NAN, NAN, NAN, NAN, NAN, NAN, req.frame_id, false, res.success, res.message);
+}
+
+bool setYawRate(SetYawRate::Request& req, SetYawRate::Response& res) {
+	return serve(_YAW_RATE, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, req.yaw_rate, NAN, NAN, NAN, NAN, "", false, res.success, res.message);
 }
 
 bool setPosition(SetPosition::Request& req, SetPosition::Response& res) {
-	return serve(POSITION, req.x, req.y, req.z, NAN, NAN, NAN, NAN, NAN, req.yaw, NAN, NAN, req.yaw_rate, NAN, NAN, NAN, NAN, req.frame_id, req.auto_arm, res.success, res.message);
+	return serve(POSITION, req.x, req.y, req.z, NAN, NAN, NAN, NAN, NAN, req.yaw, NAN, NAN, NAN, NAN, NAN, NAN, NAN, req.frame_id, req.auto_arm, res.success, res.message);
 }
 
 bool setVelocity(SetVelocity::Request& req, SetVelocity::Response& res) {
-	return serve(VELOCITY, NAN, NAN, NAN, req.vx, req.vy, req.vz, NAN, NAN, req.yaw, NAN, NAN, req.yaw_rate, NAN, NAN, NAN, NAN, req.frame_id, req.auto_arm, res.success, res.message);
+	return serve(VELOCITY, NAN, NAN, NAN, req.vx, req.vy, req.vz, NAN, NAN, req.yaw, NAN, NAN, NAN, NAN, NAN, NAN, NAN, req.frame_id, req.auto_arm, res.success, res.message);
 }
 
 bool setAttitude(SetAttitude::Request& req, SetAttitude::Response& res) {
-	return serve(ATTITUDE, NAN, NAN, NAN, NAN, NAN, NAN, req.pitch, req.roll, req.yaw, NAN, NAN, NAN, NAN, NAN, req.thrust, NAN, req.frame_id, req.auto_arm, res.success, res.message);
+	return serve(ATTITUDE, NAN, NAN, NAN, NAN, NAN, NAN, req.roll, req.pitch, req.yaw, NAN, NAN, NAN, NAN, NAN, req.thrust, NAN, req.frame_id, req.auto_arm, res.success, res.message);
 }
 
 bool setRates(SetRates::Request& req, SetRates::Response& res) {
-	return serve(RATES, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, req.pitch_rate, req.roll_rate, req.yaw_rate, NAN, NAN, req.thrust, NAN, "", req.auto_arm, res.success, res.message);
+	return serve(RATES, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, req.roll_rate, req.pitch_rate, req.yaw_rate, NAN, NAN, req.thrust, NAN, "", req.auto_arm, res.success, res.message);
 }
 
 bool land(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)
@@ -826,9 +1048,7 @@ bool land(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)
 		auto start = ros::Time::now();
 		while (ros::ok()) {
 			if (state.mode == "AUTO.LAND") {
-				res.success = true;
+				break;
-				busy = false;
-				return true;
 			}
 			if (ros::Time::now() - start > land_timeout)
 				throw std::runtime_error("Land request timed out");
@@ -837,12 +1057,37 @@ bool land(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)
 			r.sleep();
 		}
 
+		// stop setpoints and invalidate position setpoint
+		setpoint_timer.stop();
+		setpoint_type = NONE;
+		setpoint_position.header.frame_id = "";
+		setpoint_altitude.header.frame_id = "";
+		yaw_frame_id = "";
+		publishState();
+
+		res.success = true;
+		busy = false;
+		return true;
+
 	} catch (const std::exception& e) {
 		res.message = e.what();
 		ROS_INFO("%s", e.what());
 		busy = false;
 		return true;
 	}
+	return false;
+}
+
+bool release(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)
+{
+	setpoint_timer.stop();
+	setpoint_type = NONE;
+	setpoint_position.header.frame_id = "";
+	setpoint_altitude.header.frame_id = "";
+	yaw_frame_id = "";
+	publishState();
+	res.success = true;
+	return true;
 }
 
 int main(int argc, char **argv)
@@ -855,8 +1100,9 @@ int main(int argc, char **argv)
 	static_transform_broadcaster = std::make_shared<tf2_ros::StaticTransformBroadcaster>();
 
 	// Params
-	nh.param<string>("mavros/local_position/tf/frame_id", local_frame, "map");
+	nh_priv.param("mavros", mavros, string("mavros")); // for case of using multiple connections
-	nh.param<string>("mavros/local_position/tf/child_frame_id", fcu_frame, "base_link");
+	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("setpoint", setpoint.child_frame_id, string("setpoint"));
 	nh_priv.param("auto_release", auto_release, true);
@@ -865,8 +1111,17 @@ int main(int argc, char **argv)
 	nh_priv.param("check_kill_switch", check_kill_switch, true);
 	nh_priv.param("default_speed", default_speed, 0.5f);
 	nh_priv.param<string>("body_frame", body.child_frame_id, "body");
+	nh_priv.param<string>("terrain_frame", terrain.child_frame_id, "terrain");
+	nh_priv.param<string>("terrain_frame_mode", terrain_frame_mode, "altitude");
 	nh_priv.getParam("reference_frames", reference_frames);
 
+	// Default reference frames
+	std::map<string, string> default_reference_frames;
+	default_reference_frames[body.child_frame_id] = local_frame;
+	default_reference_frames[fcu_frame] = local_frame;
+	if (!target.child_frame_id.empty()) default_reference_frames[target.child_frame_id] = local_frame;
+	reference_frames.insert(default_reference_frames.begin(), default_reference_frames.end()); // merge defaults
+
 	state_timeout = ros::Duration(nh_priv.param("state_timeout", 3.0));
 	local_position_timeout = ros::Duration(nh_priv.param("local_position_timeout", 2.0));
 	velocity_timeout = ros::Duration(nh_priv.param("velocity_timeout", 2.0));
@@ -881,35 +1136,56 @@ int main(int argc, char **argv)
 	arming_timeout = ros::Duration(nh_priv.param("arming_timeout", 4.0));
 
 	// Service clients
-	arming = nh.serviceClient<mavros_msgs::CommandBool>("mavros/cmd/arming");
+	arming = nh.serviceClient<mavros_msgs::CommandBool>(mavros + "/cmd/arming");
-	set_mode = nh.serviceClient<mavros_msgs::SetMode>("mavros/set_mode");
+	set_mode = nh.serviceClient<mavros_msgs::SetMode>(mavros + "/set_mode");
 
 	// Telemetry subscribers
-	auto state_sub = nh.subscribe("mavros/state", 1, &handleState);
+	auto state_sub = nh.subscribe(mavros + "/state", 1, &handleState);
-	auto velocity_sub = nh.subscribe("mavros/local_position/velocity_body", 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 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 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 statustext_sub = nh.subscribe(mavros + "/statustext/recv", 1, &handleMessage<mavros_msgs::StatusText, statustext>);
-	auto manual_control_sub = nh.subscribe("mavros/manual_control/control", 1, &handleMessage<mavros_msgs::ManualControl, manual_control>);
+	auto manual_control_sub = nh.subscribe(mavros + "/manual_control/control", 1, &handleMessage<mavros_msgs::ManualControl, manual_control>);
-	auto local_position_sub = nh.subscribe("mavros/local_position/pose", 1, &handleLocalPosition);
+	auto local_position_sub = nh.subscribe(mavros + "/local_position/pose", 1, &handleLocalPosition);
+
+	ros::Subscriber altitude_sub;
+	if (!body.child_frame_id.empty() && !terrain.child_frame_id.empty()) {
+		terrain.header.frame_id = local_frame;
+		if (terrain_frame_mode == "altitude") {
+			altitude_sub = nh.subscribe(mavros + "/altitude", 1, &handleAltitude);
+		} else if (terrain_frame_mode == "range") {
+			string range_topic = nh_priv.param("range_topic", string("rangefinder/range"));
+			altitude_sub = nh.subscribe(range_topic, 1, &handleRange);
+		} else {
+			ROS_FATAL("Unknown terrain_frame_mode: %s, valid values: altitude, range", terrain_frame_mode.c_str());
+			ros::shutdown();
+		}
+	}
 
 	// Setpoint publishers
-	position_pub = nh.advertise<PoseStamped>("mavros/setpoint_position/local", 1);
+	position_pub = nh.advertise<PoseStamped>(mavros + "/setpoint_position/local", 1);
-	position_raw_pub = nh.advertise<PositionTarget>("mavros/setpoint_raw/local", 1);
+	position_raw_pub = nh.advertise<PositionTarget>(mavros + "/setpoint_raw/local", 1);
-	attitude_pub = nh.advertise<PoseStamped>("mavros/setpoint_attitude/attitude", 1);
+	attitude_pub = nh.advertise<PoseStamped>(mavros + "/setpoint_attitude/attitude", 1);
-	attitude_raw_pub = nh.advertise<AttitudeTarget>("mavros/setpoint_raw/attitude", 1);
+	attitude_raw_pub = nh.advertise<AttitudeTarget>(mavros + "/setpoint_raw/attitude", 1);
-	rates_pub = nh.advertise<TwistStamped>("mavros/setpoint_attitude/cmd_vel", 1);
+	rates_pub = nh.advertise<TwistStamped>(mavros + "/setpoint_attitude/cmd_vel", 1);
-	thrust_pub = nh.advertise<Thrust>("mavros/setpoint_attitude/thrust", 1);
+	thrust_pub = nh.advertise<Thrust>(mavros + "/setpoint_attitude/thrust", 1);
+
+	// State publisher
+	state_pub = nh_priv.advertise<clover::State>("state", 1, true);
 
 	 // Service servers
 	auto gt_serv = nh.advertiseService("get_telemetry", &getTelemetry);
 	auto na_serv = nh.advertiseService("navigate", &navigate);
 	auto ng_serv = nh.advertiseService("navigate_global", &navigateGlobal);
+	auto sl_serv = nh.advertiseService("set_altitude", &setAltitude);
+	auto ya_serv = nh.advertiseService("set_yaw", &setYaw);
+	auto yr_serv = nh.advertiseService("set_yaw_rate", &setYawRate);
 	auto sp_serv = nh.advertiseService("set_position", &setPosition);
 	auto sv_serv = nh.advertiseService("set_velocity", &setVelocity);
 	auto sa_serv = nh.advertiseService("set_attitude", &setAttitude);
 	auto sr_serv = nh.advertiseService("set_rates", &setRates);
 	auto ld_serv = nh.advertiseService("land", &land);
+	auto rl_serv = nh_priv.advertiseService("release", &release);
 
 	// Setpoint timer
 	setpoint_timer = nh.createTimer(ros::Duration(1 / nh_priv.param("setpoint_rate", 30.0)), &publishSetpoint, false, false);
@@ -917,7 +1193,7 @@ int main(int argc, char **argv)
 	position_msg.header.frame_id = local_frame;
 	position_raw_msg.header.frame_id = local_frame;
 	position_raw_msg.coordinate_frame = PositionTarget::FRAME_LOCAL_NED;
-	rates_msg.header.frame_id = fcu_frame;
+	//rates_msg.header.frame_id = fcu_frame;
 
 	ROS_INFO("ready");
 	ros::spin();

clover/src/vpe_publisher.cpp

@@ -11,12 +11,14 @@
 
 #include <string>
 #include <ros/ros.h>
+#include <tf/transform_datatypes.h>
 #include <tf2/transform_datatypes.h>
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
 #include <tf2_ros/static_transform_broadcaster.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 #include <geometry_msgs/TransformStamped.h>
+#include <geometry_msgs/Quaternion.h>
 #include <geometry_msgs/PoseStamped.h>
 #include <geometry_msgs/PoseWithCovarianceStamped.h>
 #include <std_srvs/Trigger.h>
@@ -25,7 +27,7 @@
 using std::string;
 using namespace geometry_msgs;
 
-bool reset_flag = false;
+bool reset_flag = true; // offset should be reset on the start
 string local_frame_id, frame_id, child_frame_id, offset_frame_id;
 tf2_ros::Buffer tf_buffer;
 ros::Publisher vpe_pub;
@@ -33,14 +35,14 @@ ros::Subscriber local_position_sub;
 ros::Timer zero_timer;
 PoseStamped vpe, pose;
 ros::Time got_local_pos(0);
-ros::Duration publish_zero_timout, publish_zero_duration, offset_timeout;
+ros::Duration publish_zero_timeout, publish_zero_duration, offset_timeout;
 TransformStamped offset;
 
 void publishZero(const ros::TimerEvent& e)
 {
-	if (e.current_real - vpe.header.stamp < publish_zero_timout) return; // have vpe
+	if (!vpe.header.stamp.isZero() && e.current_real - vpe.header.stamp < publish_zero_timeout) return; // have vpe
 
-	if (e.current_real - pose.header.stamp < publish_zero_timout) { // have local position
+	if (!pose.header.stamp.isZero() && e.current_real - pose.header.stamp < publish_zero_timeout) { // have local position
 		if (got_local_pos.isZero()) {
 			ROS_INFO("got local position");
 			got_local_pos = e.current_real;
@@ -66,6 +68,13 @@ inline Pose getPose(const PoseStampedConstPtr& pose) { return pose->pose; }
 
 inline Pose getPose(const PoseWithCovarianceStampedConstPtr& pose) { return pose->pose.pose; }
 
+inline void keepYaw(Quaternion& quaternion)
+{
+	tf::Quaternion q;
+	q.setRPY(0, 0, tf::getYaw(quaternion));
+	tf::quaternionTFToMsg(q, quaternion);
+}
+
 template <typename T>
 void callback(const T& msg)
 {
@@ -88,10 +97,29 @@ void callback(const T& msg)
 		if (!offset_frame_id.empty()) {
 			if (reset_flag || msg->header.stamp - vpe.header.stamp > offset_timeout) {
 				// calculate the offset
+				if (!frame_id.empty()) {
+					// calculate from TF
 					offset = tf_buffer.lookupTransform(local_frame_id, frame_id,
 					                                   msg->header.stamp, ros::Duration(0.02));
 					// offset.header.frame_id = vpe.header.frame_id;
 					offset.child_frame_id = offset_frame_id;
+
+				} else {
+					// calculate transform between pose in vpe frame and pose in local frame
+					TransformStamped local_pose = tf_buffer.lookupTransform(local_frame_id, child_frame_id,
+					                                                        msg->header.stamp, ros::Duration(0.02));
+					keepYaw(local_pose.transform.rotation);
+
+					tf::Transform vpeTransform, poseTransform;
+					tf::poseMsgToTF(vpe.pose, vpeTransform);
+					tf::transformMsgToTF(local_pose.transform, poseTransform);
+					tf::Transform offset_tf = vpeTransform.inverseTimes(poseTransform);
+					tf::transformTFToMsg(offset_tf, offset.transform);
+					offset.header.frame_id = local_frame_id;
+					offset.header.stamp = msg->header.stamp;
+					offset.child_frame_id = offset_frame_id;
+				}
+
 				br.sendTransform(offset);
 				reset_flag = false;
 				ROS_INFO("offset reset");
@@ -122,10 +150,11 @@ int main(int argc, char **argv) {
 
 	tf2_ros::TransformListener tf_listener(tf_buffer);
 
-	nh_priv.param<string>("frame_id", frame_id, "");
+	nh_priv.param<string>("frame_id", frame_id, ""); // name for used visual pose frame
-	nh_priv.param<string>("offset_frame_id", offset_frame_id, "");
+	nh_priv.param<string>("offset_frame_id", offset_frame_id, ""); // name for published offset frame
-	nh_priv.param<string>("mavros/local_position/frame_id", local_frame_id, "map");
+
-	nh_priv.param<string>("mavros/local_position/tf/child_frame_id", child_frame_id, "base_link");
+	nh.param<string>("mavros/local_position/frame_id", local_frame_id, "map");
+	nh.param<string>("mavros/local_position/tf/child_frame_id", child_frame_id, "base_link");
 	offset_timeout = ros::Duration(nh_priv.param("offset_timeout", 3.0));
 
 	if (!frame_id.empty()) {
@@ -141,11 +170,11 @@ int main(int argc, char **argv) {
 	vpe_pub = nh_priv.advertise<PoseStamped>("vpe", 1);
 	//vpe_cov_pub = nh_priv_.advertise<PoseStamped>("pose_cov_pub", 1);
 
-	if (nh_priv.param("publish_zero", false)) {
+	if (nh_priv.param("force_init", false) || nh_priv.param("publish_zero", false)) { // publish_zero is old name
 		// publish zero to initialize the local position
 		zero_timer = nh.createTimer(ros::Duration(0.1), &publishZero);
-		publish_zero_timout = ros::Duration(nh_priv.param("publish_zero_timout", 5.0));
+		publish_zero_timeout = ros::Duration(nh_priv.param("force_init_timeout", 5.0));
-		publish_zero_duration = ros::Duration(nh_priv.param("publish_zero_duration", 5.0));
+		publish_zero_duration = ros::Duration(nh_priv.param("force_init_duration", 5.0));
 		local_position_sub = nh.subscribe("mavros/local_position/pose", 1, &localPositionCallback);
 	}
 

clover/src/www

@@ -0,0 +1,4 @@
+#!/usr/bin/env bash
+
+export ROSWWW_DEFAULT=clover
+rosrun roswww_static update

clover/srv/GetTelemetry.srv

@@ -13,11 +13,11 @@ float32 alt
 float32 vx
 float32 vy
 float32 vz
-float32 pitch
 float32 roll
+float32 pitch
 float32 yaw
-float32 pitch_rate
 float32 roll_rate
+float32 pitch_rate
 float32 yaw_rate
 float32 voltage
 float32 cell_voltage

clover/srv/Navigate.srv

@@ -2,7 +2,6 @@ float32 x
 float32 y
 float32 z
 float32 yaw
-float32 yaw_rate
 float32 speed
 string frame_id
 bool auto_arm

clover/srv/NavigateGlobal.srv

@@ -2,7 +2,6 @@ float64 lat
 float64 lon
 float32 z
 float32 yaw
-float32 yaw_rate
 float32 speed
 string frame_id
 bool auto_arm

clover/srv/SetAltitude.srv

@@ -0,0 +1,5 @@
+float32 z
+string frame_id
+---
+bool success
+string message

clover/srv/SetAttitude.srv

@@ -1,5 +1,5 @@
-float32 pitch
 float32 roll
+float32 pitch
 float32 yaw
 float32 thrust
 string frame_id

clover/srv/SetPosition.srv

@@ -2,7 +2,6 @@ float32 x
 float32 y
 float32 z
 float32 yaw
-float32 yaw_rate
 string frame_id
 bool auto_arm
 ---

clover/srv/SetRates.srv

@@ -1,5 +1,5 @@
-float32 pitch_rate
 float32 roll_rate
+float32 pitch_rate
 float32 yaw_rate
 float32 thrust
 bool auto_arm

clover/srv/SetVelocity.srv

@@ -2,7 +2,6 @@ float32 vx
 float32 vy
 float32 vz
 float32 yaw
-float32 yaw_rate
 string frame_id
 bool auto_arm
 ---

clover/srv/SetYaw.srv

@@ -0,0 +1,5 @@
+float32 yaw
+string frame_id
+---
+bool success
+string message

clover/srv/SetYawRate.srv

@@ -0,0 +1,4 @@
+float32 yaw_rate
+---
+bool success
+string message

clover/test/basic.py

@@ -3,6 +3,7 @@ import rospy
 import pytest
 from mavros_msgs.msg import State
 from clover import srv
+import time
 
 @pytest.fixture()
 def node():
@@ -24,6 +25,7 @@ def test_simple_offboard_services_available():
     rospy.wait_for_service('set_attitude', timeout=5)
     rospy.wait_for_service('set_rates', timeout=5)
     rospy.wait_for_service('land', timeout=5)
+    rospy.wait_for_service('simple_offboard/release', timeout=5)
 
 def test_web_video_server(node):
     try:
@@ -33,3 +35,44 @@ def test_web_video_server(node):
         # Python 3
         import urllib.request as urllib
     urllib.urlopen("http://localhost:8080").read()
+
+def test_blocks(node):
+    rospy.wait_for_service('clover_blocks/run', timeout=5)
+    rospy.wait_for_service('clover_blocks/stop', timeout=5)
+    rospy.wait_for_service('clover_blocks/load', timeout=5)
+    rospy.wait_for_service('clover_blocks/store', timeout=5)
+
+    from std_msgs.msg import String
+    from clover_blocks.srv import Run
+
+    def wait_print():
+        try:
+            wait_print.result = rospy.wait_for_message('clover_blocks/print', String, timeout=5).data
+        except Exception as e:
+            wait_print.result = str(e)
+
+    import threading
+    t = threading.Thread(target=wait_print)
+    t.start()
+    rospy.sleep(0.1)
+
+    run = rospy.ServiceProxy('clover_blocks/run', Run)
+    assert run(code='print("test")').success == True
+
+    t.join()
+    assert wait_print.result == 'test'
+
+def test_long_callback():
+    from clover import long_callback
+    from time import sleep
+
+    # very basic test for long_callback
+    @long_callback
+    def cb(i):
+        cb.counter += i
+    cb.counter = 0
+    cb(2)
+    sleep(0.1)
+    cb(3)
+    sleep(1)
+    assert cb.counter == 5

clover/test/basic.test

@@ -23,10 +23,7 @@
 
     <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="clover" type="simple_offboard" required="true" output="screen">
+    <node name="simple_offboard" pkg="clover" 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"/>
 
@@ -38,6 +35,21 @@
         <rosparam param="notify">startup: { r: 255, g: 255, b: 255 }</rosparam>
     </node>
 
+    <node name="clover_blocks" pkg="clover_blocks" type="clover_blocks" output="screen" required="true"/>
+
+    <node pkg="topic_tools" name="main_camera_throttle" type="throttle" ns="main_camera"
+        args="messages image_raw 5.0 image_raw_throttled" required="true"/>
+
+    <node pkg="nodelet" type="nodelet" name="main_camera_nodelet_manager" args="manager" output="screen" required="true">
+        <param name="num_worker_threads" value="2"/>
+    </node>
+
+    <node pkg="nodelet" type="nodelet" name="rectify" args="load image_proc/rectify main_camera_nodelet_manager" required="true">
+        <remap from="image_mono" to="main_camera/image_raw"/>
+        <remap from="camera_info" to="main_camera/camera_info"/>
+        <remap from="image_rect" to="main_camera/image_rect"/>
+    </node>
+
     <param name="test_module" value="$(find clover)/test/basic.py"/>
     <test test-name="basic_test" pkg="ros_pytest" type="ros_pytest_runner"/>
 </launch>

clover/test/offboard.py

@@ -0,0 +1,437 @@
+import rospy
+import pytest
+from pytest import approx
+import threading
+import mavros_msgs.msg
+from mavros_msgs.srv import SetMode
+from geometry_msgs.msg import PoseStamped
+from clover import srv
+from clover.msg import State
+from std_srvs.srv import Trigger
+from math import nan, inf
+import tf2_ros
+import tf2_geometry_msgs
+
+@pytest.fixture()
+def node():
+    return rospy.init_node('offboard_test', anonymous=True)
+
+@pytest.fixture
+def tf_buffer():
+    buf = tf2_ros.Buffer()
+    tf2_ros.TransformListener(buf)
+    return buf
+
+def get_state():
+    return rospy.wait_for_message('/simple_offboard/state', State, timeout=1)
+
+def get_navigate_target(tf_buffer):
+    target = tf_buffer.lookup_transform('map', 'navigate_target', rospy.get_rostime(), rospy.Duration(1))
+    assert target.child_frame_id == 'navigate_target'
+    return target
+
+def test_offboard(node, tf_buffer):
+    navigate = rospy.ServiceProxy('navigate', srv.Navigate)
+    set_position = rospy.ServiceProxy('set_position', srv.SetPosition)
+    set_altitude = rospy.ServiceProxy('set_altitude', srv.SetAltitude)
+    set_yaw = rospy.ServiceProxy('set_yaw', srv.SetYaw)
+    set_yaw_rate = rospy.ServiceProxy('set_yaw_rate', srv.SetYawRate)
+    set_velocity = rospy.ServiceProxy('set_velocity', srv.SetVelocity)
+    set_attitude = rospy.ServiceProxy('set_attitude', srv.SetAttitude)
+    set_rates = rospy.ServiceProxy('set_rates', srv.SetRates)
+    get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry)
+    land = rospy.ServiceProxy('land', Trigger)
+
+    res = navigate()
+    assert res.success == False
+    assert res.message.startswith('State timeout')
+
+    telem = get_telemetry()
+    assert telem.connected == False
+
+    # mocked state publisher
+    state_pub = rospy.Publisher('/mavros/state', mavros_msgs.msg.State, latch=True, queue_size=1)
+    state_msg = mavros_msgs.msg.State(mode='OFFBOARD', armed=True)
+
+    def publish_state():
+        r = rospy.Rate(2)
+        while not rospy.is_shutdown():
+            state_msg.header.stamp = rospy.Time.now()
+            state_pub.publish(state_msg)
+            r.sleep()
+
+    # start publishing state
+    threading.Thread(target=publish_state, daemon=True).start()
+    rospy.sleep(0.5)
+
+    # set_mode service mock
+    def set_mode(req):
+        state_msg.mode = req.custom_mode # set mocked mode to requested
+        return True,
+
+    rospy.Service('/mavros/set_mode', SetMode, set_mode)
+
+    telem = get_telemetry()
+    assert telem.connected == False
+
+    res = navigate()
+    assert res.success == False
+    assert res.message.startswith('No connection to FCU')
+
+    state_msg.connected = True
+    rospy.sleep(1)
+
+    telem = get_telemetry()
+    assert telem.connected == True
+
+    res = navigate()
+    assert res.success == False
+    assert res.message.startswith('No local position')
+
+    local_position_pub = rospy.Publisher('/mavros/local_position/pose', PoseStamped, latch=True, queue_size=1)
+    local_position_msg = PoseStamped()
+    local_position_msg.header.frame_id = 'map'
+    local_position_msg.pose.position.x = 1
+    local_position_msg.pose.position.y = 2
+    local_position_msg.pose.position.z = 3
+    local_position_msg.pose.orientation.w = 1
+
+    def publish_local_position():
+        r = rospy.Rate(30)
+        while not rospy.is_shutdown():
+            local_position_msg.header.stamp = rospy.Time.now()
+            local_position_pub.publish(local_position_msg)
+            r.sleep()
+
+    # start publishing local position
+    threading.Thread(target=publish_local_position, daemon=True).start()
+    rospy.sleep(0.5)
+
+    # check body frame
+    body = tf_buffer.lookup_transform('map', 'body', rospy.get_rostime(), rospy.Duration(1))
+    assert body.child_frame_id == 'body'
+    assert body.transform.translation.x == approx(1)
+    assert body.transform.translation.y == approx(2)
+    assert body.transform.translation.z == approx(3)
+
+    res = navigate(x=3, y=2, z=1, frame_id='map')
+    assert res.success == True
+    state = get_state()
+    assert state.mode == State.MODE_NAVIGATE
+    assert state.yaw_mode == State.YAW_MODE_YAW
+    assert state.x == 3
+    assert state.y == 2
+    assert state.z == 1
+    assert state.yaw == 0
+    assert state.xy_frame_id == 'map'
+    assert state.z_frame_id == 'map'
+    assert state.yaw_frame_id == 'map'
+    target = get_navigate_target(tf_buffer)
+    assert target.header.frame_id == 'map'
+    assert target.transform.translation.x == approx(3)
+    assert target.transform.translation.y == approx(2)
+    assert target.transform.translation.z == approx(1)
+    assert target.transform.rotation.x == 0
+    assert target.transform.rotation.y == 0
+    assert target.transform.rotation.z == 0
+    assert target.transform.rotation.w == 1
+
+    # try to set only the y
+    res = navigate(x=nan, y=1, z=nan)
+    assert res.success == False
+    assert res.message.startswith('x and y can be set only together')
+
+    # set z in body frame
+    res = navigate(x=nan, y=nan, z=1, frame_id='body')
+    assert res.success == True
+    state = get_state()
+    assert state.mode == State.MODE_NAVIGATE
+    assert state.yaw_mode == State.YAW_MODE_YAW
+    assert state.x == 3
+    assert state.y == 2
+    assert state.z == 4
+    assert state.yaw == 0
+    assert state.xy_frame_id == 'map'
+    assert state.z_frame_id == 'map'
+    assert state.yaw_frame_id == 'map'
+
+    # set xy in test frame
+    res = navigate(x=1, y=2, z=nan, frame_id='test')
+    assert res.success == True
+    state = get_state()
+    assert state.mode == State.MODE_NAVIGATE
+    assert state.yaw_mode == State.YAW_MODE_YAW
+    assert state.x == 1
+    assert state.y == 2
+    assert state.z == 4
+    assert state.yaw == 0
+    assert state.xy_frame_id == 'test'
+    assert state.z_frame_id == 'map'
+    assert state.yaw_frame_id == 'test'
+
+    # auto_arm should not invalidate the setpoint if not effective
+    res = navigate(x=nan, y=nan, z=1, frame_id='map', auto_arm=True)
+    assert res.success == True
+    state = get_state()
+    assert state.mode == State.MODE_NAVIGATE
+    assert state.yaw_mode == State.YAW_MODE_YAW
+    assert state.x == 1
+    assert state.y == 2
+    assert state.z == 1
+    assert state.yaw == 0
+    assert state.xy_frame_id == 'test'
+    assert state.z_frame_id == 'map'
+    assert state.yaw_frame_id == 'map'
+
+    # auto_arm should invalidate the setpoint if effective
+    state_msg.mode = 'STABILIZED' # pretend we are not in OFFBOARD mode
+    rospy.sleep(1)
+    res = navigate(x=nan, y=nan, z=1, frame_id='map', auto_arm=True)
+    assert res.success == True
+    state = get_state()
+    assert state.mode == State.MODE_NAVIGATE
+    assert state.yaw_mode == State.YAW_MODE_YAW
+    assert state.x == 1
+    assert state.y == 2
+    assert state.z == 1
+    assert state.yaw == 0
+    assert state.xy_frame_id == 'map'
+    assert state.z_frame_id == 'map'
+    assert state.yaw_frame_id == 'map'
+    state_msg.mode = 'OFFBOARD'
+    rospy.sleep(1)
+
+    # set_attitude should invalidate the setpoint
+    res = set_attitude()
+    assert res.success == True
+
+    res = navigate(x=5, y=6, z=nan, yaw=nan, frame_id='map')
+    assert res.success == True
+    state = get_state()
+    assert state.mode == State.MODE_NAVIGATE
+    assert state.yaw_mode == State.YAW_MODE_YAW
+    assert state.x == 5
+    assert state.y == 6
+    assert state.z == 3
+    assert state.yaw == 0
+    assert state.xy_frame_id == 'map'
+    assert state.z_frame_id == 'map'
+    assert state.yaw_frame_id == 'map'
+
+    # test set_altitude
+    res = set_altitude(z=7, frame_id='test')
+    assert res.success == True
+    state = get_state()
+    assert state.mode == State.MODE_NAVIGATE
+    assert state.yaw_mode == State.YAW_MODE_YAW
+    assert state.x == 5
+    assert state.y == 6
+    assert state.z == 7
+    assert state.yaw == 0
+    assert state.xy_frame_id == 'map'
+    assert state.z_frame_id == 'test'
+    assert state.yaw_frame_id == 'map'
+
+    # test set_yaw
+    res = set_yaw(yaw=0.5, frame_id='test2')
+    assert res.success == True
+    state = get_state()
+    assert state.mode == State.MODE_NAVIGATE
+    assert state.yaw_mode == State.YAW_MODE_YAW
+    assert state.x == 5
+    assert state.y == 6
+    assert state.z == 7
+    assert state.yaw == 0.5
+    assert state.xy_frame_id == 'map'
+    assert state.z_frame_id == 'test'
+    assert state.yaw_frame_id == 'test2'
+
+    # test set_yaw_rate
+    res = set_yaw_rate(yaw_rate=2)
+    assert res.success == True
+    state = get_state()
+    assert state.mode == State.MODE_NAVIGATE
+    assert state.yaw_mode == State.YAW_MODE_YAW_RATE
+    assert state.x == 5
+    assert state.y == 6
+    assert state.z == 7
+    assert state.yaw_rate == 2
+    assert state.xy_frame_id == 'map'
+    assert state.z_frame_id == 'test'
+
+    # navigate(yaw=nan) should keep yaw rate mode
+    res = navigate(x=nan, y=nan, z=nan, yaw=nan)
+    assert res.success == True
+    state = get_state()
+    assert state.mode == State.MODE_NAVIGATE
+    assert state.yaw_mode == State.YAW_MODE_YAW_RATE
+    assert state.x == 5
+    assert state.y == 6
+    assert state.z == 7
+    assert state.yaw_rate == 2
+    assert state.xy_frame_id == 'map'
+    assert state.z_frame_id == 'test'
+
+    # set_yaw(nan) should change back to yaw mode
+    res = set_yaw(yaw=nan)
+    assert res.success == True
+    state = get_state()
+    assert state.mode == State.MODE_NAVIGATE
+    assert state.yaw_mode == State.YAW_MODE_YAW
+    assert state.yaw == 0
+    assert state.yaw_frame_id == 'map'
+
+    # test set_position
+    res = set_position(x=nan, y=nan, z=13, yaw=nan, frame_id='test2')
+    assert res.success == True
+    state = get_state()
+    assert state.mode == State.MODE_POSITION
+    assert state.yaw_mode == State.YAW_MODE_YAW
+    assert state.x == 5
+    assert state.y == 6
+    assert state.z == 13
+    assert state.yaw == 0
+    assert state.xy_frame_id == 'map'
+    assert state.z_frame_id == 'test2'
+    assert state.yaw_frame_id == 'map'
+
+    # set_altitude should not change the mode
+    res = set_altitude(z=3, frame_id='test')
+    assert res.success == True
+    state = get_state()
+    assert state.mode == State.MODE_POSITION
+    assert state.yaw_mode == State.YAW_MODE_YAW
+    assert state.x == 5
+    assert state.y == 6
+    assert state.z == 3
+    assert state.yaw == 0
+    assert state.xy_frame_id == 'map'
+    assert state.z_frame_id == 'test'
+    assert state.yaw_frame_id == 'map'
+
+    # set_yaw should not change the main mode
+    res = set_yaw(yaw=1, frame_id='test2')
+    assert res.success == True
+    state = get_state()
+    assert state.mode == State.MODE_POSITION
+    assert state.yaw_mode == State.YAW_MODE_YAW
+    assert state.x == 5
+    assert state.y == 6
+    assert state.z == 3
+    assert state.yaw == 1
+    assert state.xy_frame_id == 'map'
+    assert state.z_frame_id == 'test'
+    assert state.yaw_frame_id == 'test2'
+
+    # test set_velocity
+    res = set_velocity(vx=1, frame_id='body')
+    state = get_state()
+    assert state.mode == State.MODE_VELOCITY
+    assert state.yaw_mode == State.YAW_MODE_YAW
+    assert state.vx == 1
+    assert state.vy == 0
+    assert state.vz == 0
+    assert state.yaw == 0
+    assert state.xy_frame_id == 'map'
+    assert state.z_frame_id == 'map'
+    assert state.yaw_frame_id == 'map'
+
+    # set_altitude should not work in velocity mode
+    res = set_altitude(z=3, frame_id='test')
+    assert res.success == False
+    assert res.message.startswith('Altitude cannot be set in')
+
+    # test set_attitude
+    res = set_attitude(roll=0.1, pitch=0.2, yaw=0.3, thrust=0.5)
+    assert res.success == True
+    state = get_state()
+    assert state.mode == State.MODE_ATTITUDE
+    assert state.yaw_mode == State.YAW_MODE_YAW
+    assert state.roll == approx(0.1)
+    assert state.pitch == approx(0.2)
+    assert state.yaw == approx(0.3)
+    assert state.thrust == approx(0.5)
+    assert state.yaw_frame_id == 'map'
+    msg = rospy.wait_for_message('/mavros/setpoint_attitude/attitude', PoseStamped, timeout=3)
+    # Tait-Bryan ZYX angle (rzyx) converted to quaternion
+    assert msg.pose.orientation.x == approx(0.0342708)
+    assert msg.pose.orientation.y == approx(0.10602051)
+    assert msg.pose.orientation.z == approx(0.14357218)
+    assert msg.pose.orientation.w == approx(0.98334744)
+    msg = rospy.wait_for_message('/mavros/setpoint_attitude/thrust', mavros_msgs.msg.Thrust, timeout=3)
+    assert msg.thrust == approx(0.5)
+
+    # set_yaw should work in attitude mode
+    res = set_yaw(yaw=0.7, frame_id='test2')
+    assert res.success == True
+    state = get_state()
+    assert state.mode == State.MODE_ATTITUDE
+    assert state.yaw_mode == State.YAW_MODE_YAW
+    assert state.roll == approx(0.1)
+    assert state.pitch == approx(0.2)
+    assert state.yaw == approx(0.7)
+    assert state.thrust == approx(0.5)
+    assert state.yaw_frame_id == 'test2'
+
+    # set_yaw_rate should not work in attitude mode
+    res = set_yaw_rate(yaw_rate=0.3)
+    assert res.success == False
+    assert res.message.startswith('Yaw rate cannot be set in')
+
+    # test set_rates
+    res = set_rates(roll_rate=nan, pitch_rate=nan, yaw_rate=0.3, thrust=0.6)
+    assert res.success == True
+    state = get_state()
+    assert state.mode == State.MODE_RATES
+    assert state.yaw_mode == State.YAW_MODE_YAW_RATE
+    assert state.roll_rate == approx(0)
+    assert state.pitch_rate == approx(0)
+    assert state.yaw_rate == approx(0.3)
+    assert state.thrust == approx(0.6)
+    msg = rospy.wait_for_message('/mavros/setpoint_raw/attitude', mavros_msgs.msg.AttitudeTarget, timeout=3)
+    assert msg.thrust == approx(0.6)
+
+    res = set_rates(roll_rate=0.3, pitch_rate=0.2, yaw_rate=0.1, thrust=0.4)
+    assert res.success == True
+    state = get_state()
+    assert state.mode == State.MODE_RATES
+    assert state.yaw_mode == State.YAW_MODE_YAW_RATE
+    assert state.roll_rate == approx(0.3)
+    assert state.pitch_rate == approx(0.2)
+    assert state.yaw_rate == approx(0.1)
+    assert state.thrust == approx(0.4)
+
+    res = set_rates(roll_rate=nan, pitch_rate=nan, yaw_rate=nan, thrust=0.3)
+    assert res.success == True
+    state = get_state()
+    assert state.mode == State.MODE_RATES
+    assert state.yaw_mode == State.YAW_MODE_YAW_RATE
+    assert state.roll_rate == approx(0.3)
+    assert state.pitch_rate == approx(0.2)
+    assert state.yaw_rate == approx(0.1)
+    assert state.thrust == approx(0.3)
+    msg = rospy.wait_for_message('/mavros/setpoint_raw/attitude', mavros_msgs.msg.AttitudeTarget, timeout=3)
+    assert msg.type_mask == mavros_msgs.msg.AttitudeTarget.IGNORE_ATTITUDE
+    assert msg.body_rate.x == approx(0.3)
+    assert msg.body_rate.y == approx(0.2)
+    assert msg.body_rate.z == approx(0.1)
+
+    # set_yaw_rate should work in rates mode
+    res = set_yaw_rate(yaw_rate=0.4)
+    assert res.success == True
+    state = get_state()
+    assert state.mode == State.MODE_RATES
+    assert state.yaw_mode == State.YAW_MODE_YAW_RATE
+    assert state.roll_rate == approx(0.3)
+    assert state.pitch_rate == approx(0.2)
+    assert state.yaw_rate == approx(0.4)
+    assert state.thrust == approx(0.3)
+
+    res = set_rates(roll_rate=inf)
+    assert res.success == False
+    assert res.message == 'roll_rate argument cannot be Inf'
+
+    # test land service
+    res = land()
+    assert res.success == True
+    assert state_msg.mode == 'AUTO.LAND' # check that the mode was set correctly

clover/test/offboard.test

@@ -0,0 +1,10 @@
+<launch>
+    <node name="simple_offboard" pkg="clover" type="simple_offboard" required="true" output="screen"/>
+
+    <node pkg="tf2_ros" type="static_transform_publisher" name="test_frame" args="10 20 30 0 0 0 map test"/>
+
+    <node pkg="tf2_ros" type="static_transform_publisher" name="test2_frame" args="100 200 300 0 0 0 map test2"/>
+
+    <param name="test_module" value="$(find clover)/test/offboard.py"/>
+    <test test-name="offboard_test" pkg="ros_pytest" type="ros_pytest_runner"/>
+</launch>

clover/udev/99-px4fmu.rules

@@ -0,0 +1,54 @@
+# Source files: PX4-Autopilot/boards/**/nuttx-config/nsh/defconfig
+
+# Additional info:
+# https://docs.px4.io/main/en/flight_controller/
+# https://github.com/mavlink/qgroundcontrol/blob/master/src/comm/USBBoardInfo.json
+
+SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0001", ATTRS{product}=="PX4 GNF405", SYMLINK+="px4fmu"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0001", ATTRS{product}=="PX4 OmnibusF4SD", SYMLINK+="px4fmu"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0016", ATTRS{product}=="PX4 Crazyflie v2.0", SYMLINK+="px4fmu"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="1FC9", ATTRS{idProduct}=="001c", ATTRS{product}=="PX4 FMUK66 v3.x", SYMLINK+="px4fmu"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="1FC9", ATTRS{idProduct}=="001c", ATTRS{product}=="PX4 FMUK66 E", SYMLINK+="px4fmu"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="1FC9", ATTRS{idProduct}=="001d", ATTRS{product}=="PX4 FMURT1062 v1.x", SYMLINK+="px4fmu"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0001", ATTRS{product}=="DiatoneMambaF405 MK2", SYMLINK+="px4fmu"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="a32f", ATTRS{product}=="PX4 FMU ModalAI FCv1", SYMLINK+="px4fmu"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="a330", ATTRS{product}=="PX4 FMU ModalAI FCv2", SYMLINK+="px4fmu"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0012", ATTRS{product}=="PX4 FMU UVify Core", SYMLINK+="px4fmu"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="3162", ATTRS{idProduct}=="0050", ATTRS{product}=="PX4 KakuteH7", SYMLINK+="px4fmu"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="3162", ATTRS{idProduct}=="0050", ATTRS{product}=="PX4 KakuteH7v2", SYMLINK+="px4fmu"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="3162", ATTRS{idProduct}=="004b", ATTRS{product}=="PX4 DurandalV1", SYMLINK+="px4fmu"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0050", ATTRS{product}=="PX4 KakuteF7", SYMLINK+="px4fmu"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="3162", ATTRS{idProduct}=="0050", ATTRS{product}=="PX4 KakuteH7Mini-nand", SYMLINK+="px4fmu"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="3162", ATTRS{idProduct}=="004E", ATTRS{product}=="PX4 PIX32V5", SYMLINK+="px4fmu"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0061", ATTRS{product}=="PX4 ATL Mantis-EDU", SYMLINK+="px4fmu"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="3163", ATTRS{idProduct}=="004c", ATTRS{product}=="PX4 CUAV Nora", SYMLINK+="px4fmu"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="3163", ATTRS{idProduct}=="004c", ATTRS{product}=="PX4 CUAV X7Pro", SYMLINK+="px4fmu"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="1B8C", ATTRS{idProduct}=="0036", ATTRS{product}=="MatekH743-mini", SYMLINK+="px4fmu"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="1B8C", ATTRS{idProduct}=="0036", ATTRS{product}=="MatekH743", SYMLINK+="px4fmu"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="120A", ATTRS{idProduct}=="1004", ATTRS{product}=="Matekgnssm9nf4", SYMLINK+="px4fmu"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="1209", ATTRS{idProduct}=="1013", ATTRS{product}=="MatekH743", SYMLINK+="px4fmu"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="0037", ATTRS{product}=="PX4 FMU SmartAP AIRLink", SYMLINK+="px4fmu"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="2DAE", ATTRS{idProduct}=="1058", ATTRS{product}=="CubeOrange+", SYMLINK+="px4fmu"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="2DAE", ATTRS{idProduct}=="1012", ATTRS{product}=="CubeYellow", SYMLINK+="px4fmu"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="2DAE", ATTRS{idProduct}=="1016", ATTRS{product}=="CubeOrange", SYMLINK+="px4fmu"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="3185", ATTRS{idProduct}=="0035", ATTRS{product}=="PX4 FMU v6X.x", SYMLINK+="px4fmu"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="3185", ATTRS{idProduct}=="0038", ATTRS{product}=="PX4 FMU v6C.x", SYMLINK+="px4fmu"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="3185", ATTRS{idProduct}=="0033", ATTRS{product}=="PX4 FMU v5X.x", SYMLINK+="px4fmu"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="1B8C", ATTRS{idProduct}=="0036", ATTRS{product}=="PX4 FMU v6U.x", SYMLINK+="px4fmu"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0013", ATTRS{product}=="PX4 FMU v4.x PRO", SYMLINK+="px4fmu"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0011", ATTRS{product}=="PX4 FMU v2.x", SYMLINK+="px4fmu"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0012", ATTRS{product}=="PX4 FMU v4.x", SYMLINK+="px4fmu"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0032", ATTRS{product}=="PX4 FMU v5.x", SYMLINK+="px4fmu"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="3162", ATTRS{idProduct}=="004b", ATTRS{product}=="PX4 SP RACING H7 EXTREME", SYMLINK+="px4fmu"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0030", ATTRS{product}=="MindPX FMU v2.x", SYMLINK+="px4fmu"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="3185", ATTRS{idProduct}=="0039", ATTRS{product}=="ARK FMU v6X.x", SYMLINK+="px4fmu"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0016", ATTRS{product}=="PX4 FreeFly RTK GPS", SYMLINK+="px4fmu"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="1024", ATTRS{product}=="mRoControlZeroH7 OEM", SYMLINK+="px4fmu"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="1017", ATTRS{product}=="mRoPixracerPro", SYMLINK+="px4fmu"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="1023", ATTRS{product}=="mRoControlZeroH7", SYMLINK+="px4fmu"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="008D", ATTRS{product}=="mRoControlZeroF7", SYMLINK+="px4fmu"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0021", ATTRS{product}=="PX4 AUAV X2.1", SYMLINK+="px4fmu"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="1022", ATTRS{product}=="mRoControlZero Classic", SYMLINK+="px4fmu"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0088", ATTRS{product}=="mRo x2.1-777", SYMLINK+="px4fmu"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="35a7", ATTRS{idProduct}=="0002", ATTRS{product}=="FCC-R1", SYMLINK+="px4fmu"
+SUBSYSTEM=="tty", ATTRS{idVendor}=="35a7", ATTRS{idProduct}=="0001", ATTRS{product}=="FCC-K1", SYMLINK+="px4fmu"

clover/www/clover.err

@@ -1 +0,0 @@
-/tmp/clover.err

clover/www/clover.log

@@ -0,0 +1 @@
+/var/log/clover.log

clover/www/console.html

@@ -0,0 +1,23 @@
+<h1>
+	/var/log/clover.log
+	<a style="font-size: 0.5em; vertical-align: super; font-weight: normal" href="clover.log" download>download</a>
+</h1>
+
+<pre></pre>
+
+<script type="module">
+	var pre = document.querySelector('pre');
+
+	fetch('clover.log?' + Math.random()).then(function(response) { // random to forbid caching
+		if (response.status == 404) {
+			pre.innerHTML = '/var/log/clover.log does not exist';
+			return;
+		} else if (response.status !== 200) {
+			pre.innerHTML('Error ' + response.status);
+			return;
+		}
+		response.text().then(function(content) {
+			pre.innerHTML = content;
+		});
+	});
+</script>

clover/www/index.html

@@ -4,12 +4,12 @@
 
 <ul>
 	<li><a href="docs">View documentation</a> (snapshot of <a href="https://clover.coex.tech">clover.coex.tech</a>)</li>
+	<li><a href="topics.html">View topics</a></li>
 	<li><a href="" id="wvs">View image topics</a> (<code>web_video_server</code>)</li>
 	<li><a href="" id="butterfly">Open web terminal</a> (<code>Butterfly</code>)</li>
-	<li><a href="viz.html">View 3D visualization</a> (<code>ros3djs</code>)</li>
+	<li>View <a href="viz.html">View 3D visualization</a>, <a href="aruco_map.html">3D visualization for markers map</a> (<code>ros3djs</code>)</li>
-	<li><a href="aruco_map.html">3D visualization for markers map</a> (<code>ros3djs</code>)</li>
 	<li><a href="../clover_blocks/">Blocks programming</a> (<code>Blockly</code>)</li>
-	<li><a href="clover.err">Clover console</a> (<code>/tmp/clover.err</code>)</li>
+	<li><a href="console.html">Clover console</a> (<code>/var/log/clover.log</code>)</li>
 </ul>
 
 <div class="version"></div>
@@ -18,6 +18,14 @@
 	document.querySelector("#wvs").href = location.protocol + '//' + location.hostname + ':8080';
 	document.querySelector("#butterfly").href = location.protocol + '//' + location.hostname + ':57575';
 
+	document.querySelector("#butterfly").addEventListener('click', function(e) {
+		if (location.hostname == 'localhost' || location.hostname == '127.0.0.1') {
+			if (!confirm('Please use regular Terminal app on a local machine.\nClick OK to proceed to Butterfly anyway.')) {
+				e.preventDefault();
+			}
+		}
+	});
+
 	// Determine image version
 	fetch('clover_version').then(function(response) {
 		if (response.status !== 200) return;

clover/www/js/json-to-pretty-yaml.js

@@ -0,0 +1,236 @@
+// Browserified https://www.npmjs.com/package/json-to-pretty-yaml module
+
+(function(){function r(e,n,t){function o(i,f){if(!n[i]){if(!e[i]){var c="function"==typeof require&&require;if(!f&&c)return c(i,!0);if(u)return u(i,!0);var a=new Error("Cannot find module '"+i+"'");throw a.code="MODULE_NOT_FOUND",a}var p=n[i]={exports:{}};e[i][0].call(p.exports,function(r){var n=e[i][1][r];return o(n||r)},p,p.exports,r,e,n,t)}return n[i].exports}for(var u="function"==typeof require&&require,i=0;i<t.length;i++)o(t[i]);return o}return r})()({1:[function(require,module,exports){
+(function() {
+    "use strict";
+
+    var typeOf = require('remedial').typeOf;
+    var trimWhitespace = require('remove-trailing-spaces');
+
+    function stringify(data) {
+        var handlers, indentLevel = '';
+
+        handlers = {
+            "undefined": function() {
+                // objects will not have `undefined` converted to `null`
+                // as this may have unintended consequences
+                // For arrays, however, this behavior seems appropriate
+                return 'null';
+            },
+            "null": function() {
+                return 'null';
+            },
+            "number": function(x) {
+                return x;
+            },
+            "boolean": function(x) {
+                return x ? 'true' : 'false';
+            },
+            "string": function(x) {
+                // to avoid the string "true" being confused with the
+                // the literal `true`, we always wrap strings in quotes
+                return JSON.stringify(x);
+            },
+            "array": function(x) {
+                var output = '';
+
+                if (0 === x.length) {
+                    output += '[]';
+                    return output;
+                }
+
+                indentLevel = indentLevel.replace(/$/, '  ');
+                x.forEach(function(y, i) {
+                    // TODO how should `undefined` be handled?
+                    var handler = handlers[typeOf(y)];
+
+                    if (!handler) {
+                        throw new Error('what the crap: ' + typeOf(y));
+                    }
+
+                    output += '\n' + indentLevel + '- ' + handler(y, true);
+
+                });
+                indentLevel = indentLevel.replace(/  /, '');
+
+                return output;
+            },
+            "object": function(x, inArray, rootNode) {
+                var output = '';
+
+                if (0 === Object.keys(x).length) {
+                    output += '{}';
+                    return output;
+                }
+
+                if (!rootNode) {
+                    indentLevel = indentLevel.replace(/$/, '  ');
+                }
+
+                Object.keys(x).forEach(function(k, i) {
+                    var val = x[k],
+                        handler = handlers[typeOf(val)];
+
+                    if ('undefined' === typeof val) {
+                        // the user should do
+                        // delete obj.key
+                        // and not
+                        // obj.key = undefined
+                        // but we'll error on the side of caution
+                        return;
+                    }
+
+                    if (!handler) {
+                        throw new Error('what the crap: ' + typeOf(val));
+                    }
+
+                    if (!(inArray && i === 0)) {
+                        output += '\n' + indentLevel;
+                    }
+
+                    output += k + ': ' + handler(val);
+                });
+                indentLevel = indentLevel.replace(/  /, '');
+
+                return output;
+            },
+            "function": function() {
+                // TODO this should throw or otherwise be ignored
+                return '[object Function]';
+            }
+        };
+        return trimWhitespace(handlers[typeOf(data)](data, true, true) + '\n');
+
+    }
+
+    window.yamlStringify = stringify;
+    module.exports.stringify = stringify;
+}());
+
+},{"remedial":2,"remove-trailing-spaces":3}],2:[function(require,module,exports){
+/*jslint onevar: true, undef: true, nomen: true, eqeqeq: true, plusplus: true, bitwise: true, regexp: true, newcap: true, immed: true */
+(function () {
+    "use strict";
+
+    var global = Function('return this')()
+      , classes = "Boolean Number String Function Array Date RegExp Object".split(" ")
+      , i
+      , name
+      , class2type = {}
+      ;
+
+    for (i in classes) {
+      if (classes.hasOwnProperty(i)) {
+        name = classes[i];
+        class2type["[object " + name + "]"] = name.toLowerCase();
+      }
+    }
+
+    function typeOf(obj) {
+      return (null === obj || undefined === obj) ? String(obj) : class2type[Object.prototype.toString.call(obj)] || "object";
+    }
+
+    function isEmpty(o) {
+        var i, v;
+        if (typeOf(o) === 'object') {
+            for (i in o) { // fails jslint
+                v = o[i];
+                if (v !== undefined && typeOf(v) !== 'function') {
+                    return false;
+                }
+            }
+        }
+        return true;
+    }
+
+    if (!String.prototype.entityify) {
+        String.prototype.entityify = function () {
+            return this.replace(/&/g, "&amp;").replace(/</g,
+                "&lt;").replace(/>/g, "&gt;");
+        };
+    }
+
+    if (!String.prototype.quote) {
+        String.prototype.quote = function () {
+            var c, i, l = this.length, o = '"';
+            for (i = 0; i < l; i += 1) {
+                c = this.charAt(i);
+                if (c >= ' ') {
+                    if (c === '\\' || c === '"') {
+                        o += '\\';
+                    }
+                    o += c;
+                } else {
+                    switch (c) {
+                    case '\b':
+                        o += '\\b';
+                        break;
+                    case '\f':
+                        o += '\\f';
+                        break;
+                    case '\n':
+                        o += '\\n';
+                        break;
+                    case '\r':
+                        o += '\\r';
+                        break;
+                    case '\t':
+                        o += '\\t';
+                        break;
+                    default:
+                        c = c.charCodeAt();
+                        o += '\\u00' + Math.floor(c / 16).toString(16) +
+                            (c % 16).toString(16);
+                    }
+                }
+            }
+            return o + '"';
+        };
+    } 
+
+    if (!String.prototype.supplant) {
+        String.prototype.supplant = function (o) {
+            return this.replace(/{([^{}]*)}/g,
+                function (a, b) {
+                    var r = o[b];
+                    return typeof r === 'string' || typeof r === 'number' ? r : a;
+                }
+            );
+        };
+    }
+
+    if (!String.prototype.trim) {
+        String.prototype.trim = function () {
+            return this.replace(/^\s*(\S*(?:\s+\S+)*)\s*$/, "$1");
+        };
+    }
+
+    // CommonJS / npm / Ender.JS
+    module.exports = {
+        typeOf: typeOf,
+        isEmpty: isEmpty
+    };
+    global.typeOf = global.typeOf || typeOf;
+    global.isEmpty = global.isEmpty || isEmpty;
+}());
+
+},{}],3:[function(require,module,exports){
+"use strict";
+
+/**
+ * removeTrailingSpaces
+ * Remove the trailing spaces from a string.
+ *
+ * @name removeTrailingSpaces
+ * @function
+ * @param {String} input The input string.
+ * @returns {String} The output string.
+ */
+
+module.exports = function removeTrailingSpaces(input) {
+  // TODO If possible, use a regex
+  return input.split("\n").map(function (x) {
+    return x.trimRight();
+  }).join("\n");
+};
+},{}]},{},[1]);

clover/www/js/topics.js

@@ -0,0 +1,86 @@
+const url = 'ws://' + location.hostname + ':9090';
+const ros = new ROSLIB.Ros({ url: url });
+const params = Object.fromEntries(new URLSearchParams(window.location.search).entries());
+
+ros.on('connection', function () {
+	document.body.classList.add('connected');
+	document.body.classList.remove('closed');
+	init();
+});
+
+ros.on('close', function () {
+	document.body.classList.remove('connected');
+	document.body.classList.add('closed');
+	setTimeout(function() {
+		// reconnect
+		ros.connect(url);
+	}, 2000);
+});
+
+const title = document.querySelector('h1');
+const topicsList = document.querySelector('#topics');
+const topicMessage = document.querySelector('#topic-message');
+
+function viewTopicsList() {
+	title.innerHTML = 'Topics:';
+
+	ros.getTopics(function(topics) {
+		topicsList.innerHTML = topics.topics.map(function(topic, i) {
+			const type = topics.types[i];
+			if (type == 'sensor_msgs/Image') {
+				let url = `${location.protocol}//${location.hostname}:8080/stream_viewer?topic=${topic}`; 
+				return `<li><a href="${url}" class=topic title=${type}>${topic}</a> &#x1F5BC;</li>`;
+			} else {
+				return `<li><a href="?topic=${topic}" class=topic title=${type}>${topic}</a></li>`;
+			}
+		}).join('');
+	});
+}
+
+let rosdistro;
+
+function viewTopic(topic) {
+	let counter = 0;
+	let index = '<a href=topics.html>Topics</a>';
+	title.innerHTML = `${index}: ${topic}`;
+	topicMessage.style.display = 'block';
+
+	ros.getTopicType(topic, function(typeStr) {
+		const [pack, type] = typeStr.split('/');
+		let href = `https://docs.ros.org/en/${rosdistro}/api/${pack}/html/msg/${type}.html`;
+		title.innerHTML = `${index}: ${topic} <a id="topic-type" href=${href} target="_blank">${typeStr}</a>`;
+	});
+
+	new ROSLIB.Topic({ ros: ros, name: topic }).subscribe(function(msg) {
+		counter++;
+		document.title = topic;
+		if (mouseDown) return;
+
+		if (msg.header && msg.header.stamp) {
+			if (params.date || params.offset) {
+				let date = new Date(msg.header.stamp.secs * 1e3 + msg.header.stamp.nsecs * 1e-6);
+				if (params.date) msg.header.date = date.toISOString();
+				if (params.offset) msg.header.offset = (new Date() - date) * 1e-3;
+			}
+		}
+
+		let txt = `<div class=counter>${counter} received</div>${yamlStringify(msg)}`; // JSON.stringify(msg, null, 4);
+		topicMessage.innerHTML = txt;
+	});
+}
+
+let mouseDown;
+
+topicMessage.addEventListener('mousedown', function() { mouseDown = true; });
+topicMessage.addEventListener('mouseup', function() { mouseDown = false; });
+
+function init() {
+	if (!params.topic) {
+		viewTopicsList();
+	} else {
+		new ROSLIB.Param({ ros: ros, name: '/rosdistro'}).get(function(value) {
+			rosdistro = value.trim();
+			viewTopic(params.topic);
+		});
+	}
+}