travis: Use short commit notation

travis: Fix IMAGE_VERSION initialization
travis: Generate zip file after successful build
2026-05-31 23:19:32 +00:00 · 2020-08-10 00:19:05 +03:00 · 2020-08-09 01:00:56 +03:00 · 2020-08-08 21:26:06 +03:00 · 2020-08-07 17:36:56 +03:00 · 2020-08-07 17:28:48 +03:00
972 changed files with 2823 additions and 47660 deletions
--- a/.gitattributes
+++ b/.gitattributes
@@ -3,7 +3,4 @@ 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
--- a/.github/workflows/build-image.yaml
+++ b/.github/workflows/build-image.yaml
@@ -1,29 +0,0 @@
-name: RPi image
-
-on:
-  push:
-    branches: [ '*' ]
-  pull_request:
-    branches: [ master ]
-  release:
-    types: [ created ]
-
-jobs:
-  build:
-    runs-on: ubuntu-latest
-    steps:
-      - uses: actions/checkout@v2
-      - name: Build image
-        run: |
-          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: |
-          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' }}
-        with:
-          files: images/clover_*.zip
-          prerelease: true
-        env:
-          GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }}
--- a/.github/workflows/build.yml
+++ b/.github/workflows/build.yml
@@ -1,23 +0,0 @@
-name: Build
-
-on:
-  push:
-    branches: [ '*' ]
-  pull_request:
-    branches: [ master ]
-
-jobs:
-  # 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
-    steps:
-      - uses: actions/checkout@v2
-      - name: Native Noetic build
-        run: |
-          docker run --rm -v $(pwd):/root/catkin_ws/src/clover ros:noetic-ros-base /root/catkin_ws/src/clover/builder/standalone-install.sh
--- a/.github/workflows/docs.yml
+++ b/.github/workflows/docs.yml
@@ -1,86 +0,0 @@
-name: Documentation
-
-on:
-  push:
-    branches: [ '*' ]
-  pull_request:
-    branches: [ '*' ]
-
-permissions:
-  contents: read
-  pages: write
-  id-token: write
-
-concurrency:
-  group: "pages"
-  cancel-in-progress: true
-
-defaults:
-  run:
-    shell: bash
-
-jobs:
-  docs:
-    runs-on: ubuntu-22.04
-    steps:
-      - uses: actions/checkout@v2
-      - name: Use Node.js
-        uses: actions/setup-node@v1
-        with: { node-version: '10' }
-      - name: Setup tools
-        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
-          builder/assets/install_gitbook.sh
-          npm install markdownlint-cli@0.28.1 -g # FIXME: https://github.com/DavidAnson/markdownlint/issues/435
-          npm install svgexport -g
-          gitbook -V
-          markdownlint -V
-      - name: Run markdownlint
-        run: markdownlint docs
-      - name: Check Assets
-        run: |
-          ./check_assets_size.py
-          ./check_unused_assets.py
-      - name: Build GitBook
-        run: |
-         gitbook install
-         gitbook build
-      - name: Generate PDF
-        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
-          gs -sDEVICE=pdfwrite -dCompatibilityLevel=1.4 -dPDFSETTINGS=/default -dNOPAUSE -dQUIET -dBATCH -dDetectDuplicateImages -dCompressFonts=true -r150 -sOutputFile=_book/clover_ru_compressed.pdf _book/clover_ru.pdf
-          gs -sDEVICE=pdfwrite -dCompatibilityLevel=1.4 -dPDFSETTINGS=/default -dNOPAUSE -dQUIET -dBATCH -dDetectDuplicateImages -dCompressFonts=true -r150 -sOutputFile=_book/clover_en_compressed.pdf _book/clover_en.pdf
-          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
-          echo '::set-output name=GITBOOK_PDF_OK::1'
-      - name: Download older PDFs
-        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:
-          path: _book
-
-  deploy-docs:
-    if: ${{ github.event_name == 'push' && github.ref == 'refs/heads/master' }}
-    environment:
-      name: github-pages
-      url: ${{ steps.deployment.outputs.page_url }}
-    runs-on: ubuntu-latest
-    needs: docs
-    steps:
-      - name: Deploy to GitHub Pages
-        id: deployment
-        uses: actions/deploy-pages@v1
--- a/.github/workflows/editorconfig.yaml
+++ b/.github/workflows/editorconfig.yaml
@@ -1,18 +0,0 @@
-name: Editorconfig
-
-on:
-  push:
-    branches: [ '*' ]
-  pull_request:
-    branches: [ master ]
-
-jobs:
-  editorconfig:
-    runs-on: ubuntu-latest
-    steps:
-      - uses: actions/checkout@v2
-      - name: .editorconfig Linter
-        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|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"
--- a/.gitignore
+++ b/.gitignore
@@ -4,6 +4,3 @@
 node_modules/
 _book/
 package-lock.json
-clover_blocks/programs/*.*
-!clover_blocks/programs/examples/*
-/.vscode/
--- a/.markdownlint.json
+++ b/.markdownlint.json
@@ -21,9 +21,7 @@
            "ROS",
            "ROS Kinetic",
            "ROS Melodic",
-            "ROS Noetic",
            "OpenCV",
-            "OpenVPN",
            "Gazebo",
            "GitHub",
            "FPV",
@@ -108,10 +106,7 @@
            "UDP",
            "QR",
            "Li-ion",
-            "Nvidia",
-            "VirtualBox",
-            "VMware",
-            "DuoCam"
+            "Nvidia"
        ],
        "code_blocks": false
    },
--- a/.travis.yml
+++ b/.travis.yml
@@ -0,0 +1,126 @@
+os: linux
+dist: xenial
+language: generic
+services:
+  - docker
+env:
+  global:
+    - DOCKER="sfalexrog/img-tool:qemu-update"
+    - TARGET_REPO="https://github.com/${TRAVIS_REPO_SLUG}.git"
+    - IMAGE_VERSION=${TRAVIS_TAG:-${TRAVIS_COMMIT:0:7}}
+    - IMAGE_NAME="$(basename -s '.git' ${TARGET_REPO})_${IMAGE_VERSION}.img"
+git:
+  depth: 50
+jobs:
+  fast_finish: true
+  include:
+    - stage: Build
+      name: "Raspberry Pi Image Build"
+      cache:
+        directories:
+        - imgcache
+      before_script:
+        - docker pull ${DOCKER}
+# Check if there are any cached images, copy them to our "images" directory
+        - if [ -n "$(ls -A imgcache/*.zip)" ]; then mkdir -p images && cp imgcache/*.zip images; fi
+      script:
+        - if [[ -z ${TRAVIS_TAG} && "${TRAVIS_PULL_REQUEST}" != "false" ]]; then
+            echo "Commit range is ${TRAVIS_COMMIT_RANGE}" &&
+            if [ $(git diff --name-only ${TRAVIS_COMMIT_RANGE} | grep -v ^"docs/" | wc -l) -eq 0 ]; then
+              echo " === Docs-only change; skipping build ===" &&
+              export SKIP_BUILD=true;
+            fi;
+          fi
+        - if [ -z ${SKIP_BUILD} ]; then
+            docker run --privileged --rm -v /dev:/dev -v $(pwd):/builder/repo -e TRAVIS_TAG="${TRAVIS_TAG}" ${DOCKER};
+          fi
+      before_cache:
+        - cp images/*.zip imgcache
+      after_success:
+        - sudo chmod -R 777 *
+        - cd images && zip ${IMAGE_NAME}.zip ${IMAGE_NAME} && stat --printf="Compressed image size:%s\n" ${IMAGE_NAME}.zip
+      before_deploy:
+        # Set up git user name and tag this commit
+        - git config --local user.name "goldarte"
+        - git config --local user.email "goldartt@gmail.com"
+      deploy:
+        provider: releases
+        token: ${GITHUB_OAUTH_TOKEN}
+        file: ${IMAGE_NAME}.zip
+        skip_cleanup: true
+        on:
+          tags: true
+        draft: true
+        name: ${TRAVIS_TAG}
+    - stage: Build
+      name: "Native Kinetic build"
+      env:
+        - NATIVE_DOCKER=ros:kinetic-ros-base
+      before_script:
+        - docker pull ${NATIVE_DOCKER}
+      script:
+        - docker run --rm -v $(pwd):/root/catkin_ws/src/clover ${NATIVE_DOCKER} /root/catkin_ws/src/clover/builder/standalone-install.sh
+    - stage: Build
+      name: "Native Melodic build"
+      env:
+        - NATIVE_DOCKER=ros:melodic-ros-base
+      before_script:
+        - docker pull ${NATIVE_DOCKER}
+      script:
+        - docker run --rm -v $(pwd):/root/catkin_ws/src/clover ${NATIVE_DOCKER} /root/catkin_ws/src/clover/builder/standalone-install.sh
+    - stage: Build
+      name: "Documentation"
+      language: node_js
+      node_js:
+        - "10"
+      before_script:
+        - sudo sh -c "echo ttf-mscorefonts-installer msttcorefonts/accepted-mscorefonts-eula select true | debconf-set-selections"
+        - sudo apt update && sudo apt install -y calibre msttcorefonts
+        - npm install gitbook-cli -g
+        - npm install markdownlint-cli -g
+        - npm install svgexport -g
+        - gitbook -V
+        - markdownlint -V
+      script:
+        - markdownlint docs
+        - ./check_assets_size.py
+        - ./check_unused_assets.py
+        - gitbook install
+        - gitbook build
+        - gitbook pdf ./ _book/clover.pdf
+      deploy:
+        provider: pages
+        local_dir: _book
+        skip_cleanup: true
+        token: ${GITHUB_OAUTH_TOKEN}
+        keep_history: true
+        target_branch: master
+        repo: CopterExpress/clover.coex.tech
+        fqdn: clover.coex.tech
+        verbose: true
+        on:
+          branch: master
+    - stage: Annotate
+      name: Auto-generate changelog
+      language: python
+      python: 3.6
+      install:
+        - pip install GitPython PyGithub
+      script:
+        - PYTHONUNBUFFERED=1 python ./gen_changelog.py
+    - stage: Build
+      name: Editorconfig-lint
+      language: generic
+      before_script:
+        - wget https://github.com/okalachev/editorconfig-checker/releases/download/1.2.1-disable-spaces-amount/ec-linux-amd64
+        - chmod +x ec-linux-amd64
+      script:
+        - ./ec-linux-amd64 -spaces-after-tabs -e "roslib.js|ros3d.js|eventemitter2.js|draw.cpp|BinUtils.swift|\.idea|apps/android/app|Assets.xcassets|test_parser_pass.txt|test_node_failure.txt|aruco_pose/vendor|\.stl|\.dxf|\.dae"
+stages:
+  - Build
+  - Annotate
+# More info there
+# https://github.com/travis-ci/travis-ci/issues/6893
+# https://docs.travis-ci.com/user/customizing-the-build/
+# https://docs.travis-ci.com/user/deployment/releases
+# https://docs.travis-ci.com/user/environment-variables/
--- a/README.md
+++ b/README.md
@@ -1,14 +1,12 @@
-# clover🍀: create autonomous drones easily
+# COEX Clover Drone Kit

-<img src="docs/assets/clover42-main-margin.png" align="right" width="400px" alt="COEX Clover Drone">
+<img src="docs/assets/clever4-front-white.png" align="right" width="400px" alt="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.
+Clover is an educational programmable drone kit consisting of an unassembled quadcopter, open source software and documentation. The kit includes Pixracer-compatible autopilot running PX4 firmware, Raspberry Pi 4 as companion computer, a camera for computer vision navigation as well as additional sensors and peripheral devices.

-COEX Clover Drone is an educational programmable drone kit, suited perfectly for running clover software. The kit is shipped unassembled and includes Pixracer-compatible autopilot running PX4 firmware, Raspberry Pi 4 as a companion computer, a camera for computer vision navigation as well as additional sensors and peripheral devices. Batteries included.
+The main documentation is available [on Gitbook](https://clover.coex.tech/).

-The main documentation is available at [https://clover.coex.tech](https://clover.coex.tech/). Official website: [coex.tech/clover](https://coex.tech/clover).
-
-[__Support us on Kickstarter!__](https://www.kickstarter.com/projects/copterexpress/cloverdrone)
+Official website: <a href="https://coex.tech/clover">coex.tech/clover</a>.

 ## Video compilation

@@ -20,13 +18,12 @@ 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 all releases](https://img.shields.io/github/downloads/CopterExpress/clover/total)
+[![Build Status](https://travis-ci.org/CopterExpress/clover.svg?branch=master)](https://travis-ci.org/CopterExpress/clover)

 Image features:

 * Raspbian Buster
-* [ROS Noetic](http://wiki.ros.org/noetic)
+* [ROS Melodic](http://wiki.ros.org/melodic)
 * Configured networking
 * OpenCV
 * [`mavros`](http://wiki.ros.org/mavros)
@@ -38,10 +35,6 @@ API description for autonomous flights is available [on GitBook](https://clover.

 For manual package installation and running see [`clover` package documentation](clover/README.md).

-## Support
-
-[![Telegram Support Chat](https://img.shields.io/endpoint?label=Support%20Chat&url=https%3A%2F%2Ftelegram-badge-4mbpu8e0fit4.runkit.sh%2F%3Furl%3Dhttps%3A%2F%2Ft.me%2FCOEXHelpDesk)](https://t.me/COEXHelpdesk)
-
 ## License

 While the Clover platform source code is available under the MIT License, note, that the [documentation](docs/) is licensed under the Creative Commons Attribution-NonCommercial-ShareAlike 4.0 International License.
--- a/apps/ios/cleverrc/README.md
+++ b/apps/ios/cleverrc/README.md
@@ -1,4 +1,5 @@
-# iOS-приложение для управления Клевером
+iOS-приложение для управления Клевером
+--------------------------------------

 Для установки зависимостей необходим [CocoaPods](https://cocoapods.org):

--- a/aruco_pose/CHANGELOG.rst
+++ b/aruco_pose/CHANGELOG.rst
@@ -1,8 +0,0 @@
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-Changelog for package aruco_pose
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-
-0.21.1 (2020-11-17)
-------------------
-* First release of aruco_pose package to ROS
-* Contributors: Alamoris, Alexey Rogachevskiy, Arthur Golubtsov, Ilya Petrov, Oleg Kalachev
--- a/aruco_pose/CMakeLists.txt
+++ b/aruco_pose/CMakeLists.txt
@@ -22,21 +22,13 @@ find_package(catkin REQUIRED COMPONENTS
  dynamic_reconfigure
 )

-# Workaround for OpenCV 3/4 support
-set(_opencv_version 4)
-find_package(OpenCV ${_opencv_version} QUIET COMPONENTS core imgproc calib3d)
-if (NOT OpenCV_FOUND)
-  message(STATUS "Did not find OpenCV 4, searching for OpenCV 3")
-  set(_opencv_version 3)
-endif()
-
-find_package(OpenCV ${_opencv_version} REQUIRED COMPONENTS core imgproc calib3d)
+find_package(OpenCV 3 REQUIRED COMPONENTS core imgproc calib3d)
 if ("${OpenCV_VERSION_MINOR}" LESS "9")
  message(STATUS "OpenCV version too low, using vendored ArUco package")
  include(vendor/VendorOpenCV.cmake)
 else()
  message(STATUS "Using system OpenCV ArUco package")
-  find_package(OpenCV ${_opencv_version} REQUIRED COMPONENTS aruco)
+  find_package(OpenCV 3 REQUIRED COMPONENTS aruco)
 endif()
 message(STATUS "OpenCV include dirs: ${OpenCV_INCLUDE_DIRS}")
 message(STATUS "OpenCV libraries: ${OpenCV_LIBRARIES}")
@@ -83,10 +75,11 @@ add_message_files(
 )

 ## Generate services in the 'srv' folder
-add_service_files(
-  FILES
-  SetMarkers.srv
-)
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )

 ## Generate actions in the 'action' folder
 # add_action_files(
@@ -118,8 +111,7 @@ generate_messages(

 ## Generate dynamic reconfigure parameters in the 'cfg' folder
 generate_dynamic_reconfigure_options(
-  cfg/Detector.cfg
-  cfg/Map.cfg
+  cfg/DetectorParams.cfg
 )

 ###################################
@@ -180,13 +172,6 @@ target_link_libraries(aruco_pose
  ${OpenCV_LIBRARIES}
 )

-# Prevent aruco_pose from having undefined symbols
-set_property(TARGET aruco_pose
-  APPEND
-  PROPERTY LINK_FLAGS
-  -Wl,--no-undefined
-)
-
 #############
 ## Install ##
 #############
@@ -202,11 +187,11 @@ set_property(TARGET aruco_pose
 # )

 ## Mark executables and/or libraries for installation
-install(TARGETS ${PROJECT_NAME}
-  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-)
+# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )

 ## Mark cpp header files for installation
 # install(DIRECTORY include/${PROJECT_NAME}/
@@ -222,14 +207,6 @@ install(TARGETS ${PROJECT_NAME}
 #   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 # )

-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 ##
 #############
--- a/aruco_pose/README.md
+++ b/aruco_pose/README.md
@@ -51,7 +51,6 @@ 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

@@ -71,11 +70,10 @@ 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` – known tilt (pitch and roll) of markers map as a frame
+* `~known_tilt` – debug image width
 * `~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:
@@ -99,7 +97,6 @@ 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
--- a/aruco_pose/cfg/DetectorParams.cfg
+++ b/aruco_pose/cfg/DetectorParams.cfg
@@ -8,10 +8,6 @@ p = cv2.aruco.DetectorParameters_create()

 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)
--- a/aruco_pose/cfg/Map.cfg
+++ b/aruco_pose/cfg/Map.cfg
@@ -1,14 +0,0 @@
-#!/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"))
--- a/aruco_pose/map/cmit.txt
+++ b/aruco_pose/map/cmit.txt
@@ -1,4 +1,3 @@
-# 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	
--- a/aruco_pose/map/office.txt
+++ b/aruco_pose/map/office.txt
@@ -1,4 +1,3 @@
-# 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
--- a/aruco_pose/map/office_ceiling.txt
+++ b/aruco_pose/map/office_ceiling.txt
@@ -1,4 +1,3 @@
-# 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
--- a/aruco_pose/package.xml
+++ b/aruco_pose/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
  <name>aruco_pose</name>
-  <version>0.23.0</version>
+  <version>0.0.1</version>
  <description>Positioning with ArUco markers</description>

  <maintainer email="okalachev@gmail.com">Oleg Kalachev</maintainer>
--- a/aruco_pose/src/aruco_detect.cpp
+++ b/aruco_pose/src/aruco_detect.cpp
@@ -48,7 +48,6 @@
 #include <aruco_pose/Marker.h>
 #include <aruco_pose/MarkerArray.h>
 #include <aruco_pose/DetectorConfig.h>
-#include <aruco_pose/SetMarkers.h>

 #include "utils.h"
 #include <memory>
@@ -63,17 +62,14 @@ private:
 	std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
 	std::unique_ptr<tf2_ros::TransformListener> tf_listener_;
 	std::shared_ptr<dynamic_reconfigure::Server<aruco_pose::DetectorConfig>> dyn_srv_;
-	bool enabled_ = true;
 	cv::Ptr<cv::aruco::Dictionary> dictionary_;
 	cv::Ptr<cv::aruco::DetectorParameters> parameters_;
 	image_transport::Publisher debug_pub_;
 	image_transport::CameraSubscriber img_sub_;
 	ros::Publisher markers_pub_, vis_markers_pub_;
 	ros::Subscriber map_markers_sub_;
-	ros::ServiceServer set_markers_srv_;
 	bool estimate_poses_, send_tf_, auto_flip_;
 	double length_;
-	ros::Duration transform_timeout_;
 	std::unordered_map<int, double> length_override_;
 	std::string frame_id_prefix_, known_tilt_;
 	Mat camera_matrix_, dist_coeffs_;
@@ -100,7 +96,6 @@ public:
 			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", "");
 		auto_flip_ = nh_priv_.param("auto_flip", false);
@@ -115,16 +110,17 @@ public:
 		image_transport::ImageTransport it(nh_);
 		image_transport::ImageTransport it_priv(nh_priv_);

-		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);
-
+		map_markers_sub_ = nh_.subscribe("map_markers", 1, &ArucoDetect::mapMarkersCallback, 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);
 		img_sub_ = it.subscribeCamera("image_raw", 1, &ArucoDetect::imageCallback, this);
-		map_markers_sub_ = nh_.subscribe("map_markers", 1, &ArucoDetect::mapMarkersCallback, this);
+
+		dyn_srv_ = std::make_shared<dynamic_reconfigure::Server<aruco_pose::DetectorConfig>>(nh_priv_);
+		dynamic_reconfigure::Server<aruco_pose::DetectorConfig>::CallbackType cb;
+
+		cb = std::bind(&ArucoDetect::paramCallback, this, std::placeholders::_1, std::placeholders::_2);
+		dyn_srv_->setCallback(cb);

 		NODELET_INFO("ready");
 	}
@@ -132,8 +128,6 @@ public:
 private:
 	void imageCallback(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr &cinfo)
 	{
-		if (!enabled_) return;
-
 		Mat image = cv_bridge::toCvShare(msg, "bgr8")->image;

 		vector<int> ids;
@@ -178,7 +172,7 @@ private:
 				if (!known_tilt_.empty()) {
 					try {
 						snap_to = tf_buffer_->lookupTransform(msg->header.frame_id, known_tilt_,
-						                                      msg->header.stamp, transform_timeout_);
+						                                      msg->header.stamp, ros::Duration(0.02));
 					} catch (const tf2::TransformException& e) {
 						NODELET_WARN_THROTTLE(5, "can't snap: %s", e.what());
 					}
@@ -352,29 +346,6 @@ 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();
@@ -385,8 +356,6 @@ private:

 	void paramCallback(aruco_pose::DetectorConfig &config, uint32_t level)
 	{
-		enabled_ = config.enabled && config.length > 0;
-		length_ = config.length;
 		parameters_->adaptiveThreshConstant = config.adaptiveThreshConstant;
 		parameters_->adaptiveThreshWinSizeMin = config.adaptiveThreshWinSizeMin;
 		parameters_->adaptiveThreshWinSizeMax = config.adaptiveThreshWinSizeMax;
--- a/aruco_pose/src/aruco_map.cpp
+++ b/aruco_pose/src/aruco_map.cpp
@@ -19,13 +19,11 @@
 #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>
@@ -43,7 +41,6 @@

 #include <aruco_pose/MarkerArray.h>
 #include <aruco_pose/Marker.h>
-#include <aruco_pose/MapConfig.h>

 #include <opencv2/opencv.hpp>
 #include <opencv2/aruco.hpp>
@@ -77,9 +74,6 @@ 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_;
 	int image_width_, image_height_, image_margin_;
@@ -95,14 +89,15 @@ 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>("map", 1, true);
+		markers_pub_ = nh_priv_.advertise<aruco_pose::MarkerArray>("markers", 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);

-		type_ = nh_priv_.param<std::string>("type", "map");
+		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", "");
 		auto_flip_ = nh_priv_.param("auto_flip", false);
@@ -115,13 +110,13 @@ public:

 		// createStripLine();

-		if (type_ == "map") {
-			map_ = nh_priv_.param<std::string>("map" , "");
-			loadMap(map_);
-		} else if (type_ == "gridboard") {
+		if (type == "map") {
+			param(nh_priv_, "map", map);
+			loadMap(map);
+		} 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();
 		}

@@ -129,8 +124,6 @@ public:
 		vis_markers_pub_ = nh_priv_.advertise<visualization_msgs::MarkerArray>("visualization", 1, true);
 		debug_pub_ = it_priv.advertise("debug", 1);

-		publishMap();
-
 		image_sub_.subscribe(nh_, "image_raw", 1);
 		info_sub_.subscribe(nh_, "camera_info", 1);
 		markers_sub_.subscribe(nh_, "markers", 1);
@@ -138,11 +131,10 @@ 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);
+		publishMarkersFrames();
+		publishMarkers();
+		publishMapImage();
+		vis_markers_pub_.publish(vis_array_);

 		NODELET_INFO("ready");
 	}
@@ -151,9 +143,6 @@ 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;
@@ -279,17 +268,9 @@ publish_debug:
 		std::ifstream f(filename);
 		std::string line;

-		clearMarkers();
-
-		if (map_ == "") {
-			NODELET_INFO("No map loaded");
-			return;
-		}
-
 		if (!f.good()) {
-			NODELET_ERROR("%s - %s", strerror(errno), filename.c_str());
-			map_ = "";
-			return;
+			NODELET_FATAL("%s - %s", strerror(errno), filename.c_str());
+			ros::shutdown();
 		}

 		while (std::getline(f, line)) {
@@ -315,10 +296,9 @@ publish_debug:
 				s.putback(first);
 			} else {
 				// Probably garbage data; inform user and throw an exception, possibly killing nodelet
-				NODELET_ERROR("Malformed input: %s", line.c_str());
-				map_ = "";
-				clearMarkers();
-				return;
+				NODELET_FATAL("Malformed input: %s", line.c_str());
+				ros::shutdown();
+				throw std::runtime_error("Malformed input");
 			}

 			if (!(s >> id >> length >> x >> y)) {
@@ -349,14 +329,6 @@ 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");
@@ -398,15 +370,6 @@ 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;
@@ -546,22 +509,6 @@ 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)
--- a/aruco_pose/src/draw.cpp
+++ b/aruco_pose/src/draw.cpp
@@ -3,11 +3,26 @@

 #include "draw.h"
 #include <math.h>
-#include <vector>

 using namespace cv;
 using namespace cv::aruco;

+static void _cvProjectPoints2( const CvMat* object_points, const CvMat* rotation_vector,
+                       const CvMat* translation_vector, const CvMat* camera_matrix,
+                       const CvMat* distortion_coeffs, CvMat* image_points,
+                       CvMat* dpdrot CV_DEFAULT(NULL), CvMat* dpdt CV_DEFAULT(NULL),
+                       CvMat* dpdf CV_DEFAULT(NULL), CvMat* dpdc CV_DEFAULT(NULL),
+                       CvMat* dpddist CV_DEFAULT(NULL),
+                       double aspect_ratio CV_DEFAULT(0));
+
+static void _projectPoints( InputArray objectPoints,
+                    InputArray rvec, InputArray tvec,
+                    InputArray cameraMatrix, InputArray distCoeffs,
+                    OutputArray imagePoints,
+                    OutputArray jacobian = noArray(),
+                    double aspectRatio = 0 );
+
+
 void _drawPlanarBoard(Board *_board, Size outSize, OutputArray _img, int marginSize,
                      int borderBits, bool drawAxis) {

@@ -127,194 +142,35 @@ void _drawPlanarBoard(Board *_board, Size outSize, OutputArray _img, int marginS
 	}
 }

-/**
- * @brief Convert point coordinates from world space to camera space.
- *
- * @param points A vector of points in world space.
- * @param rvec Rotation matrix or Rodrigues rotation vector.
- * @param tvec Translation vector from world to camera space.
- *
- * @return A vector of points in camera space.
- */
-template<typename CvPointType>
-static std::vector<CvPointType> worldToCamera(const std::vector<CvPointType>& points,
-                                              const cv::Mat& rvec, const cv::Mat& tvec)
+/* Draw a (potentially partially visible) line. */
+static void linePartial(InputOutputArray image, Point3f p1, Point3f p2, const Scalar& color,
+                        int thickness = 1, int lineType = LINE_8, int shift = 0)
 {
-	// We operate with CV_64F matrices internally to avoid precision loss
-	cv::Mat rvec_64f;
-	cv::Mat tvec_64f;
-	rvec.convertTo(rvec_64f, CV_64F);
-	tvec.convertTo(tvec_64f, CV_64F);
-
-	// Convert Rodrigues vector to rotation matrix
-	cv::Mat rmat;
-	if ((rvec_64f.cols == 3 && rvec_64f.rows == 1) ||
-	    (rvec_64f.cols == 1 && rvec_64f.rows == 3))
-	{
-		Rodrigues(rvec_64f, rmat);
+	// If both points are behind the screen, don't draw anything
+	if (p1.z <= 0 && p2.z <= 0) {
+		return;
 	}
-	else
-	{
-		rmat = rvec_64f.clone();
+	Point2f p1p{p1.x, p1.y};
+	Point2f p2p{p2.x, p2.y};
+	// If points are on the different sides of the plane, compute intersection point
+	if (p1.z * p2.z < 0) {
+		// Compute intersection point with the screen
+		// We denote alpha as such:
+		// xi = (1 - alpha) * x1 + alpha * x2
+		// yi = (1 - alpha) * y1 + alpha * y2
+		// zi = (1 - alpha) * z1 + alpha * z2 = 0
+		// Thus, alpha can be expressed as
+		// alpha = z1 / (z1 - z2)
+		float alpha = p1.z / (p1.z - p2.z);
+		Point2f pi{(1 - alpha) * p1.x + alpha * p2.x, (1 - alpha) * p1.y + alpha * p2.y};
+		// Now, if z1 is negative, we draw the line from (xi, yi) to (x2, y2), else we draw from (x1, y1) to (xi, yi)
+		if (p1.z < 0) {
+			p1p = pi;
+		} else {
+			p2p = pi;
+		}
 	}
-	// Make sure tvec has a size of (3, 1)
-	if (tvec_64f.rows == 1)
-	{
-		tvec_64f = tvec_64f.t();
-	}
-	std::vector<CvPointType> result;
-	result.reserve(points.size());
-	for(const auto& point : points)
-	{
-		// Calculate point coordinates in camera frame
-		// static_casts are here to silence potential narrowing conversion warnings
-		CvPointType camPoint{
-			static_cast<decltype(CvPointType::x)>(point.x * rmat.at<double>(0,0) + point.y * rmat.at<double>(0,1) + point.z * rmat.at<double>(0,2) + tvec_64f.at<double>(0)),
-			static_cast<decltype(CvPointType::y)>(point.x * rmat.at<double>(1,0) + point.y * rmat.at<double>(1,1) + point.z * rmat.at<double>(1,2) + tvec_64f.at<double>(1)),
-			static_cast<decltype(CvPointType::z)>(point.x * rmat.at<double>(2,0) + point.y * rmat.at<double>(2,1) + point.z * rmat.at<double>(2,2) + tvec_64f.at<double>(2))
-		};
-		result.push_back(camPoint);
-	}
-	return result;
-}
-
-/**
- * @brief Project points from camera space to screen space, applying distortion in the process.
- *
- * @param points A vector of points in camera space.
- * @param cameraMatrix OpenCV intrinsic camera matrix.
- * @param distCoeffs OpenCV distortion model coefficients.
- *
- * @return A vector of points in screen space.
- */
-template<typename CvPointType>
-static std::vector<CvPointType> cameraToScreen(const std::vector<CvPointType>& points,
-                                               const cv::Mat& cameraMatrix,
-                                               const cv::Mat& distCoeffs)
-{
-	// We operate with CV_64F matrices internally to avoid precision loss
-	cv::Mat cm_64f; // camera matrix, CV_64F
-	cv::Mat dc_64f; // distortion coefficients, CV_64F
-	cameraMatrix.convertTo(cm_64f, CV_64F);
-	distCoeffs.convertTo(dc_64f, CV_64F);
-
-	// Make sure distortion vector has a size of (N, 1)
-	if (dc_64f.rows == 1)
-	{
-		dc_64f = dc_64f.t();
-	}
-
-	// We will always use 12 distortion coefficients,
-	// and we can safely pad missing ones with zeroes
-	dc_64f.resize(12, 0.0);
-
-	std::vector<CvPointType> result;
-	result.reserve(points.size());
-
-	for(const auto& point : points)
-	{
-		// Apply perspective projection, preserving initial Z coordinate
-		// Always use double-precision
-		cv::Point3d camPoint{
-			point.x / point.z,
-			point.y / point.z,
-			point.z
-		};
-
-		// Apply distortion
-		// Note that we do not consider tilted sensor distortion
-		// r^2 - distance from the image center squared
-		double r2 = camPoint.x * camPoint.x + camPoint.y * camPoint.y;
-		// r^4 - same, but to the 4th power
-		double r4 = r2 * r2;
-		// r^6 - same, but to the 6th power
-		double r6 = r4 * r2;
-		// tg1 - first tangential shift factor (2 * x * y)
-		double tg1 = 2 * camPoint.x * camPoint.y;
-		// tg2 - second tangential shift factor (r^2 + 2 * x^2)
-		double tg2 = r2 + 2 * camPoint.x * camPoint.x;
-		// tg3 - third tangential shift factor (r^2 + 2 * y^2)
-		double tg3 = r2 + 2 * camPoint.y * camPoint.y;
-		// polynomial distortion factor (numerator)
-		double pndist = 1 + dc_64f.at<double>(0) * r2 + dc_64f.at<double>(1) * r4 + dc_64f.at<double>(4) * r6;
-		// polynomial distortion factror (denominator)
-		double pddist = 1.0 / (1 + dc_64f.at<double>(5) * r2 + dc_64f.at<double>(6) * r4 + dc_64f.at<double>(7) * r6);
-		// Distorted point coordinates (always double-precision)
-		cv::Point3d distortedPoint{
-			camPoint.x * pndist * pddist + dc_64f.at<double>(2) * tg1 + dc_64f.at<double>(3) * tg2 + dc_64f.at<double>(8) * r2 + dc_64f.at<double>(9) * r4,
-			camPoint.y * pndist * pddist + dc_64f.at<double>(2) * tg3 + dc_64f.at<double>(3) * tg1 + dc_64f.at<double>(10) * r2 + dc_64f.at<double>(11) * r4,
-			camPoint.z
-		};
-
-		// Convert to screen space
-		// We use static_cast here to silence potential warnings about narrowing conversions
-		// (we expect that to be the case)
-		CvPointType screenPoint{
-			static_cast<decltype(CvPointType::x)>(distortedPoint.x * cm_64f.at<double>(0, 0) + cm_64f.at<double>(0, 2)),
-			static_cast<decltype(CvPointType::y)>(distortedPoint.y * cm_64f.at<double>(1, 1) + cm_64f.at<double>(1, 2)),
-			static_cast<decltype(CvPointType::z)>(distortedPoint.z)
-		};
-
-		result.push_back(screenPoint);
-	}
-	return result;
-}
-
-/**
- * @brief Clip a line against a clip plane.
- *
- * This function "clips" a line (described by two points in *camera space*)
- * against a clip plane that is `clipPlaneDistance` meters away from the
- * camera focal point. If both points are further away from the focal point
- * than `clipPlaneDistance`, they will be returned unmodified. If one of the
- * points is behind the clipping plane, a point *on* the clipping plane will
- * be computed and returned as one of the points.
- *
- * If none of the points are visible, an empty vector will be returned.
- *
- * @param p1 First point on the line, in camera space.
- * @param p2 Second point on the line, in camera space.
- * @param clipPlaneDistance Distance from the focal point to the clipping plane.
- * @return A vector of zero or two points on the clipped line, in camera space.
- */
-static std::vector<Point3f> lineClip(Point3f p1, Point3f p2, float clipPlaneDistance = 0.1f)
-{
-	// We don't need to compute an intersection if both points are
-	// behind us
-	if (p1.z < clipPlaneDistance && p2.z < clipPlaneDistance)
-	{
-		return {};
-	}
-	// We don't need to compute an intersection if both points are
-	// in front of us
-	if (p1.z > clipPlaneDistance && p2.z > clipPlaneDistance)
-	{
-		return {p1, p2};
-	}
-	// We don't really want to compute an intersection if both Z coordinates
-	// are sufficiently close to each other
-	if (std::abs(p1.z - p2.z) < 0.0001) // The number here is chosen arbitrarily
-	{
-		return {p1, p2};
-	}
-	// We compute the intersection as such:
-	// zi = (1 - alpha) * p1.z + alpha * p2.z = clipPlaneDistance
-	// alpha = (p1.z - clipPlaneDistance) / (p1.z - p2.z)
-	double alpha = (p1.z - clipPlaneDistance) / (p1.z - p2.z);
-	Point3f clipPlanePoint{
-		static_cast<float>((1 - alpha) * p1.x + alpha * p2.x),
-		static_cast<float>((1 - alpha) * p1.y + alpha * p2.y),
-		clipPlaneDistance
-	};
-	if (p1.z < clipPlaneDistance)
-	{
-		return {clipPlanePoint, p2};
-	}
-	else
-	{
-		return {p1, clipPlanePoint};
-	}
-	// Unreachable?
+	line(image, p1p, p2p, color, thickness, lineType, shift);
 }

 void _drawAxis(InputOutputArray _image, InputArray _cameraMatrix, InputArray _distCoeffs,
@@ -330,23 +186,647 @@ void _drawAxis(InputOutputArray _image, InputArray _cameraMatrix, InputArray _di
 	axisPoints.push_back(Point3f(length, 0, 0));
 	axisPoints.push_back(Point3f(0, length, 0));
 	axisPoints.push_back(Point3f(0, 0, length));
-	auto camAxisPoints = worldToCamera(axisPoints, _rvec.getMat(), _tvec.getMat());
-	auto axisX = cameraToScreen(lineClip(camAxisPoints[0], camAxisPoints[1]), _cameraMatrix.getMat(), _distCoeffs.getMat());
-	auto axisY = cameraToScreen(lineClip(camAxisPoints[0], camAxisPoints[2]), _cameraMatrix.getMat(), _distCoeffs.getMat());
-	auto axisZ = cameraToScreen(lineClip(camAxisPoints[0], camAxisPoints[3]), _cameraMatrix.getMat(), _distCoeffs.getMat());
-	if (axisX.size() > 0)
-	{
-		line(_image, Point2f{axisX[0].x, axisX[0].y}, Point2f{axisX[1].x, axisX[1].y},
-			Scalar(0, 0, 255), 3);
-	}
-	if (axisY.size() > 0)
-	{
-		line(_image, Point2f{axisY[0].x, axisY[0].y}, Point2f{axisY[1].x, axisY[1].y},
-			Scalar(0, 255, 0), 3);
-	}
-	if (axisZ.size() > 0)
-	{
-		line(_image, Point2f{axisZ[0].x, axisZ[0].y}, Point2f{axisZ[1].x, axisZ[1].y},
-			Scalar(255, 0, 0), 3);
-	}
+	std::vector<Point3f> imagePointsZ;
+	_projectPoints(axisPoints, _rvec, _tvec, _cameraMatrix, _distCoeffs, imagePointsZ);
+
+	// draw axis lines
+	linePartial(_image, imagePointsZ[0], imagePointsZ[1], Scalar(0, 0, 255), 3);
+	linePartial(_image, imagePointsZ[0], imagePointsZ[2], Scalar(0, 255, 0), 3);
+	linePartial(_image, imagePointsZ[0], imagePointsZ[3], Scalar(255, 0, 0), 3);
+}
+
+static CvMat _cvMat(const cv::Mat& m)
+{
+	CvMat self;
+	CV_DbgAssert(m.dims <= 2);
+	self = cvMat(m.rows, m.dims == 1 ? 1 : m.cols, m.type(), m.data);
+	self.step = (int)m.step[0];
+	self.type = (self.type & ~cv::Mat::CONTINUOUS_FLAG) | (m.flags & cv::Mat::CONTINUOUS_FLAG);
+	return self;
+}
+
+static void _projectPoints( InputArray _opoints,
+                        InputArray _rvec,
+                        InputArray _tvec,
+                        InputArray _cameraMatrix,
+                        InputArray _distCoeffs,
+                        OutputArray _ipoints,
+                        OutputArray _jacobian,
+                        double aspectRatio )
+{
+	Mat opoints = _opoints.getMat();
+	int npoints = opoints.checkVector(3), depth = opoints.depth();
+	CV_Assert(npoints >= 0 && (depth == CV_32F || depth == CV_64F));
+
+	CvMat dpdrot, dpdt, dpdf, dpdc, dpddist;
+	CvMat *pdpdrot = 0, *pdpdt = 0, *pdpdf = 0, *pdpdc = 0, *pdpddist = 0;
+
+	CV_Assert(_ipoints.needed());
+
+	_ipoints.create(npoints, 1, CV_MAKETYPE(depth, 3), -1, true);
+	Mat imagePoints = _ipoints.getMat();
+	CvMat c_imagePoints = _cvMat(imagePoints);
+	CvMat c_objectPoints = _cvMat(opoints);
+	Mat cameraMatrix = _cameraMatrix.getMat();
+
+	Mat rvec = _rvec.getMat(), tvec = _tvec.getMat();
+	CvMat c_cameraMatrix = _cvMat(cameraMatrix);
+	CvMat c_rvec = _cvMat(rvec), c_tvec = _cvMat(tvec);
+
+	double dc0buf[5] = {0};
+	Mat dc0(5, 1, CV_64F, dc0buf);
+	Mat distCoeffs = _distCoeffs.getMat();
+	if (distCoeffs.empty())
+		distCoeffs = dc0;
+	CvMat c_distCoeffs = _cvMat(distCoeffs);
+	int ndistCoeffs = distCoeffs.rows + distCoeffs.cols - 1;
+
+	Mat jacobian;
+	if (_jacobian.needed())
+	{
+		_jacobian.create(npoints * 2, 3 + 3 + 2 + 2 + ndistCoeffs, CV_64F);
+		jacobian = _jacobian.getMat();
+		pdpdrot = &(dpdrot = _cvMat(jacobian.colRange(0, 3)));
+		pdpdt = &(dpdt = _cvMat(jacobian.colRange(3, 6)));
+		pdpdf = &(dpdf = _cvMat(jacobian.colRange(6, 8)));
+		pdpdc = &(dpdc = _cvMat(jacobian.colRange(8, 10)));
+		pdpddist = &(dpddist = _cvMat(jacobian.colRange(10, 10 + ndistCoeffs)));
+	}
+
+	_cvProjectPoints2(&c_objectPoints, &c_rvec, &c_tvec, &c_cameraMatrix, &c_distCoeffs,
+					  &c_imagePoints, pdpdrot, pdpdt, pdpdf, pdpdc, pdpddist, aspectRatio);
+}
+
+namespace _detail
+{
+    template <typename FLOAT>
+    void computeTiltProjectionMatrix(FLOAT tauX,
+                                     FLOAT tauY,
+                                     Matx<FLOAT, 3, 3>* matTilt = 0,
+                                     Matx<FLOAT, 3, 3>* dMatTiltdTauX = 0,
+                                     Matx<FLOAT, 3, 3>* dMatTiltdTauY = 0,
+                                     Matx<FLOAT, 3, 3>* invMatTilt = 0)
+    {
+        FLOAT cTauX = cos(tauX);
+        FLOAT sTauX = sin(tauX);
+        FLOAT cTauY = cos(tauY);
+        FLOAT sTauY = sin(tauY);
+        Matx<FLOAT, 3, 3> matRotX = Matx<FLOAT, 3, 3>(1,0,0,0,cTauX,sTauX,0,-sTauX,cTauX);
+        Matx<FLOAT, 3, 3> matRotY = Matx<FLOAT, 3, 3>(cTauY,0,-sTauY,0,1,0,sTauY,0,cTauY);
+        Matx<FLOAT, 3, 3> matRotXY = matRotY * matRotX;
+        Matx<FLOAT, 3, 3> matProjZ = Matx<FLOAT, 3, 3>(matRotXY(2,2),0,-matRotXY(0,2),0,matRotXY(2,2),-matRotXY(1,2),0,0,1);
+        if (matTilt)
+        {
+            // Matrix for trapezoidal distortion of tilted image sensor
+            *matTilt = matProjZ * matRotXY;
+        }
+        if (dMatTiltdTauX)
+        {
+            // Derivative with respect to tauX
+            Matx<FLOAT, 3, 3> dMatRotXYdTauX = matRotY * Matx<FLOAT, 3, 3>(0,0,0,0,-sTauX,cTauX,0,-cTauX,-sTauX);
+            Matx<FLOAT, 3, 3> dMatProjZdTauX = Matx<FLOAT, 3, 3>(dMatRotXYdTauX(2,2),0,-dMatRotXYdTauX(0,2),
+                                                                 0,dMatRotXYdTauX(2,2),-dMatRotXYdTauX(1,2),0,0,0);
+            *dMatTiltdTauX = (matProjZ * dMatRotXYdTauX) + (dMatProjZdTauX * matRotXY);
+        }
+        if (dMatTiltdTauY)
+        {
+            // Derivative with respect to tauY
+            Matx<FLOAT, 3, 3> dMatRotXYdTauY = Matx<FLOAT, 3, 3>(-sTauY,0,-cTauY,0,0,0,cTauY,0,-sTauY) * matRotX;
+            Matx<FLOAT, 3, 3> dMatProjZdTauY = Matx<FLOAT, 3, 3>(dMatRotXYdTauY(2,2),0,-dMatRotXYdTauY(0,2),
+                                                                 0,dMatRotXYdTauY(2,2),-dMatRotXYdTauY(1,2),0,0,0);
+            *dMatTiltdTauY = (matProjZ * dMatRotXYdTauY) + (dMatProjZdTauY * matRotXY);
+        }
+        if (invMatTilt)
+        {
+            FLOAT inv = 1./matRotXY(2,2);
+            Matx<FLOAT, 3, 3> invMatProjZ = Matx<FLOAT, 3, 3>(inv,0,inv*matRotXY(0,2),0,inv,inv*matRotXY(1,2),0,0,1);
+            *invMatTilt = matRotXY.t()*invMatProjZ;
+        }
+    }
+}
+
+static const char* cvDistCoeffErr = "Distortion coefficients must be 1x4, 4x1, 1x5, 5x1, 1x8, 8x1, 1x12, 12x1, 1x14 or 14x1 floating-point vector";
+
+static void _cvProjectPoints2Internal( const CvMat* objectPoints,
+                                      const CvMat* r_vec,
+                                      const CvMat* t_vec,
+                                      const CvMat* A,
+                                      const CvMat* distCoeffs,
+                                      CvMat* imagePoints, CvMat* dpdr CV_DEFAULT(NULL),
+                                      CvMat* dpdt CV_DEFAULT(NULL), CvMat* dpdf CV_DEFAULT(NULL),
+                                      CvMat* dpdc CV_DEFAULT(NULL), CvMat* dpdk CV_DEFAULT(NULL),
+                                      CvMat* dpdo CV_DEFAULT(NULL),
+                                      double aspectRatio CV_DEFAULT(0) )
+{
+    Ptr<CvMat> matM, _m;
+    Ptr<CvMat> _dpdr, _dpdt, _dpdc, _dpdf, _dpdk;
+    Ptr<CvMat> _dpdo;
+
+    int i, j, count;
+    int calc_derivatives;
+    const CvPoint3D64f* M;
+    CvPoint3D64f* m;
+    double r[3], R[9], dRdr[27], t[3], a[9], k[14] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, fx, fy, cx, cy;
+    Matx33d matTilt = Matx33d::eye();
+    Matx33d dMatTiltdTauX(0,0,0,0,0,0,0,-1,0);
+    Matx33d dMatTiltdTauY(0,0,0,0,0,0,1,0,0);
+    CvMat _r, _t, _a = cvMat( 3, 3, CV_64F, a ), _k;
+    CvMat matR = cvMat( 3, 3, CV_64F, R ), _dRdr = cvMat( 3, 9, CV_64F, dRdr );
+    double *dpdr_p = 0, *dpdt_p = 0, *dpdk_p = 0, *dpdf_p = 0, *dpdc_p = 0;
+    double* dpdo_p = 0;
+    int dpdr_step = 0, dpdt_step = 0, dpdk_step = 0, dpdf_step = 0, dpdc_step = 0;
+    int dpdo_step = 0;
+    bool fixedAspectRatio = aspectRatio > FLT_EPSILON;
+
+    if( !CV_IS_MAT(objectPoints) || !CV_IS_MAT(r_vec) ||
+        !CV_IS_MAT(t_vec) || !CV_IS_MAT(A) ||
+        /*!CV_IS_MAT(distCoeffs) ||*/ !CV_IS_MAT(imagePoints) )
+        CV_Error( CV_StsBadArg, "One of required arguments is not a valid matrix" );
+
+    int total = objectPoints->rows * objectPoints->cols * CV_MAT_CN(objectPoints->type);
+    if(total % 3 != 0)
+    {
+        //we have stopped support of homogeneous coordinates because it cause ambiguity in interpretation of the input data
+        CV_Error( CV_StsBadArg, "Homogeneous coordinates are not supported" );
+    }
+    count = total / 3;
+
+    if( CV_IS_CONT_MAT(objectPoints->type) &&
+        (CV_MAT_DEPTH(objectPoints->type) == CV_32F || CV_MAT_DEPTH(objectPoints->type) == CV_64F)&&
+        ((objectPoints->rows == 1 && CV_MAT_CN(objectPoints->type) == 3) ||
+         (objectPoints->rows == count && CV_MAT_CN(objectPoints->type)*objectPoints->cols == 3) ||
+         (objectPoints->rows == 3 && CV_MAT_CN(objectPoints->type) == 1 && objectPoints->cols == count)))
+    {
+        matM.reset(cvCreateMat( objectPoints->rows, objectPoints->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(objectPoints->type)) ));
+        cvConvert(objectPoints, matM);
+    }
+    else
+    {
+//        matM = cvCreateMat( 1, count, CV_64FC3 );
+//        cvConvertPointsHomogeneous( objectPoints, matM );
+        CV_Error( CV_StsBadArg, "Homogeneous coordinates are not supported" );
+    }
+
+    if( CV_IS_CONT_MAT(imagePoints->type) &&
+        (CV_MAT_DEPTH(imagePoints->type) == CV_32F || CV_MAT_DEPTH(imagePoints->type) == CV_64F) &&
+        ((imagePoints->rows == 1 && CV_MAT_CN(imagePoints->type) == 3) ||
+         (imagePoints->rows == count && CV_MAT_CN(imagePoints->type)*imagePoints->cols == 3) ||
+         (imagePoints->rows == 3 && CV_MAT_CN(imagePoints->type) == 1 && imagePoints->cols == count)))
+    {
+        _m.reset(cvCreateMat( imagePoints->rows, imagePoints->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(imagePoints->type)) ));
+        cvConvert(imagePoints, _m);
+    }
+    else
+    {
+//        _m = cvCreateMat( 1, count, CV_64FC2 );
+        CV_Error( CV_StsBadArg, "Homogeneous coordinates are not supported" );
+    }
+
+    M = (CvPoint3D64f*)matM->data.db;
+    m = (CvPoint3D64f*)_m->data.db;
+
+    if( (CV_MAT_DEPTH(r_vec->type) != CV_64F && CV_MAT_DEPTH(r_vec->type) != CV_32F) ||
+        (((r_vec->rows != 1 && r_vec->cols != 1) ||
+          r_vec->rows*r_vec->cols*CV_MAT_CN(r_vec->type) != 3) &&
+         ((r_vec->rows != 3 && r_vec->cols != 3) || CV_MAT_CN(r_vec->type) != 1)))
+        CV_Error( CV_StsBadArg, "Rotation must be represented by 1x3 or 3x1 "
+                                "floating-point rotation vector, or 3x3 rotation matrix" );
+
+    if( r_vec->rows == 3 && r_vec->cols == 3 )
+    {
+        _r = cvMat( 3, 1, CV_64FC1, r );
+        cvRodrigues2( r_vec, &_r );
+        cvRodrigues2( &_r, &matR, &_dRdr );
+        cvCopy( r_vec, &matR );
+    }
+    else
+    {
+        _r = cvMat( r_vec->rows, r_vec->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(r_vec->type)), r );
+        cvConvert( r_vec, &_r );
+        cvRodrigues2( &_r, &matR, &_dRdr );
+    }
+
+    if( (CV_MAT_DEPTH(t_vec->type) != CV_64F && CV_MAT_DEPTH(t_vec->type) != CV_32F) ||
+        (t_vec->rows != 1 && t_vec->cols != 1) ||
+        t_vec->rows*t_vec->cols*CV_MAT_CN(t_vec->type) != 3 )
+        CV_Error( CV_StsBadArg,
+                  "Translation vector must be 1x3 or 3x1 floating-point vector" );
+
+    _t = cvMat( t_vec->rows, t_vec->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(t_vec->type)), t );
+    cvConvert( t_vec, &_t );
+
+    if( (CV_MAT_TYPE(A->type) != CV_64FC1 && CV_MAT_TYPE(A->type) != CV_32FC1) ||
+        A->rows != 3 || A->cols != 3 )
+        CV_Error( CV_StsBadArg, "Instrinsic parameters must be 3x3 floating-point matrix" );
+
+    cvConvert( A, &_a );
+    fx = a[0]; fy = a[4];
+    cx = a[2]; cy = a[5];
+
+    if( fixedAspectRatio )
+        fx = fy*aspectRatio;
+
+    if( distCoeffs )
+    {
+        if( !CV_IS_MAT(distCoeffs) ||
+            (CV_MAT_DEPTH(distCoeffs->type) != CV_64F &&
+             CV_MAT_DEPTH(distCoeffs->type) != CV_32F) ||
+            (distCoeffs->rows != 1 && distCoeffs->cols != 1) ||
+            (distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 4 &&
+             distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 5 &&
+             distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 8 &&
+             distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 12 &&
+             distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 14) )
+            CV_Error( CV_StsBadArg, cvDistCoeffErr );
+
+        _k = cvMat( distCoeffs->rows, distCoeffs->cols,
+                    CV_MAKETYPE(CV_64F,CV_MAT_CN(distCoeffs->type)), k );
+        cvConvert( distCoeffs, &_k );
+        if(k[12] != 0 || k[13] != 0)
+        {
+            _detail::computeTiltProjectionMatrix(k[12], k[13],
+                                                &matTilt, &dMatTiltdTauX, &dMatTiltdTauY);
+        }
+    }
+
+    if( dpdr )
+    {
+        if( !CV_IS_MAT(dpdr) ||
+            (CV_MAT_TYPE(dpdr->type) != CV_32FC1 &&
+             CV_MAT_TYPE(dpdr->type) != CV_64FC1) ||
+            dpdr->rows != count*2 || dpdr->cols != 3 )
+            CV_Error( CV_StsBadArg, "dp/drot must be 2Nx3 floating-point matrix" );
+
+        if( CV_MAT_TYPE(dpdr->type) == CV_64FC1 )
+        {
+            _dpdr.reset(cvCloneMat(dpdr));
+        }
+        else
+            _dpdr.reset(cvCreateMat( 2*count, 3, CV_64FC1 ));
+        dpdr_p = _dpdr->data.db;
+        dpdr_step = _dpdr->step/sizeof(dpdr_p[0]);
+    }
+
+    if( dpdt )
+    {
+        if( !CV_IS_MAT(dpdt) ||
+            (CV_MAT_TYPE(dpdt->type) != CV_32FC1 &&
+             CV_MAT_TYPE(dpdt->type) != CV_64FC1) ||
+            dpdt->rows != count*2 || dpdt->cols != 3 )
+            CV_Error( CV_StsBadArg, "dp/dT must be 2Nx3 floating-point matrix" );
+
+        if( CV_MAT_TYPE(dpdt->type) == CV_64FC1 )
+        {
+            _dpdt.reset(cvCloneMat(dpdt));
+        }
+        else
+            _dpdt.reset(cvCreateMat( 2*count, 3, CV_64FC1 ));
+        dpdt_p = _dpdt->data.db;
+        dpdt_step = _dpdt->step/sizeof(dpdt_p[0]);
+    }
+
+    if( dpdf )
+    {
+        if( !CV_IS_MAT(dpdf) ||
+            (CV_MAT_TYPE(dpdf->type) != CV_32FC1 && CV_MAT_TYPE(dpdf->type) != CV_64FC1) ||
+            dpdf->rows != count*2 || dpdf->cols != 2 )
+            CV_Error( CV_StsBadArg, "dp/df must be 2Nx2 floating-point matrix" );
+
+        if( CV_MAT_TYPE(dpdf->type) == CV_64FC1 )
+        {
+            _dpdf.reset(cvCloneMat(dpdf));
+        }
+        else
+            _dpdf.reset(cvCreateMat( 2*count, 2, CV_64FC1 ));
+        dpdf_p = _dpdf->data.db;
+        dpdf_step = _dpdf->step/sizeof(dpdf_p[0]);
+    }
+
+    if( dpdc )
+    {
+        if( !CV_IS_MAT(dpdc) ||
+            (CV_MAT_TYPE(dpdc->type) != CV_32FC1 && CV_MAT_TYPE(dpdc->type) != CV_64FC1) ||
+            dpdc->rows != count*2 || dpdc->cols != 2 )
+            CV_Error( CV_StsBadArg, "dp/dc must be 2Nx2 floating-point matrix" );
+
+        if( CV_MAT_TYPE(dpdc->type) == CV_64FC1 )
+        {
+            _dpdc.reset(cvCloneMat(dpdc));
+        }
+        else
+            _dpdc.reset(cvCreateMat( 2*count, 2, CV_64FC1 ));
+        dpdc_p = _dpdc->data.db;
+        dpdc_step = _dpdc->step/sizeof(dpdc_p[0]);
+    }
+
+    if( dpdk )
+    {
+        if( !CV_IS_MAT(dpdk) ||
+            (CV_MAT_TYPE(dpdk->type) != CV_32FC1 && CV_MAT_TYPE(dpdk->type) != CV_64FC1) ||
+            dpdk->rows != count*2 || (dpdk->cols != 14 && dpdk->cols != 12 && dpdk->cols != 8 && dpdk->cols != 5 && dpdk->cols != 4 && dpdk->cols != 2) )
+            CV_Error( CV_StsBadArg, "dp/df must be 2Nx14, 2Nx12, 2Nx8, 2Nx5, 2Nx4 or 2Nx2 floating-point matrix" );
+
+        if( !distCoeffs )
+            CV_Error( CV_StsNullPtr, "distCoeffs is NULL while dpdk is not" );
+
+        if( CV_MAT_TYPE(dpdk->type) == CV_64FC1 )
+        {
+            _dpdk.reset(cvCloneMat(dpdk));
+        }
+        else
+            _dpdk.reset(cvCreateMat( dpdk->rows, dpdk->cols, CV_64FC1 ));
+        dpdk_p = _dpdk->data.db;
+        dpdk_step = _dpdk->step/sizeof(dpdk_p[0]);
+    }
+
+    if( dpdo )
+    {
+        if( !CV_IS_MAT( dpdo ) || ( CV_MAT_TYPE( dpdo->type ) != CV_32FC1
+                                    && CV_MAT_TYPE( dpdo->type ) != CV_64FC1 )
+            || dpdo->rows != count * 2 || dpdo->cols != count * 3 )
+            CV_Error( CV_StsBadArg, "dp/do must be 2Nx3N floating-point matrix" );
+
+        if( CV_MAT_TYPE( dpdo->type ) == CV_64FC1 )
+        {
+            _dpdo.reset( cvCloneMat( dpdo ) );
+        }
+        else
+            _dpdo.reset( cvCreateMat( 2 * count, 3 * count, CV_64FC1 ) );
+        cvZero(_dpdo);
+        dpdo_p = _dpdo->data.db;
+        dpdo_step = _dpdo->step / sizeof( dpdo_p[0] );
+    }
+
+    calc_derivatives = dpdr || dpdt || dpdf || dpdc || dpdk || dpdo;
+
+    for( i = 0; i < count; i++ )
+    {
+        double X = M[i].x, Y = M[i].y, Z = M[i].z;
+        double x = R[0]*X + R[1]*Y + R[2]*Z + t[0];
+        double y = R[3]*X + R[4]*Y + R[5]*Z + t[1];
+        double z = R[6]*X + R[7]*Y + R[8]*Z + t[2];
+        double r2, r4, r6, a1, a2, a3, cdist, icdist2;
+        double xd, yd, xd0, yd0, invProj;
+        Vec3d vecTilt;
+        Vec3d dVecTilt;
+        Matx22d dMatTilt;
+        Vec2d dXdYd;
+
+        double z0 = z;
+        z = z ? 1./z : 1;
+        x *= z; y *= z;
+
+        r2 = x*x + y*y;
+        r4 = r2*r2;
+        r6 = r4*r2;
+        a1 = 2*x*y;
+        a2 = r2 + 2*x*x;
+        a3 = r2 + 2*y*y;
+        cdist = 1 + k[0]*r2 + k[1]*r4 + k[4]*r6;
+        icdist2 = 1./(1 + k[5]*r2 + k[6]*r4 + k[7]*r6);
+        xd0 = x*cdist*icdist2 + k[2]*a1 + k[3]*a2 + k[8]*r2+k[9]*r4;
+        yd0 = y*cdist*icdist2 + k[2]*a3 + k[3]*a1 + k[10]*r2+k[11]*r4;
+
+        // additional distortion by projecting onto a tilt plane
+        vecTilt = matTilt*Vec3d(xd0, yd0, 1);
+        invProj = vecTilt(2) ? 1./vecTilt(2) : 1;
+        xd = invProj * vecTilt(0);
+        yd = invProj * vecTilt(1);
+
+        m[i].x = xd*fx + cx;
+        m[i].y = yd*fy + cy;
+        m[i].z = z; // Just put the projected Z coordinate here, we mainly care about the sign
+
+        if( calc_derivatives )
+        {
+            if( dpdc_p )
+            {
+                dpdc_p[0] = 1; dpdc_p[1] = 0; // dp_xdc_x; dp_xdc_y
+                dpdc_p[dpdc_step] = 0;
+                dpdc_p[dpdc_step+1] = 1;
+                dpdc_p += dpdc_step*2;
+            }
+
+            if( dpdf_p )
+            {
+                if( fixedAspectRatio )
+                {
+                    dpdf_p[0] = 0; dpdf_p[1] = xd*aspectRatio; // dp_xdf_x; dp_xdf_y
+                    dpdf_p[dpdf_step] = 0;
+                    dpdf_p[dpdf_step+1] = yd;
+                }
+                else
+                {
+                    dpdf_p[0] = xd; dpdf_p[1] = 0;
+                    dpdf_p[dpdf_step] = 0;
+                    dpdf_p[dpdf_step+1] = yd;
+                }
+                dpdf_p += dpdf_step*2;
+            }
+            for (int row = 0; row < 2; ++row)
+                for (int col = 0; col < 2; ++col)
+                    dMatTilt(row,col) = matTilt(row,col)*vecTilt(2)
+                                        - matTilt(2,col)*vecTilt(row);
+            double invProjSquare = (invProj*invProj);
+            dMatTilt *= invProjSquare;
+            if( dpdk_p )
+            {
+                dXdYd = dMatTilt*Vec2d(x*icdist2*r2, y*icdist2*r2);
+                dpdk_p[0] = fx*dXdYd(0);
+                dpdk_p[dpdk_step] = fy*dXdYd(1);
+                dXdYd = dMatTilt*Vec2d(x*icdist2*r4, y*icdist2*r4);
+                dpdk_p[1] = fx*dXdYd(0);
+                dpdk_p[dpdk_step+1] = fy*dXdYd(1);
+                if( _dpdk->cols > 2 )
+                {
+                    dXdYd = dMatTilt*Vec2d(a1, a3);
+                    dpdk_p[2] = fx*dXdYd(0);
+                    dpdk_p[dpdk_step+2] = fy*dXdYd(1);
+                    dXdYd = dMatTilt*Vec2d(a2, a1);
+                    dpdk_p[3] = fx*dXdYd(0);
+                    dpdk_p[dpdk_step+3] = fy*dXdYd(1);
+                    if( _dpdk->cols > 4 )
+                    {
+                        dXdYd = dMatTilt*Vec2d(x*icdist2*r6, y*icdist2*r6);
+                        dpdk_p[4] = fx*dXdYd(0);
+                        dpdk_p[dpdk_step+4] = fy*dXdYd(1);
+
+                        if( _dpdk->cols > 5 )
+                        {
+                            dXdYd = dMatTilt*Vec2d(
+                                    x*cdist*(-icdist2)*icdist2*r2, y*cdist*(-icdist2)*icdist2*r2);
+                            dpdk_p[5] = fx*dXdYd(0);
+                            dpdk_p[dpdk_step+5] = fy*dXdYd(1);
+                            dXdYd = dMatTilt*Vec2d(
+                                    x*cdist*(-icdist2)*icdist2*r4, y*cdist*(-icdist2)*icdist2*r4);
+                            dpdk_p[6] = fx*dXdYd(0);
+                            dpdk_p[dpdk_step+6] = fy*dXdYd(1);
+                            dXdYd = dMatTilt*Vec2d(
+                                    x*cdist*(-icdist2)*icdist2*r6, y*cdist*(-icdist2)*icdist2*r6);
+                            dpdk_p[7] = fx*dXdYd(0);
+                            dpdk_p[dpdk_step+7] = fy*dXdYd(1);
+                            if( _dpdk->cols > 8 )
+                            {
+                                dXdYd = dMatTilt*Vec2d(r2, 0);
+                                dpdk_p[8] = fx*dXdYd(0); //s1
+                                dpdk_p[dpdk_step+8] = fy*dXdYd(1); //s1
+                                dXdYd = dMatTilt*Vec2d(r4, 0);
+                                dpdk_p[9] = fx*dXdYd(0); //s2
+                                dpdk_p[dpdk_step+9] = fy*dXdYd(1); //s2
+                                dXdYd = dMatTilt*Vec2d(0, r2);
+                                dpdk_p[10] = fx*dXdYd(0);//s3
+                                dpdk_p[dpdk_step+10] = fy*dXdYd(1); //s3
+                                dXdYd = dMatTilt*Vec2d(0, r4);
+                                dpdk_p[11] = fx*dXdYd(0);//s4
+                                dpdk_p[dpdk_step+11] = fy*dXdYd(1); //s4
+                                if( _dpdk->cols > 12 )
+                                {
+                                    dVecTilt = dMatTiltdTauX * Vec3d(xd0, yd0, 1);
+                                    dpdk_p[12] = fx * invProjSquare * (
+                                            dVecTilt(0) * vecTilt(2) - dVecTilt(2) * vecTilt(0));
+                                    dpdk_p[dpdk_step+12] = fy*invProjSquare * (
+                                            dVecTilt(1) * vecTilt(2) - dVecTilt(2) * vecTilt(1));
+                                    dVecTilt = dMatTiltdTauY * Vec3d(xd0, yd0, 1);
+                                    dpdk_p[13] = fx * invProjSquare * (
+                                            dVecTilt(0) * vecTilt(2) - dVecTilt(2) * vecTilt(0));
+                                    dpdk_p[dpdk_step+13] = fy * invProjSquare * (
+                                            dVecTilt(1) * vecTilt(2) - dVecTilt(2) * vecTilt(1));
+                                }
+                            }
+                        }
+                    }
+                }
+                dpdk_p += dpdk_step*2;
+            }
+
+            if( dpdt_p )
+            {
+                double dxdt[] = { z, 0, -x*z }, dydt[] = { 0, z, -y*z };
+                for( j = 0; j < 3; j++ )
+                {
+                    double dr2dt = 2*x*dxdt[j] + 2*y*dydt[j];
+                    double dcdist_dt = k[0]*dr2dt + 2*k[1]*r2*dr2dt + 3*k[4]*r4*dr2dt;
+                    double dicdist2_dt = -icdist2*icdist2*(k[5]*dr2dt + 2*k[6]*r2*dr2dt + 3*k[7]*r4*dr2dt);
+                    double da1dt = 2*(x*dydt[j] + y*dxdt[j]);
+                    double dmxdt = (dxdt[j]*cdist*icdist2 + x*dcdist_dt*icdist2 + x*cdist*dicdist2_dt +
+                                    k[2]*da1dt + k[3]*(dr2dt + 4*x*dxdt[j]) + k[8]*dr2dt + 2*r2*k[9]*dr2dt);
+                    double dmydt = (dydt[j]*cdist*icdist2 + y*dcdist_dt*icdist2 + y*cdist*dicdist2_dt +
+                                    k[2]*(dr2dt + 4*y*dydt[j]) + k[3]*da1dt + k[10]*dr2dt + 2*r2*k[11]*dr2dt);
+                    dXdYd = dMatTilt*Vec2d(dmxdt, dmydt);
+                    dpdt_p[j] = fx*dXdYd(0);
+                    dpdt_p[dpdt_step+j] = fy*dXdYd(1);
+                }
+                dpdt_p += dpdt_step*2;
+            }
+
+            if( dpdr_p )
+            {
+                double dx0dr[] =
+                        {
+                                X*dRdr[0] + Y*dRdr[1] + Z*dRdr[2],
+                                X*dRdr[9] + Y*dRdr[10] + Z*dRdr[11],
+                                X*dRdr[18] + Y*dRdr[19] + Z*dRdr[20]
+                        };
+                double dy0dr[] =
+                        {
+                                X*dRdr[3] + Y*dRdr[4] + Z*dRdr[5],
+                                X*dRdr[12] + Y*dRdr[13] + Z*dRdr[14],
+                                X*dRdr[21] + Y*dRdr[22] + Z*dRdr[23]
+                        };
+                double dz0dr[] =
+                        {
+                                X*dRdr[6] + Y*dRdr[7] + Z*dRdr[8],
+                                X*dRdr[15] + Y*dRdr[16] + Z*dRdr[17],
+                                X*dRdr[24] + Y*dRdr[25] + Z*dRdr[26]
+                        };
+                for( j = 0; j < 3; j++ )
+                {
+                    double dxdr = z*(dx0dr[j] - x*dz0dr[j]);
+                    double dydr = z*(dy0dr[j] - y*dz0dr[j]);
+                    double dr2dr = 2*x*dxdr + 2*y*dydr;
+                    double dcdist_dr = (k[0] + 2*k[1]*r2 + 3*k[4]*r4)*dr2dr;
+                    double dicdist2_dr = -icdist2*icdist2*(k[5] + 2*k[6]*r2 + 3*k[7]*r4)*dr2dr;
+                    double da1dr = 2*(x*dydr + y*dxdr);
+                    double dmxdr = (dxdr*cdist*icdist2 + x*dcdist_dr*icdist2 + x*cdist*dicdist2_dr +
+                                    k[2]*da1dr + k[3]*(dr2dr + 4*x*dxdr) + (k[8] + 2*r2*k[9])*dr2dr);
+                    double dmydr = (dydr*cdist*icdist2 + y*dcdist_dr*icdist2 + y*cdist*dicdist2_dr +
+                                    k[2]*(dr2dr + 4*y*dydr) + k[3]*da1dr + (k[10] + 2*r2*k[11])*dr2dr);
+                    dXdYd = dMatTilt*Vec2d(dmxdr, dmydr);
+                    dpdr_p[j] = fx*dXdYd(0);
+                    dpdr_p[dpdr_step+j] = fy*dXdYd(1);
+                }
+                dpdr_p += dpdr_step*2;
+            }
+
+            if( dpdo_p )
+            {
+                double dxdo[] = { z * ( R[0] - x * z * z0 * R[6] ),
+                                  z * ( R[1] - x * z * z0 * R[7] ),
+                                  z * ( R[2] - x * z * z0 * R[8] ) };
+                double dydo[] = { z * ( R[3] - y * z * z0 * R[6] ),
+                                  z * ( R[4] - y * z * z0 * R[7] ),
+                                  z * ( R[5] - y * z * z0 * R[8] ) };
+                for( j = 0; j < 3; j++ )
+                {
+                    double dr2do = 2 * x * dxdo[j] + 2 * y * dydo[j];
+                    double dr4do = 2 * r2 * dr2do;
+                    double dr6do = 3 * r4 * dr2do;
+                    double da1do = 2 * y * dxdo[j] + 2 * x * dydo[j];
+                    double da2do = dr2do + 4 * x * dxdo[j];
+                    double da3do = dr2do + 4 * y * dydo[j];
+                    double dcdist_do
+                            = k[0] * dr2do + k[1] * dr4do + k[4] * dr6do;
+                    double dicdist2_do = -icdist2 * icdist2
+                                         * ( k[5] * dr2do + k[6] * dr4do + k[7] * dr6do );
+                    double dxd0_do = cdist * icdist2 * dxdo[j]
+                                     + x * icdist2 * dcdist_do + x * cdist * dicdist2_do
+                                     + k[2] * da1do + k[3] * da2do + k[8] * dr2do
+                                     + k[9] * dr4do;
+                    double dyd0_do = cdist * icdist2 * dydo[j]
+                                     + y * icdist2 * dcdist_do + y * cdist * dicdist2_do
+                                     + k[2] * da3do + k[3] * da1do + k[10] * dr2do
+                                     + k[11] * dr4do;
+                    dXdYd = dMatTilt * Vec2d( dxd0_do, dyd0_do );
+                    dpdo_p[i * 3 + j] = fx * dXdYd( 0 );
+                    dpdo_p[dpdo_step + i * 3 + j] = fy * dXdYd( 1 );
+                }
+                dpdo_p += dpdo_step * 2;
+            }
+        }
+    }
+
+    if( _m != imagePoints )
+        cvConvert( _m, imagePoints );
+
+    if( _dpdr != dpdr )
+        cvConvert( _dpdr, dpdr );
+
+    if( _dpdt != dpdt )
+        cvConvert( _dpdt, dpdt );
+
+    if( _dpdf != dpdf )
+        cvConvert( _dpdf, dpdf );
+
+    if( _dpdc != dpdc )
+        cvConvert( _dpdc, dpdc );
+
+    if( _dpdk != dpdk )
+        cvConvert( _dpdk, dpdk );
+
+    if( _dpdo != dpdo )
+        cvConvert( _dpdo, dpdo );
+}
+
+static void _cvProjectPoints2( const CvMat* objectPoints,
+                        const CvMat* r_vec,
+                        const CvMat* t_vec,
+                        const CvMat* A,
+                        const CvMat* distCoeffs,
+                        CvMat* imagePoints, CvMat* dpdr,
+                        CvMat* dpdt, CvMat* dpdf,
+                        CvMat* dpdc, CvMat* dpdk,
+                        double aspectRatio )
+{
+    _cvProjectPoints2Internal( objectPoints, r_vec, t_vec, A, distCoeffs, imagePoints, dpdr, dpdt,
+                               dpdf, dpdc, dpdk, NULL, aspectRatio );
 }
--- a/aruco_pose/src/genmap.py
+++ b/aruco_pose/src/genmap.py
@@ -13,7 +13,7 @@
 Generate map file for aruco_map nodelet.

 Usage:
-  genmap.py <length> <x> <y> <dist_x> <dist_y> [<first>] [<x0>] [<y0>] [--top-left | --bottom-left] [-o <filename>]
+  genmap.py <length> <x> <y> <dist_x> <dist_y> [<first>] [<x0>] [<y0>] [--top-left | --bottom-left]
  genmap.py (-h | --help)

 Options:
@@ -27,16 +27,13 @@ Options:
  <y0>           Y coordinate for the first marker [default: 0]
  --top-left     First marker is on top-left (default)
  --bottom-left  First marker is on bottom-left
-  -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 -o test_map.txt
+  rosrun aruco_pose genmap.py 0.33 2 4 1 1 0 > $(catkin_find aruco_pose map)/test_map.txt
 """

 from __future__ import print_function

-import sys
-from os import path
 from docopt import docopt


@@ -52,19 +49,14 @@ dist_x = float(arguments['<dist_x>'])
 dist_y = float(arguments['<dist_y>'])
 bottom_left = arguments['--bottom-left']

-if arguments['-o'] is None:
-    output = sys.stdout
-else:
-    output = open(path.join(path.dirname(__file__), '..', 'map', arguments['-o']), 'w')
-
 max_y = y0 + (markers_y - 1) * dist_y

-output.write('# id\tlength\tx\ty\tz\trot_z\trot_y\trot_x\n')
+print('# id\tlength\tx\ty\tz\trot_z\trot_y\trot_x')
 for y in range(markers_y):
    for x in range(markers_x):
        pos_x = x0 + x * dist_x
        pos_y = y0 + y * dist_y
        if not bottom_left:
            pos_y = max_y - pos_y
-        output.write('{}\t{}\t{}\t{}\t{}\t{}\t{}\t{}\n'.format(first, length, pos_x, pos_y, 0, 0, 0, 0))
+        print('{}\t{}\t{}\t{}\t{}\t{}\t{}\t{}'.format(first, length, pos_x, pos_y, 0, 0, 0, 0))
        first += 1
--- a/aruco_pose/srv/SetMarkers.srv
+++ b/aruco_pose/srv/SetMarkers.srv
@@ -1,7 +0,0 @@
-# * 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
--- a/aruco_pose/test/basic.py
+++ b/aruco_pose/test/basic.py
@@ -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/map', MarkerArray, timeout=5)
+    markers = rospy.wait_for_message('aruco_map/markers', MarkerArray, timeout=5)
    assert markers.markers[0].id == 1
    assert markers.markers[1].id == 2
    assert markers.markers[2].id == 3
--- a/aruco_pose/test/test_node_failure.py
+++ b/aruco_pose/test/test_node_failure.py
@@ -2,7 +2,6 @@ import rospy
 import pytest

 from visualization_msgs.msg import MarkerArray as VisMarkerArray
-from aruco_pose.msg import MarkerArray


@pytest.fixture
@@ -10,5 +9,5 @@ def node():
    return rospy.init_node('aruco_pose_test', anonymous=True)

 def test_node_failure(node):
-    assert rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5).markers == []
-    assert rospy.wait_for_message('aruco_map/map', MarkerArray, timeout=5).markers == []
+    with pytest.raises(rospy.exceptions.ROSException):
+        rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5)
--- a/aruco_pose/vendor/VendorOpenCV.cmake
+++ b/aruco_pose/vendor/VendorOpenCV.cmake
@@ -7,7 +7,6 @@ endif()

 message(STATUS "Adding vendored aruco_pose OpenCV module")
 add_library(_opencv_aruco STATIC
-  vendor/aruco/src/apriltag_quad_thresh.cpp
  vendor/aruco/src/aruco.cpp
  vendor/aruco/src/charuco.cpp
  vendor/aruco/src/dictionary.cpp
@@ -24,7 +23,7 @@ target_compile_definitions(_opencv_aruco PRIVATE
  CV_OVERRIDE=override
 )
 target_compile_options(_opencv_aruco PRIVATE
-  -fpic -fPIC -fvisibility=hidden
+  -fpic -fPIC
 )

 target_include_directories(_opencv_aruco PUBLIC
--- a/aruco_pose/vendor/aruco/src/apriltag_quad_thresh.cpp
+++ b/aruco_pose/vendor/aruco/src/apriltag_quad_thresh.cpp
@@ -94,7 +94,7 @@ void ptsort_(struct pt *pts, int sz)

    // Use stack storage if it's not too big.
    cv::AutoBuffer<struct pt, 1024> _tmp_stack(sz);
-    memcpy(_tmp_stack, pts, sizeof(struct pt) * sz);
+    memcpy(_tmp_stack.data(), pts, sizeof(struct pt) * sz);

    int asz = sz/2;
    int bsz = sz - asz;
@@ -470,11 +470,11 @@ int quad_segment_agg(int sz, struct line_fit_pt *lfps, int indices[4]){
    int rvalloc_pos = 0;
    int rvalloc_size = 3*sz;
    cv::AutoBuffer<struct remove_vertex, 0> rvalloc_(std::max(1, rvalloc_size));
-    memset(rvalloc_, 0, sizeof(rvalloc_[0]) * rvalloc_.size()); // TODO Add AutoBuffer zero fill
-    struct remove_vertex *rvalloc = rvalloc_;
+    memset(rvalloc_.data(), 0, sizeof(rvalloc_[0]) * rvalloc_.size()); // TODO Add AutoBuffer zero fill
+    struct remove_vertex *rvalloc = rvalloc_.data();
    cv::AutoBuffer<struct segment, 0> segs_(std::max(1, sz)); // TODO Add AutoBuffer zero fill
-    memset(segs_, 0, sizeof(segs_[0]) * segs_.size());
-    struct segment *segs = segs_;
+    memset(segs_.data(), 0, sizeof(segs_[0]) * segs_.size());
+    struct segment *segs = segs_.data();

    // populate with initial entries
    for (int i = 0; i < sz; i++) {
@@ -753,8 +753,8 @@ int fit_quad(const Ptr<DetectorParameters> &_params, const Mat im, zarray_t *clu
    // efficiently computed for any contiguous range of indices.

    cv::AutoBuffer<struct line_fit_pt, 64> lfps_(sz);
-    memset(lfps_, 0, sizeof(lfps_[0]) * lfps_.size()); // TODO Add AutoBuffer zero fill
-    struct line_fit_pt *lfps = lfps_;
+    memset(lfps_.data(), 0, sizeof(lfps_[0]) * lfps_.size()); // TODO Add AutoBuffer zero fill
+    struct line_fit_pt *lfps = lfps_.data();

    for (int i = 0; i < sz; i++) {
        struct pt *p;
--- a/book.json
+++ b/book.json
@@ -9,8 +9,7 @@
        "richquotes@https://github.com/okalachev/gitbook-plugin-richquotes.git",
        "yametrika",
        "anchors",
-        "collapsible-menu",
-        "validate-links@https://github.com/okalachev/gitbook-plugin-validate-links.git",
+        "validate-links",
        "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",
--- a/builder/assets/clever/setup.py
+++ b/builder/assets/clever/setup.py
@@ -1,4 +1,4 @@
-#!/usr/bin/env python3
+#!/usr/bin/env python

 from distutils.core import setup

--- a/builder/assets/clover.service
+++ b/builder/assets/clover.service
@@ -8,9 +8,5 @@ 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
--- a/builder/assets/examples/flight.py
+++ b/builder/assets/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)

-print('Take off and hover 1 m above the ground')
+# Take off and hover 1 m above the ground
 navigate(x=0, y=0, z=1, frame_id='body', auto_arm=True)

-# Wait for 5 seconds
-rospy.sleep(5)
+# Wait for 3 seconds
+rospy.sleep(3)

-print('Fly forward 1 m')
+# Fly forward 1 m
 navigate(x=1, y=0, z=0, frame_id='body')

-# Wait for 5 seconds
-rospy.sleep(5)
+# Wait for 3 seconds
+rospy.sleep(3)

-print('Perform landing')
+# Perform landing
 land()
--- a/builder/assets/examples/flight_marker.py
+++ b/builder/assets/examples/flight_marker.py
@@ -1,4 +1,4 @@
-# Information: https://clover.coex.tech/aruco
+# Information: https://clover.coex.tech/en/aruco.html

 import rospy
 from clover import srv
@@ -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)

-print('Take off and hover 1 m above the ground')
+# Take off and hover 1 m above the ground
 navigate(x=0, y=0, z=1, frame_id='body', auto_arm=True)

-# Wait for 5 seconds
-rospy.sleep(5)
+# Wait for 3 seconds
+rospy.sleep(3)

-print('Fly 1 meter above ArUco marker 0')
+# Fly 1 meter above ArUco marker 0
 navigate(x=0, y=0, z=1, frame_id='aruco_0')

-# Wait for 5 seconds
-rospy.sleep(5)
+# Wait for 3 seconds
+rospy.sleep(3)

-print('Fly to x=1 y=1 z=1 relative to ArUco markers map')
+# 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 5 seconds
-rospy.sleep(5)
+# Wait for 3 seconds
+rospy.sleep(3)

-print('Perform landing')
+# Perform landing
 land()
--- a/builder/assets/examples/leds.py
+++ b/builder/assets/examples/leds.py
@@ -1,4 +1,4 @@
-# Information: https://clover.coex.tech/led
+# Information: https://clover.coex.tech/en/leds.html

 import rospy
 from clover.srv import SetLEDEffect
@@ -7,25 +7,19 @@ rospy.init_node('leds')

 set_effect = rospy.ServiceProxy('led/set_effect', SetLEDEffect)  # define proxy to ROS-service

-print('Fill red')
 set_effect(r=255, g=0, b=0)  # fill strip with red color
 rospy.sleep(2)

-print('Fill green')
 set_effect(r=0, g=100, b=0)  # fill strip with green color
 rospy.sleep(2)

-print('Fade to blue')
 set_effect(effect='fade', r=0, g=0, b=255)  # fade to blue color
 rospy.sleep(2)

-print('Flash red')
 set_effect(effect='flash', r=255, g=0, b=0)  # flash twice with red color
-rospy.sleep(2)
+rospy.sleep(5)

-print('Blink white')
 set_effect(effect='blink', r=255, g=255, b=255)  # blink with white color
 rospy.sleep(5)

-print('Rainbow')
 set_effect(effect='rainbow')  # show rainbow
--- a/builder/assets/examples/navigate_wait.py
+++ b/builder/assets/examples/navigate_wait.py
@@ -1,4 +1,4 @@
-# Information: https://clover.coex.tech/en/snippets.html#navigate_wait
+# Information: https://clover.coex.tech/en/snippets.html#block-nav

 import math
 import rospy
@@ -31,11 +31,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)

-print('Take off 1 meter')
+# Take off 1 meter
 navigate_wait(z=1, frame_id='body', auto_arm=True)

-print('Fly forward 1 m')
+# Fly forward 1 m
 navigate_wait(x=1, frame_id='body')

-print('Land')
+# Land
 land()
--- a/builder/assets/hardware_setup.sh
+++ b/builder/assets/hardware_setup.sh
@@ -65,7 +65,7 @@ echo_stamp "#6 Turn on UART"
 # https://github.com/RPi-Distro/raspi-config/pull/75
 /usr/bin/raspi-config nonint do_serial 1
 /usr/bin/raspi-config nonint set_config_var enable_uart 1 /boot/config.txt
-echo dtoverlay=pi3-disable-bt >> /boot/config.txt
+/usr/bin/raspi-config nonint set_config_var dtoverlay pi3-disable-bt /boot/config.txt
 systemctl disable hciuart.service

 # After adding to Raspbian OS
--- a/builder/assets/install_gitbook.sh
+++ b/builder/assets/install_gitbook.sh
@@ -1,9 +0,0 @@
-#!/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
--- a/builder/assets/noetic-rosdep-clover.yaml
+++ b/builder/assets/noetic-rosdep-clover.yaml
@@ -1,18 +0,0 @@
-async_web_server_cpp:
-  debian:
-    buster: [ros-noetic-async-web-server-cpp]
-led_msgs:
-  debian:
-    buster: [ros-noetic-led-msgs]
-ros_pytest:
-  debian:
-    buster: [ros-noetic-ros-pytest]
-tf2_web_republisher:
-  debian:
-    buster: [ros-noetic-tf2-web-republisher]
-web_video_server:
-  debian:
-    buster: [ros-noetic-web-video-server]
-ws281x:
-  debian:
-    buster: [ros-noetic-ws281x]
--- a/builder/assets/roscore.service
+++ b/builder/assets/roscore.service
@@ -3,7 +3,7 @@ Description=Launcher for the ROS master, parameter server and rosout logging nod

 [Service]
 User=pi
-ExecStart=/bin/sh -c ". /opt/ros/noetic/setup.sh; ROS_HOSTNAME=`hostname`.local exec roscore"
+ExecStart=/bin/sh -c ". /opt/ros/melodic/setup.sh; ROS_HOSTNAME=`hostname`.local exec roscore"
 Restart=on-failure
 RestartSec=3

--- a/builder/image-build.sh
+++ b/builder/image-build.sh
@@ -15,8 +15,7 @@

 set -e # Exit immidiately on non-zero result

-# 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"
+SOURCE_IMAGE="https://downloads.raspberrypi.org/raspbian_lite/images/raspbian_lite-2020-02-14/2020-02-13-raspbian-buster-lite.zip"

 export DEBIAN_FRONTEND=${DEBIAN_FRONTEND:='noninteractive'}
 export LANG=${LANG:='C.UTF-8'}
@@ -105,6 +104,8 @@ ${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/'
 # network setup
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} exec ${SCRIPTS_DIR}'/image-network.sh'
 # avahi setup
@@ -113,11 +114,15 @@ ${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/noetic-rosdep-clover.yaml' '/etc/ros/rosdep/'
+${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/melodic-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'
--- a/builder/image-ros.sh
+++ b/builder/image-ros.sh
@@ -13,7 +13,7 @@
 # copies or substantial portions of the Software.
 #

-set -ex # exit on error, echo commands
+set -e # Exit immidiately on non-zero result

 REPO=$1
 REF=$2
@@ -21,9 +21,6 @@ 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
@@ -71,8 +68,7 @@ my_travis_retry() {
 # TODO: 'kinetic-rosdep-clover.yaml' should add only if we use our repo?
 echo_stamp "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/melodic-rosdep-clover.yaml" >> /etc/ros/rosdep/sources.list.d/20-default.list
 my_travis_retry rosdep update

 echo_stamp "Populate rosdep for ROS user"
@@ -80,40 +76,20 @@ 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_stamp "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"
-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.1-0buster \
-ros-${ROS_DISTRO}-image-publisher=1.15.3-0buster \
-ros-${ROS_DISTRO}-web-video-server=0.2.1-0buster
-apt-mark hold \
-ros-${ROS_DISTRO}-compressed-image-transport \
-ros-${ROS_DISTRO}-cv-bridge \
-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
-my_travis_retry apt-get install -y --no-install-recommends libboost-dev libboost-all-dev
-
 echo_stamp "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 \
+my_travis_retry rosdep install -y --from-paths src --ignore-src --rosdistro melodic --os=debian:buster \
  --skip-keys=gazebo_ros --skip-keys=gazebo_plugins
-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
+my_travis_retry pip install wheel
+my_travis_retry pip install -r /home/pi/catkin_ws/src/clover/clover/requirements.txt
+source /opt/ros/melodic/setup.bash
 # Don't build simulation plugins for actual drone
-catkin_make -j2 -DCMAKE_BUILD_TYPE=RelWithDebInfo
-source devel/setup.bash
+catkin_make -j2 -DCMAKE_BUILD_TYPE=Release -DCATKIN_BLACKLIST_PACKAGES=clover_gazebo_plugins

 echo_stamp "Install clever package (for backwards compatibility)"
 cd /home/pi/catkin_ws/src/clover/builder/assets/clever
@@ -122,27 +98,30 @@ rm -rf build  # remove build artifacts

 echo_stamp "Build Clover documentation"
 cd /home/pi/catkin_ws/src/clover
-builder/assets/install_gitbook.sh
-gitbook install
+NPM_CONFIG_UNSAFE_PERM=true npm install gitbook-cli -g
+NPM_CONFIG_UNSAFE_PERM=true gitbook install
 gitbook build
 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"
 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 \
-    ros-${ROS_DISTRO}-vl53l1x \
-    ros-${ROS_DISTRO}-ws281x \
-    ros-${ROS_DISTRO}-rosshow \
-    ros-${ROS_DISTRO}-cmake-modules \
-    ros-${ROS_DISTRO}-image-view
+    ros-melodic-dynamic-reconfigure \
+    ros-melodic-compressed-image-transport \
+    ros-melodic-rosbridge-suite \
+    ros-melodic-rosserial \
+    ros-melodic-usb-cam \
+    ros-melodic-vl53l1x \
+    ros-melodic-ws281x \
+    ros-melodic-rosshow

 # TODO move GeographicLib datasets to Mavros debian package
 echo_stamp "Install GeographicLib datasets (needed for mavros)" \
 && wget -qO- https://raw.githubusercontent.com/mavlink/mavros/master/mavros/scripts/install_geographiclib_datasets.sh | bash

+# FIXME: Buster comes with tornado==5.1.1 but we need tornado==4.2.1 for rosbridge_suite
+# (note that Python 3 will still have a more recent version)
+pip install tornado==4.2.1
+
 echo_stamp "Running tests"
 cd /home/pi/catkin_ws
 # FIXME: Investigate failing tests
@@ -151,26 +130,12 @@ catkin_make run_tests #&& catkin_test_results
 echo_stamp "Change permissions for catkin_ws"
 chown -Rf pi:pi /home/pi/catkin_ws

-echo_stamp "Make \$HOME/examples symlink"
-ln -s "$(catkin_find clover examples --first-only)" /home/pi
-chown -Rf pi:pi /home/pi/examples
-
-echo_stamp "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_stamp "Make udev rules symlink"
-ln -s "$(catkin_find clover udev --first-only)"/* /lib/udev/rules.d/
-
 echo_stamp "Setup ROS environment"
 cat << EOF >> /home/pi/.bashrc
 LANG='C.UTF-8'
 LC_ALL='C.UTF-8'
 export ROS_HOSTNAME=\`hostname\`.local
-source /opt/ros/${ROS_DISTRO}/setup.bash
+source /opt/ros/melodic/setup.bash
 source /home/pi/catkin_ws/devel/setup.bash
 EOF

--- a/builder/image-software.sh
+++ b/builder/image-software.sh
@@ -64,14 +64,15 @@ echo "APT::Acquire::Retries \"3\";" > /etc/apt/apt.conf.d/80-retries
 echo_stamp "Install apt keys & repos"

 # TODO: This STDOUT consist 'OK'
+curl http://deb.coex.tech/aptly_repo_signing.key 2> /dev/null | apt-key add -
 apt-get update \
 && apt-get install --no-install-recommends -y dirmngr > /dev/null \
 && apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654

 echo "deb http://packages.ros.org/ros/ubuntu buster main" > /etc/apt/sources.list.d/ros-latest.list
-
-wget -O - 'http://packages.coex.tech/key.asc' | apt-key add - 
-echo 'deb http://packages.coex.tech buster main' >> /etc/apt/sources.list
+echo "deb http://deb.coex.tech/opencv3 buster main" > /etc/apt/sources.list.d/opencv3.list
+echo "deb http://deb.coex.tech/rpi-ros-melodic buster main" > /etc/apt/sources.list.d/rpi-ros-melodic.list
+echo "deb http://deb.coex.tech/clover buster main" > /etc/apt/sources.list.d/clover.list

 echo_stamp "Update apt cache"

@@ -81,7 +82,6 @@ apt-get update

 # Let's retry fetching those packages several times, just in case
 echo_stamp "Software installing"
-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 \
 zip \
@@ -94,22 +94,23 @@ lsof \
 git \
 dnsmasq  \
 tmux \
-tree \
 vim \
+cmake \
 libjpeg8 \
 tcpdump \
+ltrace \
 libpoco-dev \
 libzbar0 \
-python3-rosdep \
-python3-rosinstall-generator \
-python3-wstool \
-python3-rosinstall \
+python-rosdep \
+python-rosinstall-generator \
+python-wstool \
+python-rosinstall \
 build-essential \
 libffi-dev \
 monkey \
 pigpio python-pigpio python3-pigpio \
 i2c-tools \
-espeak espeak-data python-espeak python3-espeak \
+espeak espeak-data python-espeak \
 ntpdate \
 python-dev \
 python3-dev \
@@ -123,10 +124,9 @@ sed -i "s/updates_available//" /usr/share/byobu/status/status

 echo_stamp "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
-python get-pip2.py
-rm get-pip.py get-pip2.py
+python get-pip.py
+rm get-pip.py
 #my_travis_retry pip install --upgrade pip
 #my_travis_retry pip3 install --upgrade pip

@@ -136,26 +136,22 @@ pip3 --version

 echo_stamp "Install and enable Butterfly (web terminal)"
 echo_stamp "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"
-my_travis_retry pip3 install --prefer-binary rpi_ws281x
+my_travis_retry pip install --prefer-binary rpi_ws281x

 echo_stamp "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"
 cd /home/pi
-wget --no-verbose https://nodejs.org/dist/v10.15.0/node-v10.15.0-linux-armv6l.tar.gz
+wget https://nodejs.org/dist/v10.15.0/node-v10.15.0-linux-armv6l.tar.gz
 tar -xzf node-v10.15.0-linux-armv6l.tar.gz
 cp -R node-v10.15.0-linux-armv6l/* /usr/local/
 rm -rf node-v10.15.0-linux-armv6l/
--- a/builder/image-validate.sh
+++ b/builder/image-validate.sh
@@ -16,24 +16,16 @@ set -ex

 echo "Run image tests"

-export ROS_DISTRO='noetic'
+export ROS_DISTRO='melodic'
 export ROS_IP='127.0.0.1'
-source /opt/ros/${ROS_DISTRO}/setup.bash
+source /opt/ros/melodic/setup.bash
 source /home/pi/catkin_ws/devel/setup.bash
-systemctl start roscore

 cd /home/pi/catkin_ws/src/clover/builder/test/
 ./tests.sh
 ./tests.py
 ./tests_py3.py
-[[ $(./test_qr.py) == "Found QRCODE with data Проверка Unicode with center at x=66.0, y=66.0" ]] 
 [[ $(./tests_clever.py) == "Warning: clever package is renamed to clover" ]]  # test backwards compatibility

-systemctl stop roscore
-
-# 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
--- a/builder/standalone-install.sh
+++ b/builder/standalone-install.sh
@@ -3,45 +3,25 @@
 # Perform a "standalone install" in a Docker container
 set -e
 # Step 1: Install pip
-apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 # https://github.com/osrf/docker_images/issues/535
-apt-get update
-apt-get install -y curl
-if [ "x${ROS_PYTHON_VERSION}" = "x3" ]; then
-  PYTHON=python3
-  curl https://bootstrap.pypa.io/get-pip.py -o get-pip.py
-else
-  PYTHON=python
-  curl https://bootstrap.pypa.io/pip/2.7/get-pip.py -o get-pip.py
-fi
-${PYTHON} ./get-pip.py
+apt update
+apt install -y curl
+curl https://bootstrap.pypa.io/get-pip.py -o get-pip.py
+python ./get-pip.py

 # Step 1.5: Add deb.coex.tech to apt
 curl http://deb.coex.tech/aptly_repo_signing.key 2> /dev/null | apt-key add -
 echo "deb http://deb.coex.tech/ros xenial main" > /etc/apt/sources.list.d/coex.tech.list
 echo "yaml file:///etc/ros/rosdep/coex.yaml" > /etc/ros/rosdep/sources.list.d/99-coex.list
-CODENAME=$(lsb_release -sc)
-
 cat <<EOF > /etc/ros/rosdep/coex.yaml
 led_msgs:
  ubuntu:
-    ${CODENAME}: [ros-${ROS_DISTRO}-led-msgs]
-async_web_server_cpp:
-  ubuntu:
-    ${CODENAME}: [ros-${ROS_DISTRO}-async-web-server-cpp]
-ros_pytest:
-  ubuntu:
-    ${CODENAME}: [ros-${ROS_DISTRO}-ros-pytest]
-tf2_web_republisher:
-  ubuntu:
-    ${CODENAME}: [ros-${ROS_DISTRO}-tf2-web-republisher]
-web_video_server:
-  ubuntu:
-    ${CODENAME}: [ros-${ROS_DISTRO}-web-video-server]
-ws281x:
-  ubuntu:
-    ${CODENAME}: [ros-${ROS_DISTRO}-ws281x]
+    xenial: ros-kinetic-led-msgs
+    bionic: ros-melodic-led-msgs
+  debian:
+    stretch: ros-kinetic-led-msgs
+    buster: ros-melodic-led-msgs
 EOF
-apt-get update
+apt update
 rosdep update

 # Step 2: Run rosdep to install all dependencies
@@ -57,10 +37,7 @@ cd /root/catkin_ws
 catkin_make

 # Step 4: Run tests
-${PYTHON} -m pip install --upgrade pytest
+pip install --upgrade pytest
 cd /root/catkin_ws
 source devel/setup.bash
 catkin_make run_tests && catkin_test_results
-
-# Step 5: Install packages
-catkin_make install
--- a/builder/test/qr.png
+++ b/builder/test/qr.png
--- a/builder/test/test_qr.py
+++ b/builder/test/test_qr.py
@@ -1,42 +0,0 @@
-#!/usr/bin/env python3
-
-# Test QG recognition example
-# Should be synced with the documentation: /docs/en/camera.md, /docs/ru/camera.md
-# TODO: use real ROS topics
-
-import rospy
-from pyzbar import pyzbar
-from cv_bridge import CvBridge
-from sensor_msgs.msg import Image
-
-bridge = CvBridge()
-
-# rospy.init_node('barcode_test')
-
-# Image subscriber callback function
-def image_callback(data):
-    cv_image = bridge.imgmsg_to_cv2(data, 'bgr8')  # OpenCV image
-    barcodes = pyzbar.decode(cv_image)
-    for barcode in barcodes:
-        b_data = barcode.data.decode("utf-8")
-        b_type = barcode.type
-        (x, y, w, h) = barcode.rect
-        xc = x + w/2
-        yc = y + h/2
-        print("Found {} with data {} with center at x={}, y={}".format(b_type, b_data, xc, yc))
-    # rospy.signal_shutdown('done')
-
-# image_sub = rospy.Subscriber('main_camera/image_raw', Image, image_callback, queue_size=1)
-
-# ==============================================================================
-# Publish test image
-# rospy.sleep(2)
-
-import cv2
-img = cv2.imread('qr.png')
-image_callback(bridge.cv2_to_imgmsg(img, 'bgr8'))
-
-# image_pub = rospy.Publisher('/main_camera/image_raw', Image, queue_size=1, latch=True)
-# image_pub.publish(bridge.cv2_to_imgmsg(img, 'bgr8'))
-
-# rospy.spin()
--- a/builder/test/tests.py
+++ b/builder/test/tests.py
@@ -1,40 +1,31 @@
-#!/usr/bin/env python3
+#!/usr/bin/env python

 # validate all required modules installed

 import rospy
 from geometry_msgs.msg import PoseStamped
-from sensor_msgs.msg import Range, BatteryState

 import cv2
 import cv2.aruco
-from sensor_msgs.msg import Image
-from cv_bridge import CvBridge

 import numpy
 import mavros
-from mavros_msgs.msg import State, StatusText, ExtendedState, RCIn, Mavlink
+from mavros_msgs.msg import State, StatusText, ExtendedState
 from mavros_msgs.srv import CommandBool, CommandLong, SetMode

 from std_srvs.srv import Trigger
 from clover.srv import GetTelemetry, Navigate, NavigateGlobal, SetPosition, SetVelocity, \
    SetAttitude, SetRates, SetLEDEffect
-from led_msgs.srv import SetLEDs
-from led_msgs.msg import LEDStateArray, LEDState
-from aruco_pose.msg import Marker, MarkerArray, Point2D
-
-import dynamic_reconfigure.client

 import tf2_ros
 import tf2_geometry_msgs

 import VL53L1X
 import pymavlink
-import lxml
 from pymavlink import mavutil
 import rpi_ws281x
 import pigpio
-# from espeak import espeak
+from espeak import espeak
 from pyzbar import pyzbar

-print(cv2.getBuildInformation())
+print cv2.getBuildInformation()
--- a/builder/test/tests.sh
+++ b/builder/test/tests.sh
@@ -32,9 +32,8 @@ monkey --version
 pigpiod -v
 i2cdetect -V
 butterfly -h
-# espeak --version
+espeak --version
 mjpg_streamer --version
-systemctl --version

 # ros stuff

@@ -44,8 +43,6 @@ 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
@@ -55,12 +52,6 @@ rosversion usb_cam
 rosversion cv_camera
 rosversion web_video_server
 rosversion rosshow
-rosversion nodelet
-rosversion image_view
-
-# validate some versions
-[[ $(rosversion cv_camera) == "0.5.1" ]] # patched version with init fix
-[[ $(rosversion ws281x) == "0.0.13" ]]

 # validate examples are present
 [[ $(ls /home/pi/examples/*) ]]
--- a/builder/test/tests_clever.py
+++ b/builder/test/tests_clever.py
@@ -1,4 +1,4 @@
-#!/usr/bin/env python3
+#!/usr/bin/env python

 # test backwards compatibility

--- a/check_unused_assets.py
+++ b/check_unused_assets.py
@@ -4,9 +4,7 @@ import os
 import sys
 import subprocess

-EXCLUDE = ('clever4-front-white.png', 'clever4-front-white-large.png', '.DS_Store',
-    'clever4-front-black-large.png','clover42-black.png', 'clover42-main-margin.png')
-
+EXCLUDE = ('clever4-front-white.png', '.DS_Store', 'clever4-front-black-large.png')
 code = 0

 os.chdir('./docs')
--- a/clover/CHANGELOG.rst
+++ b/clover/CHANGELOG.rst
@@ -1,8 +0,0 @@
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-Changelog for package clover
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-
-0.21.1 (2020-11-17)
-------------------
-* First release of clover package to ROS
-* Contributors: Alexey Rogachevskiy, Arthur Golubtsov, Oleg Kalachev
--- a/clover/CMakeLists.txt
+++ b/clover/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.0)
+cmake_minimum_required(VERSION 2.8.3)
 project(clover)

 ## Compile as C++11, supported in ROS Kinetic and newer
@@ -17,7 +17,6 @@ find_package(catkin REQUIRED COMPONENTS
  message_generation
  geometry_msgs
  sensor_msgs
-  led_msgs
  geographic_msgs
  tf
  tf2
@@ -25,22 +24,13 @@ find_package(catkin REQUIRED COMPONENTS
  tf2_ros
  image_transport
  cv_bridge
-  dynamic_reconfigure
 )

 list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/cmake")

 find_package(GeographicLib REQUIRED)

-# Workaround for OpenCV 3/4 support
-set(_opencv_version 4)
-find_package(OpenCV ${_opencv_version} QUIET COMPONENTS calib3d imgproc)
-if (NOT OpenCV_FOUND)
-  message(STATUS "Did not find OpenCV 4, searching for OpenCV 3")
-  set(_opencv_version 3)
-endif()
-
-find_package(OpenCV ${_opencv_version} REQUIRED
+find_package(OpenCV 3 REQUIRED
  COMPONENTS
    calib3d
    imgproc
@@ -128,9 +118,10 @@ generate_messages(
 ##     and list every .cfg file to be processed

 ## Generate dynamic reconfigure parameters in the 'cfg' folder
-generate_dynamic_reconfigure_options(
-  cfg/Flow.cfg
-)
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )

 ###################################
 ## catkin specific configuration ##
@@ -212,8 +203,6 @@ 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
@@ -244,12 +233,12 @@ target_link_libraries(${PROJECT_NAME}
 #   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 # )

-# Mark executables and/or libraries for installation
-install(TARGETS simple_offboard clover_rc camera_markers vpe_publisher clover_led shell clover
-  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-)
+## Mark executables and/or libraries for installation
+# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )

 ## Mark cpp header files for installation
 # install(DIRECTORY include/${PROJECT_NAME}/
@@ -265,25 +254,13 @@ install(TARGETS simple_offboard clover_rc camera_markers vpe_publisher clover_le
 #   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 # )

-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
-    udev/99-px4fmu.rules
+    config/99-px4fmu.rules
    DESTINATION /lib/udev/rules.d
  )
 else()
--- a/clover/README.md
+++ b/clover/README.md
@@ -4,7 +4,7 @@ A bundle for autonomous navigation and drone control.

 ## Manual installation

-Install ROS Noetic according to the [documentation](http://wiki.ros.org/noetic/Installation), then [create a Catkin workspace](http://wiki.ros.org/catkin/Tutorials/create_a_workspace).
+Install ROS Melodic according to the [documentation](http://wiki.ros.org/melodic/Installation), then [create a Catkin workspace](http://wiki.ros.org/catkin/Tutorials/create_a_workspace).

 Clone this repo to directory `~/catkin_ws/src/clover`:

@@ -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/udev
+cd ~/catkin_ws/src/clover/clover/config
 sudo cp 99-px4fmu.rules /lib/udev/rules.d
 ```

@@ -44,12 +44,16 @@ Alternatively you may change the `fcu_url` property in `mavros.launch` file to p

 ## Running

+To start connection to SITL, use:
+
+```bash
+roslaunch clover sitl.launch
+```
+
 To start connection to the flight controller, use:

 ```bash
 roslaunch clover clover.launch
 ```

-For the simulation information see the [corresponding article](https://clover.coex.tech/en/simulation.html).
-
 > Note that the package is configured to connect to `/dev/px4fmu` by default (see [previous section](#manual-installation)). Install udev rules or specify path to your FCU device in `mavros.launch`.
--- a/clover/cfg/Flow.cfg
+++ b/clover/cfg/Flow.cfg
@@ -1,10 +0,0 @@
-#!/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"))
--- a/clover/config/99-px4fmu.rules
+++ b/clover/config/99-px4fmu.rules
@@ -12,6 +12,4 @@ SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0016", ATTRS{produ
 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"
-# CUAV X7 Pro
-SUBSYSTEM=="tty", ATTRS{idVendor}=="3163", ATTRS{idProduct}=="004c", ATTRS{product}=="PX4 CUAV X7Pro", SYMLINK+="px4fmu"

--- a/clover/examples/get_telemetry.py
+++ b/clover/examples/get_telemetry.py
@@ -1,11 +0,0 @@
-# Information: https://clover.coex.tech/en/simple_offboard.html#gettelemetry
-
-import rospy
-from clover import srv
-
-rospy.init_node('flight')
-
-get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry)
-
-# Print drone's state
-print(get_telemetry())
--- a/clover/examples/gps.py
+++ b/clover/examples/gps.py
@@ -1,47 +0,0 @@
-# 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()
--- a/clover/examples/subscriber.py
+++ b/clover/examples/subscriber.py
@@ -1,15 +0,0 @@
-# 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()
--- a/clover/launch/aruco.launch
+++ b/clover/launch/aruco.launch
@@ -2,41 +2,30 @@
    <arg name="aruco_detect" default="true"/>
    <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.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="$(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">
+    <node name="aruco_detect" pkg="nodelet" if="$(arg aruco_detect)" type="nodelet" args="load aruco_pose/aruco_detect nodelet_manager" output="screen" clear_params="true">
        <remap from="image_raw" to="main_camera/image_raw"/>
        <remap from="camera_info" to="main_camera/camera_info"/>
-        <remap from="map_markers" to="aruco_map/map"/>
+        <remap from="map_markers" to="aruco_map/markers" if="$(arg aruco_map)"/>
        <param name="estimate_poses" value="true"/>
        <param name="send_tf" value="true"/>
-        <param name="known_tilt" value="map" if="$(eval placement == 'floor')"/>
-        <param name="known_tilt" value="map_flipped" if="$(eval placement == 'ceiling')"/>
-        <param name="length" value="$(arg length)"/>
-        <param name="transform_timeout" value="0.1"/>
+        <param name="known_tilt" value="map"/>
+        <param name="length" value="0.33"/>
        <!-- aruco detector parameters -->
        <param name="cornerRefinementMethod" value="2"/> <!-- contour refinement -->
        <param name="minMarkerPerimeterRate" value="0.075"/> <!-- 0.075 for 320x240, 0.0375 for 640x480 -->
-        <!-- length override example: -->
-        <!-- <param name="length_override/3" value="0.1"/> -->
    </node>

    <!-- aruco_map: estimate aruco map pose -->
-    <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">
+    <node name="aruco_map" pkg="nodelet" type="nodelet" if="$(arg aruco_map)" args="load aruco_pose/aruco_map nodelet_manager" output="screen" clear_params="true">
        <remap from="image_raw" to="main_camera/image_raw"/>
        <remap from="camera_info" to="main_camera/camera_info"/>
        <remap from="markers" to="aruco_detect/markers"/>
-        <param name="map" value="$(find aruco_pose)/map/$(arg map)"/>
-        <param name="known_tilt" value="map" if="$(eval placement == 'floor')"/>
-        <param name="known_tilt" value="map_flipped" if="$(eval placement == 'ceiling')"/>
+        <param name="map" value="$(find aruco_pose)/map/map.txt"/>
+        <param name="known_tilt" value="map"/>
        <param name="image_axis" value="true"/>
        <param name="frame_id" value="aruco_map_detected" if="$(arg aruco_vpe)"/>
        <param name="frame_id" value="aruco_map" unless="$(arg aruco_vpe)"/>
@@ -45,11 +34,11 @@
    </node>

    <!-- vpe publisher from aruco markers -->
-    <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" if="$(arg aruco_vpe)"/>
+    <node name="vpe_publisher" pkg="clover" type="vpe_publisher" if="$(arg aruco_vpe)" output="screen" clear_params="true">
+        <remap from="~pose_cov" to="aruco_map/pose"/>
        <remap from="~vpe" to="mavros/vision_pose/pose"/>
-        <param name="frame_id" value="aruco_map_detected" if="$(arg aruco_vpe)"/>
-        <param name="force_init" value="$(arg force_init)"/>
+        <param name="frame_id" value="aruco_map_detected"/>
+        <param name="publish_zero" value="true"/>
        <param name="offset_frame_id" value="aruco_map"/>
    </node>
 </launch>
--- a/clover/launch/clover.launch
+++ b/clover/launch/clover.launch
@@ -10,9 +10,8 @@
    <arg name="aruco" default="false"/>
    <arg name="rangefinder_vl53l1x" default="true"/>
    <arg name="led" default="true"/>
-    <arg name="blocks" default="false"/>
-    <arg name="rc" default="false"/>
-    <arg name="force_init" value="true"/> <!-- force estimator to init by publishing zero pose -->
+    <arg name="rc" default="true"/>
+    <arg name="shell" default="true"/>

    <arg name="simulator" default="false"/> <!-- flag that we are operating on a simulated drone -->

@@ -34,24 +33,28 @@
    </node>

    <!-- aruco markers -->
-    <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>
+    <include file="$(find clover)/launch/aruco.launch" if="$(arg aruco)"/>

    <!-- 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">
+    <node pkg="nodelet" type="nodelet" name="optical_flow" args="load clover/optical_flow nodelet_manager" if="$(arg optical_flow)" clear_params="true" output="screen">
        <remap from="image_raw" to="main_camera/image_raw"/>
        <remap from="camera_info" to="main_camera/camera_info"/>
        <param name="calc_flow_gyro" value="true"/>
        <param name="roi_rad" value="0.8"/>
    </node>

+    <!-- main nodelet manager -->
+    <node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager" output="screen" clear_params="true">
+        <param name="num_worker_threads" value="2"/>
+    </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/main_camera_optical" value="map"/>
+        <param name="reference_frames/body" value="map"/>
+        <param name="reference_frames/base_link" value="map"/>
+        <param name="reference_frames/navigate_target" value="map"/>
    </node>

    <!-- main camera -->
@@ -77,15 +80,15 @@
        <arg name="simulator" value="$(arg simulator)"/>
    </include>

-    <!-- Clover Blocks -->
-    <node name="clover_blocks" pkg="clover_blocks" type="clover_blocks" output="screen" if="$(arg blocks)"/>
-
    <!-- rc backend -->
    <node name="rc" pkg="clover" type="rc" output="screen" if="$(arg rc)" clear_params="true">
        <!-- Send fake GCS heartbeats. Set to "true" for upstream PX4 -->
        <param name="use_fake_gcs" value="false"/>
    </node>

+    <!-- Shell access through ROS service -->
+    <node name="shell" pkg="clover" type="shell" output="screen" if="$(arg shell)"/>
+
    <!-- Update static directory -->
    <node pkg="roswww_static" name="roswww_static" type="main.py" clear_params="true">
        <param name="default_package" value="clover"/>
--- a/clover/launch/led.launch
+++ b/clover/launch/led.launch
@@ -2,17 +2,14 @@
    <arg name="ws281x" default="true"/>
    <arg name="led_effect" default="true"/>
    <arg name="led_notify" default="true"/>
-    <arg name="led_count" default="72"/>
-    <arg name="gpio_pin" default="21"/>
-
    <arg name="simulator" default="false"/>

    <!-- For additional help go to https://clover.coex.tech/led -->

    <!-- ws281x led strip driver -->
    <node pkg="ws281x" name="led" type="ws281x_node" clear_params="true" output="screen" if="$(eval ws281x and not simulator)">
-        <param name="led_count" value="$(arg led_count)"/>
-        <param name="gpio_pin" value="$(arg gpio_pin)"/>
+        <param name="led_count" value="58"/>
+        <param name="gpio_pin" value="21"/>
        <param name="brightness" value="64"/>
        <param name="strip_type" value="WS2811_STRIP_GRB"/>
        <param name="target_frequency" value="800000"/>
@@ -35,8 +32,8 @@
            altctl: { r: 255, g: 255, b: 40 }
            posctl: { r: 50, g: 100, b: 220 }
            offboard: { r: 220, g: 20, b: 250 }
-            low_battery: { threshold: 3.6, effect: blink_fast, r: 255, g: 0, b: 0 }
-            error: { effect: flash, r: 255, g: 0, b: 0, ignore: [ "[lpe] vision position timeout" ]}
+            low_battery: { threshold: 3.7, effect: blink_fast, r: 255, g: 0, b: 0 }
+            error: { effect: flash, r: 255, g: 0, b: 0 }
        </rosparam>
    </node>
 </launch>
--- a/clover/launch/main_camera.launch
+++ b/clover/launch/main_camera.launch
@@ -3,29 +3,21 @@

    <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="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"/>
    <node if="$(eval direction_z == 'down' and direction_y == 'forward')" 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"/>
    <node if="$(eval direction_z == 'up' and direction_y == 'backward')" pkg="tf2_ros" type="static_transform_publisher" name="main_camera_frame" args="0.05 0 0.07 1.5707963 0 0 base_link main_camera_optical"/>
    <node if="$(eval direction_z == 'up' and direction_y == 'forward')" pkg="tf2_ros" type="static_transform_publisher" name="main_camera_frame" args="0.05 0 0.07 -1.5707963 0 0 base_link main_camera_optical"/>
-    <node if="$(eval direction_z == 'forward')" pkg="tf2_ros" type="static_transform_publisher" name="main_camera_frame" args="0.03 0 0.05 -1.5707963 0 -1.5707963 base_link main_camera_optical"/>
-    <node if="$(eval direction_z == 'backward')" pkg="tf2_ros" type="static_transform_publisher" name="main_camera_frame" args="-0.03 0 0.05 1.5707963 0 -1.5707963 base_link main_camera_optical"/>

    <!-- Template for custom camera orientation -->
    <!-- Camera position and orientation are represented by base_link -> main_camera_optical transform -->
    <!-- static_transform_publisher arguments: x y z yaw pitch roll frame_id child_frame_id -->
    <!-- <node 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"/> -->

-    <!-- camera nodelet manager -->
-    <node pkg="nodelet" type="nodelet" name="main_camera_nodelet_manager" args="manager" output="screen" clear_params="true" respawn="true">
-        <param name="num_worker_threads" value="2"/>
-    </node>
-
    <!-- camera node -->
-    <node pkg="nodelet" type="nodelet" name="main_camera" args="load cv_camera/CvCameraNodelet main_camera_nodelet_manager" launch-prefix="rosrun clover waitfile $(arg device)" clear_params="true" unless="$(arg simulator)" respawn="true">
-        <param name="device_path" value="$(arg device)"/>
+    <node pkg="nodelet" type="nodelet" name="main_camera" args="load cv_camera/CvCameraNodelet nodelet_manager" clear_params="true" unless="$(arg simulator)">
+        <param name="device_path" value="/dev/video0"/> <!-- v4l2 device -->
        <param name="frame_id" value="main_camera_optical"/>
        <param name="camera_info_url" value="file://$(find clover)/camera_info/fisheye_cam.yaml"/>

--- a/clover/launch/mavros.launch
+++ b/clover/launch/mavros.launch
@@ -6,16 +6,13 @@
    <arg name="viz" default="true"/>
    <arg name="respawn" default="true"/>
    <arg name="distance_sensor_remap" default="rangefinder/range"/>
-    <arg name="usb_device" default="/dev/px4fmu"/>
-    <arg name="prefix" default="" unless="$(eval fcu_conn == 'usb')"/>
-    <arg name="prefix" default="rosrun clover waitfile $(arg usb_device)" if="$(eval fcu_conn == 'usb')"/>

-    <node pkg="mavros" type="mavros_node" name="mavros" launch-prefix="$(arg prefix)" required="false" clear_params="true" respawn="$(arg respawn)" unless="$(eval fcu_conn == 'none')" respawn_delay="1" output="screen">
+    <node pkg="mavros" type="mavros_node" name="mavros" required="false" clear_params="true" respawn="$(arg respawn)" unless="$(eval fcu_conn == 'none')" respawn_delay="1" output="screen">
        <!-- UART connection -->
        <param name="fcu_url" value="/dev/ttyAMA0:921600" if="$(eval fcu_conn is None or fcu_conn == 'uart')"/>

        <!-- USB connection -->
-        <param name="fcu_url" value="$(arg usb_device)" if="$(eval fcu_conn == 'usb')"/>
+        <param name="fcu_url" value="/dev/px4fmu" if="$(eval fcu_conn == 'usb')"/>

        <!-- sitl before PX4 1.9.0 -->
        <param name="fcu_url" value="udp://@$(arg fcu_ip):14557" if="$(eval fcu_conn == 'udp')"/>
@@ -39,7 +36,7 @@
        <rosparam command="load" file="$(find clover)/launch/mavros_config.yaml"/>

        <!-- remap rangefinder -->
-        <remap from="mavros/distance_sensor/rangefinder_sub" to="$(arg distance_sensor_remap)" if="$(eval bool(distance_sensor_remap))"/>
+        <remap from="mavros/distance_sensor/rangefinder_sub" to="rangefinder/range"/>

        <rosparam param="plugin_whitelist">
            - altitude
--- a/clover/launch/mavros_config.yaml
+++ b/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: false
+startup_px4_usb_quirk: true

 conn:
-  heartbeat_rate: 1.0    # send heartbeat rate in Hertz
-  timeout: 10.0          # heartbeat timeout in seconds
+  heartbeat_rate: 1.0    # send hertbeat rate in Hertz
+  timeout: 10.0          # hertbeat 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,7 +13,6 @@ 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
@@ -78,9 +77,6 @@ distance_sensor:
      field_of_view: 0.5
  rangefinder_sub:
      subscriber: true
-      id: 1
-      orientation: PITCH_270
-      covariance: 1  # cm

 # fake_gps
 fake_gps:
--- a/clover/launch/simulator.launch
+++ b/clover/launch/simulator.launch
@@ -1,4 +0,0 @@
-<launch>
-    <!-- shurtcut for running the simulation (`roslaunch clover simulator.launch`) -->
-    <include file="$(find clover_simulation)/launch/simulator.launch"/>
-</launch>
--- a/clover/launch/sitl.launch
+++ b/clover/launch/sitl.launch
@@ -0,0 +1,19 @@
+<launch>
+    <!-- clover configuration for testing in sitl -->
+    <arg name="ip" default="127.0.0.1"/>
+    <arg name="rosbridge" default="false"/>
+
+    <include file="$(find clover)/launch/clover.launch">
+        <arg name="fcu_conn" value="udp"/>
+        <arg name="fcu_ip" value="$(arg ip)"/>
+        <arg name="gcs_bridge" value="false"/>
+        <arg name="optical_flow" value="false"/>
+        <arg name="web_video_server" default="false"/>
+        <arg name="main_camera" default="false"/>
+        <arg name="rosbridge" value="$(arg rosbridge)"/>
+        <arg name="aruco" default="false"/>
+        <arg name="rangefinder_vl53l1x" default="false"/>
+        <arg name="led" default="false"/>
+        <arg name="rc" default="false"/>
+    </include>
+</launch>
--- a/clover/package.xml
+++ b/clover/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
-<package format="3">
+<package format="2">
  <name>clover</name>
-  <version>0.23.0</version>
+  <version>0.0.1</version>
  <description>The Clover package</description>

  <maintainer email="okalachev@gmail.com">Oleg Kalachev</maintainer>
@@ -37,7 +37,7 @@
  <depend>rosbridge_server</depend>
  <depend>web_video_server</depend>
  <depend>tf2_web_republisher</depend>
-  <depend>dynamic_reconfigure</depend>
+  <depend>python-lxml</depend>
  <exec_depend>python-pymavlink</exec_depend>
  <!-- Use test_depend for packages you need only for testing: -->
  <!--   <test_depend>gtest</test_depend> -->
--- a/clover/src/autotest/autotest_aruco.py
+++ b/clover/src/autotest/autotest_aruco.py
@@ -1,87 +0,0 @@
-#!/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
-
-flow_client = dynamic_reconfigure.client.Client('optical_flow')
-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=float('nan'), yaw_rate=0, speed=0.5, \
-        frame_id='body', tolerance=0.2, auto_arm=False):
-
-    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
-
-    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()
-
-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()
--- a/clover/src/autotest/autotest_flight.py
+++ b/clover/src/autotest/autotest_flight.py
@@ -1,100 +0,0 @@
-#!/usr/bin/env python3
-
-import rospy
-import math
-from math import nan
-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_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, yaw_rate=0, speed=0.5, \
-        frame_id='body', tolerance=0.2, auto_arm=False):
-
-    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
-
-    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° [enter] ')
-navigate(yaw=-math.pi / 2, frame_id='navigate_target')
-rospy.sleep(3)
-
-input('Use set_attitude to fly backwards [enter]')
-set_attitude(pitch=-0.3, roll=0, 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(pitch=0, roll=0.3, 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 yaw_rate [enter]')
-set_position(x=nan, y=nan, z=nan, frame_id='body', yaw=nan, yaw_rate=-1)
-rospy.sleep(2 * math.pi)
-set_position(frame_id='body')
-
-input('Return to start point [enter]')
-navigate_wait(x=start.x, y=start.y, z=start.z, yaw=start.yaw, speed=1, frame_id='map')
-
-input('Land [enter]')
-land()
--- a/clover/src/autotest/autotest_led.py
+++ b/clover/src/autotest/autotest_led.py
@@ -1,72 +0,0 @@
-#!/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()
--- a/clover/src/autotest/util.py
+++ b/clover/src/autotest/util.py
@@ -1,11 +0,0 @@
-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
--- a/clover/src/led.cpp
+++ b/clover/src/led.cpp
@@ -12,7 +12,6 @@

 #include <ros/ros.h>
 #include <string>
-#include <vector>
 #include <boost/algorithm/string.hpp>

 #include <clover/SetLEDEffect.h>
@@ -30,7 +29,7 @@ ros::Timer timer;
 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;
@@ -86,8 +85,9 @@ void proceed(const ros::TimerEvent& event)
 	set_leds.request.leds.resize(led_count);

 	if (current_effect.effect == "blink" || current_effect.effect == "blink_fast") {
-		// enable on odd counter
-		if (counter % 2 != 0) {
+		blink_state = !blink_state;
+		// toggle all leds
+		if (blink_state) {
 			fill(current_effect.r, current_effect.g, current_effect.b);
 		} else {
 			fill(0, 0, 0);
@@ -220,7 +220,6 @@ 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;
 }
@@ -275,10 +274,6 @@ void handleMavrosState(const mavros_msgs::State& msg)
 void handleLog(const rosgraph_msgs::Log& log)
 {
 	if (log.level >= rosgraph_msgs::Log::ERROR) {
-		// check if ignored
-		for (auto const& str : error_ignore) {
-			if (log.msg.find(str) != std::string::npos) return;
-		}
 		notify("error");
 	}
 }
@@ -307,7 +302,6 @@ int main(int argc, char **argv)
 	nh_priv.param("rainbow_period", rainbow_period, 5.0);

 	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
 	set_leds_srv = nh.serviceClient<led_msgs::SetLEDs>("set_leds", true);
--- a/clover/src/optical_flow.cpp
+++ b/clover/src/optical_flow.cpp
@@ -22,13 +22,11 @@
 #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/Vector3Stamped.h>
 #include <geometry_msgs/PointStamped.h>
 #include <geometry_msgs/TwistStamped.h>
-#include <clover/FlowConfig.h>

 using cv::Mat;

@@ -40,7 +38,6 @@ public:
 	{}

 private:
-	bool enabled_;
 	ros::Publisher flow_pub_, velo_pub_, shift_pub_;
 	ros::Time prev_stamp_;
 	std::string fcu_frame_id_, local_frame_id_;
@@ -56,8 +53,6 @@ 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_;
-	std::shared_ptr<dynamic_reconfigure::Server<clover::FlowConfig>> dyn_srv_;

 	void onInit()
 	{
@@ -74,25 +69,20 @@ 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_sub_ = it.subscribeCamera("image_raw", 1, &OpticalFlow::flow, this);
 		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);
-
-		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");
 	}

@@ -119,8 +109,6 @@ private:

 	void flow(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cinfo)
 	{
-		if (!enabled_) return;
-
 		parseCameraInfo(cinfo);

 		auto img = cv_bridge::toCvShare(msg, "mono8")->image;
@@ -164,7 +152,7 @@ private:
 			cv::Point2d shift = cv::phaseCorrelate(prev_, curr_, hann_, &response);

 			// Publish raw shift in pixels
-			geometry_msgs::Vector3Stamped shift_vec;
+			static geometry_msgs::Vector3Stamped shift_vec;
 			shift_vec.header.stamp = msg->header.stamp;
 			shift_vec.header.frame_id = msg->header.frame_id;
 			shift_vec.vector.x = shift.x;
@@ -190,8 +178,8 @@ 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
-			geometry_msgs::Vector3Stamped flow_camera, flow_fcu;
+			// // Convert to FCU frame
+			static geometry_msgs::Vector3Stamped flow_camera, flow_fcu;
 			flow_camera.header.frame_id = msg->header.frame_id;
 			flow_camera.header.stamp = msg->header.stamp;
 			flow_camera.vector.x = flow_y; // +y means counter-clockwise rotation around Y axis
@@ -207,21 +195,18 @@ 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);
-					geometry_msgs::Vector3Stamped flow_gyro_fcu;
+					static geometry_msgs::Vector3Stamped flow_gyro_fcu;
 					tf_buffer_->transform(flow_gyro_camera, flow_gyro_fcu, fcu_frame_id_);
 					flow_.integrated_xgyro = flow_gyro_fcu.vector.x;
 					flow_.integrated_ygyro = flow_gyro_fcu.vector.y;
 					flow_.integrated_zgyro = flow_gyro_fcu.vector.z;
 				} catch (const tf2::TransformException& e) {
-					// Transform not available, keep NANs in flow gyro
+					// Invalidate previous frame
+					prev_.release();
+					return;
 				}
 			}

@@ -233,10 +218,6 @@ private:
 			flow_.quality = (uint8_t)(response * 255);
 			flow_pub_.publish(flow_);

-			prev_ = curr_.clone();
-			prev_stamp_ = msg->header.stamp;
-
-publish_debug:
 			// Publish debug image
 			if (img_pub_.getNumSubscribers() > 0) {
 				// publish debug image
@@ -250,12 +231,15 @@ publish_debug:
 			}

 			// Publish estimated angular velocity
-			geometry_msgs::TwistStamped velo;
+			static 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.twist.angular.x = flow_.integrated_x / integration_time.toSec();
+			velo.twist.angular.y = flow_.integrated_y / integration_time.toSec();
 			velo_pub_.publish(velo);
+
+			prev_ = curr_.clone();
+			prev_stamp_ = msg->header.stamp;
 		}
 	}

@@ -276,14 +260,6 @@ publish_debug:

 		return flow;
 	}
-
-	void paramCallback(clover::FlowConfig &config, uint32_t level)
-	{
-		enabled_ = config.enabled;
-		if (!enabled_) {
-			prev_ = Mat(); // clear previous frame
-		}
-	}
 };

 PLUGINLIB_EXPORT_CLASS(OpticalFlow, nodelet::Nodelet)
--- a/clover/src/rc.cpp
+++ b/clover/src/rc.cpp
@@ -91,7 +91,7 @@ private:
 	void fakeGCSThread()
 	{
 		// Awful workaround for fixing PX4 not sending STATUSTEXTs
-		// if there is no GCS heartbeats.
+		// if there is no GCS hearbeats.
 		// TODO: use timer
 		// TODO: remove, when PX4 get this fixed.
 		ros::Publisher mavlink_pub = nh.advertise<mavros_msgs::Mavlink>("mavlink/to", 1);
--- a/clover/src/selfcheck.py
+++ b/clover/src/selfcheck.py
@@ -30,7 +30,6 @@ from visualization_msgs.msg import MarkerArray as VisualizationMarkerArray
 import tf.transformations as t
 from aruco_pose.msg import MarkerArray
 from mavros import mavlink
-import locale


 # TODO: check attitude is present
@@ -44,10 +43,6 @@ import locale

 rospy.init_node('selfcheck')

-os.environ['ROSCONSOLE_FORMAT']='[${severity}]: ${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)
@@ -143,7 +138,7 @@ def mavlink_exec(cmd, timeout=3.0):
        timeout=3,
        baudrate=0,
        count=len(cmd),
-        data=[ord(c) for c in cmd.ljust(70, '\0')])
+        data=map(ord, cmd.ljust(70, '\0')))
    msg.pack(link)
    ros_msg = mavlink.convert_to_rosmsg(msg)
    mavlink_pub.publish(ros_msg)
@@ -198,27 +193,24 @@ def check_fcu():
            failure('no connection to the FCU (check wiring)')
            return

-        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')

-        for line in version_str.split('\n'):
-            if line.startswith('FW version: '):
-                info(line[len('FW version: '):])
-            elif line.startswith('FW git tag: '): # only Clover's firmware
-                tag = line[len('FW git tag: '):]
-                clover_fw = clover_tag.search(tag)
-                info(tag)
-            elif line.startswith('HW arch: '):
-                info(line[len('HW arch: '):])
+        r = re.compile(r'^FW (git tag|version): (v?\d\.\d\.\d.*)$')
+        is_clover_firmware = False
+        for ver_line in version_str.split('\n'):
+            match = r.search(ver_line)
+            if match is not None:
+                field, version = match.groups()
+                info('firmware %s: %s' % (field, version))
+                if 'clover' in version or 'clever' in version:
+                    is_clover_firmware = True

-        if not clover_fw:
-            info('not Clover PX4 firmware, check https://clover.coex.tech/firmware')
+        if not is_clover_firmware:
+            failure('not running Clover PX4 firmware, https://clover.coex.tech/firmware')

        est = get_param('SYS_MC_EST_GROUP')
        if est == 1:
@@ -336,7 +328,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 command line (including arguments) to match
+            args.append('-f')  # use full process name to match
        args.append(binary)
        subprocess.check_output(args)
        return True
@@ -491,12 +483,6 @@ 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')

@@ -534,8 +520,6 @@ def check_global_position():
        rospy.wait_for_message('mavros/global_position/global', NavSatFix, timeout=1)
    except rospy.ROSException:
        info('no global position')
-        if get_param('SYS_MC_EST_GROUP') == 2 and (get_param('EKF2_AID_MASK') & (1 << 0)):
-            failure('enabled GPS fusion may suppress vision position aiding')


@check('Optical flow')
@@ -625,37 +609,34 @@ def check_rangefinder():

@check('Boot duration')
 def check_boot_duration():
-    output = subprocess.check_output('systemd-analyze').decode()
+    output = subprocess.check_output('systemd-analyze')
    r = re.compile(r'([\d\.]+)s\s*$', flags=re.MULTILINE)
    duration = float(r.search(output).groups()[0])
-    if duration > 20:
+    if duration > 15:
        failure('long Raspbian boot duration: %ss (systemd-analyze for analyzing)', duration)


@check('CPU usage')
 def check_cpu_usage():
-    WHITELIST = 'nodelet', 'gzclient', 'gzserver'
+    WHITELIST = 'nodelet',
    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()
+    output = subprocess.check_output(CMD, shell=True)
    processes = output.split('\n')
    for process in processes:
        if not process:
            continue
        pid, cpu, cmd = process.split('\t')

-        if cmd.strip() not in WHITELIST and locale.atof(cpu) > 30:
+        if cmd.strip() not in WHITELIST and float(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()
+                                         stderr=subprocess.STDOUT)
    except subprocess.CalledProcessError as e:
        failure('systemctl returned %s: %s', e.returncode, e.output)
        return
@@ -665,22 +646,13 @@ def check_clover_service():
    elif 'failed' in output:
        failure('service failed to run, check your launch-files')

-    BLACKLIST = 'Unexpected command 520', 'Time jump detected', 'different index:'
-
-    r = re.compile(r'^(.*)\[(FATAL|ERROR| WARN)\] \[\d+.\d+\]: (.*?)(\x1b(.*))?$')
+    r = re.compile(r'^(.*)\[(FATAL|ERROR)\] \[\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].strip() + ': ' + node_error.groups()[2]
+                msg = node_error.groups()[1] + ': ' + node_error.groups()[2]
                if msg in error_count:
                    error_count[msg] += 1
                else:
@@ -708,10 +680,6 @@ def check_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')
@@ -733,10 +701,6 @@ 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:
@@ -759,14 +723,6 @@ 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?
@@ -795,9 +751,9 @@ def check_rpi_health():
        # <parameter>=<value>
        # In case of `get_throttled`, <value> is a hexadecimal number
        # with some of the FLAGs OR'ed together
-        output = subprocess.check_output(['vcgencmd', 'get_throttled']).decode()
+        output = subprocess.check_output(['vcgencmd', 'get_throttled'])
    except OSError:
-        info('could not call vcgencmd binary; not a Raspberry Pi?')
+        failure('could not call vcgencmd binary; not a Raspberry Pi?')
        return

    throttle_mask = int(output.split('=')[1], base=16)
--- a/clover/src/simple_offboard.cpp
+++ b/clover/src/simple_offboard.cpp
@@ -36,7 +36,6 @@
 #include <mavros_msgs/Thrust.h>
 #include <mavros_msgs/State.h>
 #include <mavros_msgs/StatusText.h>
-#include <mavros_msgs/ManualControl.h>

 #include <clover/GetTelemetry.h>
 #include <clover/Navigate.h>
@@ -47,7 +46,6 @@
 #include <clover/SetRates.h>

 using std::string;
-using std::isnan;
 using namespace geometry_msgs;
 using namespace sensor_msgs;
 using namespace clover;
@@ -61,7 +59,6 @@ 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;
@@ -74,10 +71,9 @@ ros::Duration state_timeout;
 ros::Duration velocity_timeout;
 ros::Duration global_position_timeout;
 ros::Duration battery_timeout;
-ros::Duration manual_control_timeout;
 float default_speed;
 bool auto_release;
-bool land_only_in_offboard, nav_from_sp, check_kill_switch;
+bool land_only_in_offboard, nav_from_sp;
 std::map<string, string> reference_frames;

 // Publishers
@@ -125,7 +121,6 @@ enum { YAW, YAW_RATE, TOWARDS } setpoint_yaw_type;
 // Last received telemetry messages
 mavros_msgs::State state;
 mavros_msgs::StatusText statustext;
-mavros_msgs::ManualControl manual_control;
 PoseStamped local_position;
 TwistStamped velocity;
 NavSatFix global_position;
@@ -150,9 +145,6 @@ 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));
@@ -185,10 +177,9 @@ inline bool waitTransform(const string& target, const string& source,
 		ros::spinOnce();
 		r.sleep();
 	}
-	return false;
 }

-#define TIMEOUT(msg, timeout) (msg.header.stamp.isZero() || (ros::Time::now() - msg.header.stamp > timeout))
+#define TIMEOUT(msg, timeout) (ros::Time::now() - msg.header.stamp > timeout)

 bool getTelemetry(GetTelemetry::Request& req, GetTelemetry::Response& res)
 {
@@ -446,10 +437,6 @@ void publish(const ros::Time stamp)

 		// 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;
@@ -498,27 +485,6 @@ void publishSetpoint(const ros::TimerEvent& event)
 	publish(event.current_real);
 }

-inline void checkManualControl()
-{
-	if (!manual_control_timeout.isZero() && TIMEOUT(manual_control, manual_control_timeout)) {
-		throw std::runtime_error("Manual control timeout, RC is switched off?");
-	}
-
-	if (check_kill_switch) {
-		// switch values: https://github.com/PX4/PX4-Autopilot/blob/c302514a0809b1765fafd13c014d705446ae1113/msg/manual_control_setpoint.msg#L3
-		const uint8_t SWITCH_POS_NONE = 0; // switch is not mapped
-		const uint8_t SWITCH_POS_ON = 1; // switch activated
-		const uint8_t SWITCH_POS_MIDDLE = 2; // middle position
-		const uint8_t SWITCH_POS_OFF = 3; // switch not activated
-
-		const int KILL_SWITCH_BIT = 12; // https://github.com/PX4/Firmware/blob/c302514a0809b1765fafd13c014d705446ae1113/src/modules/mavlink/mavlink_messages.cpp#L3975
-		uint8_t kill_switch = (manual_control.buttons & (0b11 << KILL_SWITCH_BIT)) >> KILL_SWITCH_BIT;
-
-		if (kill_switch == SWITCH_POS_ON)
-			throw std::runtime_error("Kill switch is on");
-	}
-}
-
 inline void checkState()
 {
 	if (TIMEOUT(state, state_timeout))
@@ -541,88 +507,25 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl
 		if (busy)
 			throw std::runtime_error("Busy");

+		ENSURE_FINITE(x);
+		ENSURE_FINITE(y);
+		ENSURE_FINITE(z);
+		ENSURE_FINITE(vx);
+		ENSURE_FINITE(vy);
+		ENSURE_FINITE(vz);
+		ENSURE_FINITE(pitch);
+		ENSURE_FINITE(roll);
+		ENSURE_FINITE(pitch_rate);
+		ENSURE_FINITE(roll_rate);
+		ENSURE_FINITE(lat);
+		ENSURE_FINITE(lon);
+		ENSURE_FINITE(thrust);
+
 		busy = true;

 		// Checks
 		checkState();

-		if (auto_arm) {
-			checkManualControl();
-		}
-
-		// default frame is local frame
-		if (frame_id.empty())
-			frame_id = local_frame;
-
-		// look up for reference frame
-		auto search = reference_frames.find(frame_id);
-		const string& reference_frame = search == reference_frames.end() ? frame_id : search->second;
-
-		// Serve "partial" commands
-
-		if (!auto_arm && std::isfinite(yaw) &&
-		    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);
-			ENSURE_FINITE(vy);
-			ENSURE_FINITE(vz);
-		} else if (sp_type == ATTITUDE) {
-			ENSURE_FINITE(pitch);
-			ENSURE_FINITE(roll);
-			ENSURE_FINITE(thrust);
-		} else if (sp_type == RATES) {
-			ENSURE_FINITE(pitch_rate);
-			ENSURE_FINITE(roll_rate);
-			ENSURE_FINITE(thrust);
-		}
-
 		if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL) {
 			if (TIMEOUT(local_position, local_position_timeout))
 				throw std::runtime_error("No local position, check settings");
@@ -647,6 +550,14 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl
 				throw std::runtime_error("No global position");
 		}

+		// default frame is local frame
+		if (frame_id.empty())
+			frame_id = local_frame;
+
+		// look up for reference frame
+		auto search = reference_frames.find(frame_id);
+		const string& reference_frame = search == reference_frames.end() ? frame_id : search->second;
+
 		if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == POSITION || sp_type == VELOCITY || sp_type == ATTITUDE) {
 			// make sure transform from frame_id to reference frame available
 			if (!waitTransform(reference_frame, frame_id, stamp, transform_timeout))
@@ -694,7 +605,7 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl
 		// }

 		if (sp_type == POSITION || sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == VELOCITY || sp_type == ATTITUDE) {
-			// destination point and/or attitude
+			// destination point and/or yaw
 			PoseStamped ps;
 			ps.header.frame_id = frame_id;
 			ps.header.stamp = stamp;
@@ -703,12 +614,7 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl
 			ps.pose.position.z = z;
 			ps.pose.orientation.w = 1.0; // Ensure quaternion is always valid

-			if (sp_type == ATTITUDE) {
-				ps.pose.position.x = 0;
-				ps.pose.position.y = 0;
-				ps.pose.position.z = 0;
-				ps.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);
-			} else if (std::isnan(yaw)) {
+			if (std::isnan(yaw)) {
 				setpoint_yaw_type = YAW_RATE;
 				setpoint_yaw_rate = yaw_rate;
 			} else if (std::isinf(yaw) && yaw > 0) {
@@ -719,14 +625,14 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl
 			} else {
 				setpoint_yaw_type = YAW;
 				setpoint_yaw_rate = 0;
-				ps.pose.orientation = tf::createQuaternionMsgFromYaw(yaw);
+				ps.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);
 			}

 			tf_buffer.transform(ps, setpoint_position, reference_frame);
 		}

 		if (sp_type == VELOCITY) {
-			Vector3Stamped vel;
+			static Vector3Stamped vel;
 			vel.header.frame_id = frame_id;
 			vel.header.stamp = stamp;
 			vel.vector.x = vx;
@@ -747,7 +653,6 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl

 		wait_armed = auto_arm;

-publish_setpoint:
 		publish(stamp); // calculate initial transformed messages first
 		setpoint_timer.start();

@@ -788,27 +693,27 @@ 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, 0, 0, 0, 0, 0, req.yaw, 0, 0, req.yaw_rate, 0, 0, 0, req.speed, req.frame_id, req.auto_arm, res.success, res.message);
 }

 bool navigateGlobal(NavigateGlobal::Request& req, NavigateGlobal::Response& res) {
-	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, 0, 0, req.z, 0, 0, 0, 0, 0, req.yaw, 0, 0, req.yaw_rate, req.lat, req.lon, 0, req.speed, req.frame_id, req.auto_arm, res.success, res.message);
 }

 bool setPosition(SetPosition::Request& req, SetPosition::Response& res) {
-	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, 0, 0, 0, 0, 0, req.yaw, 0, 0, req.yaw_rate, 0, 0, 0, 0, req.frame_id, req.auto_arm, res.success, res.message);
 }

 bool setVelocity(SetVelocity::Request& req, SetVelocity::Response& res) {
-	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, 0, 0, 0, req.vx, req.vy, req.vz, 0, 0, req.yaw, 0, 0, req.yaw_rate, 0, 0, 0, 0, req.frame_id, req.auto_arm, res.success, res.message);
 }

 bool setAttitude(SetAttitude::Request& req, SetAttitude::Response& res) {
-	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, 0, 0, 0, 0, 0, 0, req.pitch, req.roll, req.yaw, 0, 0, 0, 0, 0, req.thrust, 0, req.frame_id, req.auto_arm, res.success, res.message);
 }

 bool setRates(SetRates::Request& req, SetRates::Response& res) {
-	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, 0, 0, 0, 0, 0, 0, 0, 0, 0, req.pitch_rate, req.roll_rate, req.yaw_rate, 0, 0, req.thrust, 0, "", req.auto_arm, res.success, res.message);
 }

 bool land(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)
@@ -857,7 +762,6 @@ bool land(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)
 		busy = false;
 		return true;
 	}
-	return false;
 }

 int main(int argc, char **argv)
@@ -870,32 +774,22 @@ int main(int argc, char **argv)
 	static_transform_broadcaster = std::make_shared<tf2_ros::StaticTransformBroadcaster>();

 	// Params
-	nh_priv.param("mavros", mavros, string("mavros")); // for case of using multiple connections
-	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.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);
 	nh_priv.param("land_only_in_offboard", land_only_in_offboard, true);
 	nh_priv.param("nav_from_sp", nav_from_sp, true);
-	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.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));
 	global_position_timeout = ros::Duration(nh_priv.param("global_position_timeout", 10.0));
 	battery_timeout = ros::Duration(nh_priv.param("battery_timeout", 2.0));
-	manual_control_timeout = ros::Duration(nh_priv.param("manual_control_timeout", 0.0));

 	transform_timeout = ros::Duration(nh_priv.param("transform_timeout", 0.5));
 	telemetry_transform_timeout = ros::Duration(nh_priv.param("telemetry_transform_timeout", 0.5));
@@ -904,25 +798,24 @@ 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");
-	set_mode = nh.serviceClient<mavros_msgs::SetMode>(mavros + "/set_mode");
+	arming = nh.serviceClient<mavros_msgs::CommandBool>("mavros/cmd/arming");
+	set_mode = nh.serviceClient<mavros_msgs::SetMode>("mavros/set_mode");

 	// Telemetry subscribers
-	auto state_sub = nh.subscribe(mavros + "/state", 1, &handleState);
-	auto velocity_sub = nh.subscribe(mavros + "/local_position/velocity_body", 1, &handleMessage<TwistStamped, velocity>);
-	auto global_position_sub = nh.subscribe(mavros + "/global_position/global", 1, &handleMessage<NavSatFix, global_position>);
-	auto battery_sub = nh.subscribe(mavros + "/battery", 1, &handleMessage<BatteryState, battery>);
-	auto statustext_sub = nh.subscribe(mavros + "/statustext/recv", 1, &handleMessage<mavros_msgs::StatusText, statustext>);
-	auto 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 state_sub = nh.subscribe("mavros/state", 1, &handleState);
+	auto velocity_sub = nh.subscribe("mavros/local_position/velocity_body", 1, &handleMessage<TwistStamped, velocity>);
+	auto global_position_sub = nh.subscribe("mavros/global_position/global", 1, &handleMessage<NavSatFix, global_position>);
+	auto battery_sub = nh.subscribe("mavros/battery", 1, &handleMessage<BatteryState, battery>);
+	auto statustext_sub = nh.subscribe("mavros/statustext/recv", 1, &handleMessage<mavros_msgs::StatusText, statustext>);
+	auto local_position_sub = nh.subscribe("mavros/local_position/pose", 1, &handleLocalPosition);

 	// Setpoint publishers
-	position_pub = nh.advertise<PoseStamped>(mavros + "/setpoint_position/local", 1);
-	position_raw_pub = nh.advertise<PositionTarget>(mavros + "/setpoint_raw/local", 1);
-	attitude_pub = nh.advertise<PoseStamped>(mavros + "/setpoint_attitude/attitude", 1);
-	attitude_raw_pub = nh.advertise<AttitudeTarget>(mavros + "/setpoint_raw/attitude", 1);
-	rates_pub = nh.advertise<TwistStamped>(mavros + "/setpoint_attitude/cmd_vel", 1);
-	thrust_pub = nh.advertise<Thrust>(mavros + "/setpoint_attitude/thrust", 1);
+	position_pub = nh.advertise<PoseStamped>("mavros/setpoint_position/local", 1);
+	position_raw_pub = nh.advertise<PositionTarget>("mavros/setpoint_raw/local", 1);
+	attitude_pub = nh.advertise<PoseStamped>("mavros/setpoint_attitude/attitude", 1);
+	attitude_raw_pub = nh.advertise<AttitudeTarget>("mavros/setpoint_raw/attitude", 1);
+	rates_pub = nh.advertise<TwistStamped>("mavros/setpoint_attitude/cmd_vel", 1);
+	thrust_pub = nh.advertise<Thrust>("mavros/setpoint_attitude/thrust", 1);

 	 // Service servers
 	auto gt_serv = nh.advertiseService("get_telemetry", &getTelemetry);
--- a/clover/src/vpe_publisher.cpp
+++ b/clover/src/vpe_publisher.cpp
@@ -33,14 +33,14 @@ ros::Subscriber local_position_sub;
 ros::Timer zero_timer;
 PoseStamped vpe, pose;
 ros::Time got_local_pos(0);
-ros::Duration publish_zero_timeout, publish_zero_duration, offset_timeout;
+ros::Duration publish_zero_timout, publish_zero_duration, offset_timeout;
 TransformStamped offset;

 void publishZero(const ros::TimerEvent& e)
 {
-	if (!vpe.header.stamp.isZero() && e.current_real - vpe.header.stamp < publish_zero_timeout) return; // have vpe
+	if (e.current_real - vpe.header.stamp < publish_zero_timout) return; // have vpe

-	if (!pose.header.stamp.isZero() && e.current_real - pose.header.stamp < publish_zero_timeout) { // have local position
+	if (e.current_real - pose.header.stamp < publish_zero_timout) { // have local position
 		if (got_local_pos.isZero()) {
 			ROS_INFO("got local position");
 			got_local_pos = e.current_real;
@@ -53,7 +53,7 @@ void publishZero(const ros::TimerEvent& e)
 	}

 	ROS_INFO_THROTTLE(10, "publish zero");
-	geometry_msgs::PoseStamped zero;
+	static geometry_msgs::PoseStamped zero;
 	zero.header.frame_id = local_frame_id;
 	zero.header.stamp = e.current_real;
 	zero.pose.orientation.w = 1;
@@ -124,8 +124,8 @@ int main(int argc, char **argv) {

 	nh_priv.param<string>("frame_id", frame_id, "");
 	nh_priv.param<string>("offset_frame_id", offset_frame_id, "");
-	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");
+	nh_priv.param<string>("mavros/local_position/frame_id", local_frame_id, "map");
+	nh_priv.param<string>("mavros/local_position/tf/child_frame_id", child_frame_id, "base_link");
 	offset_timeout = ros::Duration(nh_priv.param("offset_timeout", 3.0));

 	if (!frame_id.empty()) {
@@ -141,11 +141,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("force_init", false) || nh_priv.param("publish_zero", false)) { // publish_zero is old name
+	if (nh_priv.param("publish_zero", false)) {
 		// publish zero to initialize the local position
 		zero_timer = nh.createTimer(ros::Duration(0.1), &publishZero);
-		publish_zero_timeout = ros::Duration(nh_priv.param("force_init_timeout", 5.0));
-		publish_zero_duration = ros::Duration(nh_priv.param("force_init_duration", 5.0));
+		publish_zero_timout = ros::Duration(nh_priv.param("publish_zero_timout", 5.0));
+		publish_zero_duration = ros::Duration(nh_priv.param("publish_zero_duration", 5.0));
 		local_position_sub = nh.subscribe("mavros/local_position/pose", 1, &localPositionCallback);
 	}

--- a/clover/src/waitfile
+++ b/clover/src/waitfile
@@ -1,9 +0,0 @@
-#!/usr/bin/env bash
-
-# $ ./waitfile <file> <command> <args...> 
-# wait until <file> appears and then invoke <command> with <args>
-
-echo "wait for file $1"
-while [ ! -e "$1" ]; do sleep 1; done;
-echo "file $1 appeared"
-exec "${@:2}"
--- a/clover/test/basic.py
+++ b/clover/test/basic.py
@@ -26,36 +26,21 @@ def test_simple_offboard_services_available():
    rospy.wait_for_service('land', timeout=5)

 def test_web_video_server(node):
-    try:
-        # Python 2
-        import urllib2 as urllib
-    except ModuleNotFoundError:
-        # Python 3
-        import urllib.request as urllib
-    urllib.urlopen("http://localhost:8080").read()
+    import urllib2
+    urllib2.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)
+def test_shell(node):
+    execute = rospy.ServiceProxy('exec', srv.Execute)
+    execute.wait_for_service(5)

-    from std_msgs.msg import String
-    from clover_blocks.srv import Run
+    res = execute(cmd='echo foo')
+    assert res.code == 0
+    assert res.output == 'foo\n'

-    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)
+    res = execute(cmd='foo')
+    assert res.code == 32512
+    assert res.output == ''

-    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'
+    res = execute(cmd='ls foo')
+    assert res.code == 512
+    assert res.output == ''
--- a/clover/test/basic.test
+++ b/clover/test/basic.test
@@ -23,7 +23,10 @@

    <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"/>

@@ -35,8 +38,6 @@
        <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"/>
-
    <param name="test_module" value="$(find clover)/test/basic.py"/>
    <test test-name="basic_test" pkg="ros_pytest" type="ros_pytest_runner"/>
 </launch>
--- a/clover/www/clover.log
+++ b/clover/www/clover.log
@@ -1 +0,0 @@
-/var/log/clover.log
--- a/clover/www/clover_version
+++ b/clover/www/clover_version
@@ -1 +0,0 @@
-/etc/clover_version
--- a/clover/www/console.html
+++ b/clover/www/console.html
@@ -1,23 +0,0 @@
-<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>
--- a/clover/www/index.html
+++ b/clover/www/index.html
@@ -1,36 +1,24 @@
-<title>Clover Drone Kit Tools</title>
-
 <h1>Clover Drone Kit Tools</h1>

 <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>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="../clover_blocks/">Blocks programming</a> (<code>Blockly</code>)</li>
-	<li><a href="console.html">Clover console</a> (<code>/var/log/clover.log</code>)</li>
+	<li><a href="viz.html">View 3D visualization</a> (<code>ros3djs</code>)</li>
+	<li><a href="aruco_map.html">3D visualization for markers map</a> (<code>ros3djs</code>)</li>
 </ul>

 <div class="version"></div>

+<script src="js/roslib.js"></script>
 <script type="text/javascript">
 	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;
-		response.text().then(function(text) {
-			document.querySelector('.version').innerHTML = 'Version: ' + text;
-		});
+	var ros = new ROSLIB.Ros({ url: 'ws://' + location.hostname + ':9090' });
+	var exec = new ROSLIB.Service({ ros: ros, name : '/exec', serviceType : 'clover/Execute' });
+	exec.callService(new ROSLIB.ServiceRequest({ cmd: 'cat /etc/clover_version' }), function(result) {
+		document.querySelector('.version').innerHTML = 'Version: ' + result.output;
 	});
 </script>
--- a/clover/www/js/json-to-pretty-yaml.js
+++ b/clover/www/js/json-to-pretty-yaml.js
@@ -1,236 +0,0 @@
-// 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]);
--- a/clover/www/js/topics.js
+++ b/clover/www/js/topics.js
@@ -1,83 +0,0 @@
-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 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) {
-		document.title = topic;
-		if (mouseDown) return;
-
-		if (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;
-			}
-		}
-
-		topicMessage.innerHTML = yamlStringify(msg); // JSON.stringify(msg, null, 4);
-	});
-}
-
-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);
-		});
-	}
-}
--- a/clover/www/topics.html
+++ b/clover/www/topics.html
@@ -1,28 +0,0 @@
-<html lang="en">
-	<head>
-		<title>ROS topics</title>
-		<script src="js/roslib.js"></script>
-		<link rel="icon" href="data:,"> <!-- make chrome don't request icon -->
-		<script type="module" src="js/topics.js"></script>
-		<script src="js/json-to-pretty-yaml.js"></script>
-		<style>
-			#topics { line-height: 1.2em; }
-			#topic-view {
-				display: none;
-			}
-			#topic-message {
-				display: none;
-				white-space: pre;
-				font-family: monospace;
-			}
-			#topic-type { font-family: monospace; font-size: 0.5em; vertical-align: super; font-weight: normal; }
-			.topic { font-family: monospace; }
-			body.closed { background-color: rgb(207, 207, 207); }
-		</style>
-	</head>
-	<body>
-		<h1>&nbsp;</h1>
-		<ul id="topics"></ul>
-		<code id="topic-message">No messages received</code>
-	</body>
-</html>
--- a/clover_blocks/CHANGELOG.rst
+++ b/clover_blocks/CHANGELOG.rst
@@ -1,8 +0,0 @@
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-Changelog for package clover_blocks
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-
-0.21.1 (2020-11-17)
-------------------
-* First release of clover_blocks package to ROS
-* Contributors: Oleg Kalachev
--- a/clover_blocks/CMakeLists.txt
+++ b/clover_blocks/CMakeLists.txt
@@ -1,94 +0,0 @@
-cmake_minimum_required(VERSION 2.8.3)
-project(clover_blocks)
-
-find_package(catkin REQUIRED COMPONENTS message_generation)
-
-add_message_files(
-  FILES
-  Prompt.msg
-)
-
-add_service_files(
-  FILES
-  Run.srv
-  Load.srv
-  Store.srv
-)
-
-generate_messages(
-  DEPENDENCIES
-  # std_msgs  # Or other packages containing msgs
-)
-
-catkin_package(
-#  INCLUDE_DIRS include
-#  LIBRARIES roslaunch_editor
-  CATKIN_DEPENDS message_runtime
-#  DEPENDS system_lib
-)
-
-#############
-## Install ##
-#############
-
-# all install targets should use catkin DESTINATION variables
-# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
-
-## Mark executable scripts (Python etc.) for installation
-## in contrast to setup.py, you can choose the destination
-# install(PROGRAMS
-#   scripts/my_python_script
-#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark executables for installation
-## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
-# install(TARGETS ${PROJECT_NAME}_node
-#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark libraries for installation
-## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
-# install(TARGETS ${PROJECT_NAME}
-#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
-# )
-
-## Mark cpp header files for installation
-# install(DIRECTORY include/${PROJECT_NAME}/
-#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-#   FILES_MATCHING PATTERN "*.h"
-#   PATTERN ".svn" EXCLUDE
-# )
-
-## Mark other files for installation (e.g. launch and bag files, etc.)
-# install(FILES
-#   # myfile1
-#   # myfile2
-#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-# )
-
-catkin_install_python(PROGRAMS src/clover_blocks
-  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-)
-
-# TODO: store programs in home directory?
-install(DIRECTORY programs
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-)
-
-install(DIRECTORY www DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
-
-#############
-## Testing ##
-#############
-
-## Add gtest based cpp test target and link libraries
-# catkin_add_gtest(${PROJECT_NAME}-test test/test_roslaunch_editor.cpp)
-# if(TARGET ${PROJECT_NAME}-test)
-#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
-# endif()
-
-## Add folders to be run by python nosetests
-# catkin_add_nosetests(test)
--- a/clover_blocks/README.md
+++ b/clover_blocks/README.md
@@ -1,55 +0,0 @@
-# clover_blocks
-
-Blockly programming support for Clover.
-
-<img src="screenshot.png" width=700>
-
-See user documentation at the [main Clover documentation site](https://clover.coex.tech/en/blocks.html).
-
-Internal package documentation is given below.
-
-## Frontend
-
-The frontend files are located in [`www`](./www/) subdirectory. The frontend application uses [`roslib.js`](http://wiki.ros.org/roslibjs) library for communicating with backend node and other ROS resources.
-
-## `clover_blocks` node
-
-`clover_blocks` is the blocks programming backend, implementing all the services and topics needed for running Blockly-generated Python script.
-
-### Services
-
-* `~run` ([*clover_blocks/Run*](srv/Run.srv)) – run Blockly-generated program (in Python).
-* `~stop` ([*std_srvs/Trigger*](http://docs.ros.org/noetic/api/std_srvs/html/srv/Trigger.html)) – terminate the running program.
-* `~store` ([*clover_blocks/load*](srv/Store.srv)) – store a user program (to `<package_path>/programs` by default).
-* `~load` ([*clover_blocks/load*](srv/Load.srv)) – load all the stored programs.
-
-### Parameters
-
-* `~programs_dir` (*string*) – directory for user programs.
-
-Parameters read by frontend:
-
-* `~navigate_tolerance` (*float*) – distance tolerance in meters, used for navigate-like blocks (default: 0.2).
-* `~navigate_global_tolerance` (*float*) – distance tolerance for global coordinates navigation (default: 1).
-* `~yaw_tolerance` (*float*) – yaw angle tolerance in degrees, used in set_yaw block (default: 1).
-* `~sleep_time` (*float*) – duration of sleep in loop cycles, used for navigate-like blocks (default: 0.2).
-* `~confirm_run` (*bool*) – enable confirmation to run the program (default: true).
-
-These parameters also can be set as URL GET-parameters, for example:
-
-```
-http://<hostname>/clover_blocks/?navigate_tolerance=0.5&sleep_time=0.1
-```
-
-### Topics
-
-#### Published
-
-* `~running` ([*std_msgs/Bool*](http://docs.ros.org/noetic/api/std_msgs/html/msg/Bool.html)) – indicates if the program is currently running.
-* `~block` ([*std_msgs/String*](http://docs.ros.org/noetic/api/std_msgs/html/msg/String.html)) – current executing block (maximum topic rate is limited).
-* `~error` ([*std_msgs/String*](http://docs.ros.org/noetic/api/std_msgs/html/msg/String.html)) – user program errors and exceptions.
-* `~prompt` ([*clover_blocks/Prompt*](msg/Prompt.msg)) – user input request (includes random request ID string).
-
-This topic is published from the frontend side:
-
-* `~prompt/<request_id>` ([*std_msgs/String*](http://docs.ros.org/noetic/api/std_msgs/html/msg/String.html)) – user input response.
--- a/clover_blocks/msg/Prompt.msg
+++ b/clover_blocks/msg/Prompt.msg
@@ -1,2 +0,0 @@
-string message  # message for prompt
-string id  # user response should be published to ~input/<id> topic
--- a/clover_blocks/package.xml
+++ b/clover_blocks/package.xml
@@ -1,48 +0,0 @@
-<?xml version="1.0"?>
-<package format="2">
-  <name>clover_blocks</name>
-  <version>0.23.0</version>
-  <description>Blockly programming support for Clover</description>
-  <maintainer email="okalachev@gmail.com">Oleg Kalachev</maintainer>
-  <license>MIT</license>
-
-  <!-- Url tags are optional, but multiple are allowed, one per tag -->
-  <!-- Optional attribute type can be: website, bugtracker, or repository -->
-  <url type="repository">https://github.com/CopterExpress/clover</url>
-
-  <!-- Author tags are optional, multiple are allowed, one per tag -->
-  <!-- Authors do not have to be maintainers, but could be -->
-  <author email="okalachev@gmail.com">Oleg Kalachev</author>
-
-  <!-- The *depend tags are used to specify dependencies -->
-  <!-- Dependencies can be catkin packages or system dependencies -->
-  <!-- Examples: -->
-  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
-  <!--   <depend>roscpp</depend> -->
-  <!--   Note that this is equivalent to the following: -->
-  <!--   <build_depend>roscpp</build_depend> -->
-  <!--   <exec_depend>roscpp</exec_depend> -->
-  <!-- Use build_depend for packages you need at compile time: -->
-  <!--   <build_depend>message_generation</build_depend> -->
-  <!-- Use build_export_depend for packages you need in order to build against this package: -->
-  <!--   <build_export_depend>message_generation</build_export_depend> -->
-  <!-- Use buildtool_depend for build tool packages: -->
-  <!--   <buildtool_depend>catkin</buildtool_depend> -->
-  <!-- Use exec_depend for packages you need at runtime: -->
-  <!--   <exec_depend>message_runtime</exec_depend> -->
-  <!-- Use test_depend for packages you need only for testing: -->
-  <!--   <test_depend>gtest</test_depend> -->
-  <!-- Use doc_depend for packages you need only for building documentation: -->
-  <!--   <doc_depend>doxygen</doc_depend> -->
-  <buildtool_depend>catkin</buildtool_depend>
-
-  <depend>message_generation</depend>
-  <depend>message_runtime</depend>
-  <depend>rospy</depend>
-
-  <!-- The export tag contains other, unspecified, tags -->
-  <export>
-    <!-- Other tools can request additional information be placed here -->
-
-  </export>
-</package>
--- a/clover_blocks/programs/examples/aruco-map.xml
+++ b/clover_blocks/programs/examples/aruco-map.xml
@@ -1,73 +0,0 @@
-<xml xmlns="https://developers.google.com/blockly/xml">
-  <block type="take_off" id="w4,Hi[}0b.mX68K|hOJv" x="38" y="63">
-    <field name="WAIT">TRUE</field>
-    <value name="ALT">
-      <shadow type="math_number" id="q!Ln!d`jutn*3Gy56B0A">
-        <field name="NUM">1.5</field>
-      </shadow>
-    </value>
-    <next>
-      <block type="controls_repeat_ext" id="fD/HWb]kb!^pYK{UkM;0">
-        <value name="TIMES">
-          <shadow type="math_number" id="%FP0k|kaf~A`0N;M{Oiy">
-            <field name="NUM">10</field>
-          </shadow>
-        </value>
-        <statement name="DO">
-          <block type="navigate" id="qM5ROiq4r:$lq}8U_wol">
-            <field name="FRAME_ID">ARUCO_MAP</field>
-            <field name="WAIT">TRUE</field>
-            <value name="X">
-              <shadow type="math_number" id="=MBV;9zGH8S`Xh57/E;C">
-                <field name="NUM">0</field>
-              </shadow>
-              <block type="text_prompt_ext" id="rUDMP2~yFR!7E`Q]pR/i">
-                <mutation type="NUMBER"></mutation>
-                <field name="TYPE">NUMBER</field>
-                <value name="TEXT">
-                  <shadow type="text" id="z+Q3vS?)kxHl?HTcuI?(">
-                    <field name="TEXT">Enter X</field>
-                  </shadow>
-                </value>
-              </block>
-            </value>
-            <value name="Y">
-              <shadow type="math_number" id="%$`av8A.w}vNqG-]1GOn">
-                <field name="NUM">0</field>
-              </shadow>
-              <block type="text_prompt_ext" id=".AjhH^Rh*i}+14s)K-:g">
-                <mutation type="NUMBER"></mutation>
-                <field name="TYPE">NUMBER</field>
-                <value name="TEXT">
-                  <shadow type="text" id="x#)(kg5PB;0uZA5;{(GT">
-                    <field name="TEXT">Enter Y</field>
-                  </shadow>
-                </value>
-              </block>
-            </value>
-            <value name="Z">
-              <shadow type="math_number" id="T,]q+-@V;tCYLy$-(o3}">
-                <field name="NUM">1.5</field>
-              </shadow>
-            </value>
-            <value name="ID">
-              <shadow type="math_number" id="VRo?s#0d)Gl@9fsK@gR=">
-                <field name="NUM">0</field>
-              </shadow>
-            </value>
-            <value name="SPEED">
-              <shadow type="math_number" id="mmZc+d=W})w.AN*B_dSe">
-                <field name="NUM">0.5</field>
-              </shadow>
-            </value>
-          </block>
-        </statement>
-        <next>
-          <block type="land" id="JB%yj/X~K=8A2=Q*7cRs">
-            <field name="WAIT">TRUE</field>
-          </block>
-        </next>
-      </block>
-    </next>
-  </block>
-</xml>
--- a/clover_blocks/programs/examples/aruco-marker.xml
+++ b/clover_blocks/programs/examples/aruco-marker.xml
@@ -1,64 +0,0 @@
-<xml xmlns="https://developers.google.com/blockly/xml">
-  <block type="take_off" id="R@=3~Fq;AxFaRO{ZDFUM" x="37" y="63">
-    <field name="WAIT">TRUE</field>
-    <value name="ALT">
-      <shadow type="math_number" id="cf8/,N48N;Fvn@EWB%,B">
-        <field name="NUM">1.5</field>
-      </shadow>
-    </value>
-    <next>
-      <block type="controls_repeat_ext" id="qc$F96[A^%lj,-l[fhX+">
-        <value name="TIMES">
-          <shadow type="math_number" id="v0QgNIpGzMIb`P@8D@}.">
-            <field name="NUM">3</field>
-          </shadow>
-        </value>
-        <statement name="DO">
-          <block type="navigate" id="]kCOMt@NSf$9:N1Fx~dE">
-            <field name="FRAME_ID">ARUCO</field>
-            <field name="WAIT">TRUE</field>
-            <value name="X">
-              <shadow type="math_number" id="}(7P5,{6%^::Gzb;kRaf">
-                <field name="NUM">0</field>
-              </shadow>
-            </value>
-            <value name="Y">
-              <shadow type="math_number" id="hz#sFvY/;IL5IRLQVa.K">
-                <field name="NUM">0</field>
-              </shadow>
-            </value>
-            <value name="Z">
-              <shadow type="math_number" id="n0ULZn64%k.:,l(,D?TZ">
-                <field name="NUM">1</field>
-              </shadow>
-            </value>
-            <value name="ID">
-              <shadow type="math_number" id="P4^twyJo6as#G1}^Dlq8">
-                <field name="NUM">0</field>
-              </shadow>
-              <block type="text_prompt_ext" id="ot;{twL$8`H*Wiafz3D,">
-                <mutation type="NUMBER"></mutation>
-                <field name="TYPE">NUMBER</field>
-                <value name="TEXT">
-                  <shadow type="text" id="@P[TyPj[n1*;EcUC?lwF">
-                    <field name="TEXT">Enter marker ID</field>
-                  </shadow>
-                </value>
-              </block>
-            </value>
-            <value name="SPEED">
-              <shadow type="math_number" id="WrsUXacZ2w)D]d?:@ec_">
-                <field name="NUM">0.5</field>
-              </shadow>
-            </value>
-          </block>
-        </statement>
-        <next>
-          <block type="land" id="G5+Fs.6Y4(K9Dw`wjgua">
-            <field name="WAIT">TRUE</field>
-          </block>
-        </next>
-      </block>
-    </next>
-  </block>
-</xml>
--- a/clover_blocks/programs/examples/flight-led.xml
+++ b/clover_blocks/programs/examples/flight-led.xml
@@ -1,97 +0,0 @@
-<xml xmlns="https://developers.google.com/blockly/xml">
-  <block type="take_off" id="R@=3~Fq;AxFaRO{ZDFUM" x="162" y="63">
-    <field name="WAIT">TRUE</field>
-    <value name="ALT">
-      <shadow type="math_number" id="cf8/,N48N;Fvn@EWB%,B">
-        <field name="NUM">1</field>
-      </shadow>
-    </value>
-    <next>
-      <block type="set_effect" id="5K8qDD0/@Ab!k+ad6IPj">
-        <field name="EFFECT">FILL</field>
-        <value name="COLOR">
-          <shadow type="colour_picker" id="=*Y*L:1DI7y$.RR$6jNQ">
-            <field name="COLOUR">#ff0000</field>
-          </shadow>
-        </value>
-        <next>
-          <block type="navigate" id="kE7TL3+{PI.GiXby~SyO">
-            <field name="FRAME_ID">BODY</field>
-            <field name="WAIT">TRUE</field>
-            <value name="X">
-              <shadow type="math_number" id="W$eQ*Xv/e(*`e%8--W:P">
-                <field name="NUM">1</field>
-              </shadow>
-            </value>
-            <value name="Y">
-              <shadow type="math_number" id="i~r{%5mpU|}FL|9p!5yG">
-                <field name="NUM">0</field>
-              </shadow>
-            </value>
-            <value name="Z">
-              <shadow type="math_number" id="fZ{Cz~^LoWvM#,#;!co_">
-                <field name="NUM">0</field>
-              </shadow>
-            </value>
-            <value name="ID">
-              <shadow type="math_number" id="z$Hj|-?4e3zy@Jx$/THh">
-                <field name="NUM">0</field>
-              </shadow>
-            </value>
-            <value name="SPEED">
-              <shadow type="math_number" id="FjRM:aex=:/l_``^zcO]">
-                <field name="NUM">0.5</field>
-              </shadow>
-            </value>
-            <next>
-              <block type="set_effect" id="Q^DQoW/6G$7#YPlXc}%]">
-                <field name="EFFECT">FILL</field>
-                <value name="COLOR">
-                  <shadow type="colour_picker" id="mjOY6]=uMY4$r8^n:n|}">
-                    <field name="COLOUR">#3333ff</field>
-                  </shadow>
-                </value>
-                <next>
-                  <block type="navigate" id=";|j@sXR{9)9@ran7g[;~">
-                    <field name="FRAME_ID">BODY</field>
-                    <field name="WAIT">TRUE</field>
-                    <value name="X">
-                      <shadow type="math_number" id="TH(htYn}*/a{[::sFQ[x">
-                        <field name="NUM">-1</field>
-                      </shadow>
-                    </value>
-                    <value name="Y">
-                      <shadow type="math_number" id="h@`=v?y+Tuh,I)wv?+`f">
-                        <field name="NUM">0</field>
-                      </shadow>
-                    </value>
-                    <value name="Z">
-                      <shadow type="math_number" id="pzz:U%?m/~~B$-FF!fD?">
-                        <field name="NUM">0</field>
-                      </shadow>
-                    </value>
-                    <value name="ID">
-                      <shadow type="math_number" id="Ze2Nun;[K{X.1kE0anMF">
-                        <field name="NUM">0</field>
-                      </shadow>
-                    </value>
-                    <value name="SPEED">
-                      <shadow type="math_number" id="+[A5TSXF:hw4T1M]e~Wu">
-                        <field name="NUM">0.5</field>
-                      </shadow>
-                    </value>
-                    <next>
-                      <block type="land" id="G5+Fs.6Y4(K9Dw`wjgua">
-                        <field name="WAIT">TRUE</field>
-                      </block>
-                    </next>
-                  </block>
-                </next>
-              </block>
-            </next>
-          </block>
-        </next>
-      </block>
-    </next>
-  </block>
-</xml>
--- a/clover_blocks/programs/examples/flip.xml
+++ b/clover_blocks/programs/examples/flip.xml
@@ -1,308 +0,0 @@
-<xml xmlns="https://developers.google.com/blockly/xml">
-  <variables>
-    <variable id="MEu_bi$Xk5e(#Q`Yu=k0">start_x</variable>
-    <variable id="GtS.)8^|(~RL*T4.~#VV">start_y</variable>
-    <variable id="jksdQ,]0F47Xan8{_(?+">start_z</variable>
-  </variables>
-  <block type="procedures_defnoreturn" id="Uo{/lI4(1i[O|U^jyh]y" x="188" y="62">
-    <field name="NAME">flip</field>
-    <comment pinned="false" h="80" w="160">Describe this function...</comment>
-    <statement name="STACK">
-      <block type="procedures_callnoreturn" id="G7`l2OBCg~ISh(XsrkRv">
-        <mutation name="memorize position"></mutation>
-        <next>
-          <block type="setpoint" id="8%(,J3=!CEUALDr5gFT+">
-            <field name="TYPE">RATES</field>
-            <field name="FRAME_ID">BODY</field>
-            <comment pinned="false" h="80" w="160">bump up</comment>
-            <value name="VX">
-              <shadow type="math_number" id="!@rHW+cG*U6.Py:.;qc)">
-                <field name="NUM">0</field>
-              </shadow>
-            </value>
-            <value name="VY">
-              <shadow type="math_number" id="n#I~l7$%1m^@Oz+w;k[H">
-                <field name="NUM">0</field>
-              </shadow>
-            </value>
-            <value name="VZ">
-              <shadow type="math_number" id="l0EphD#!^4jX#%UA-rxX">
-                <field name="NUM">0</field>
-              </shadow>
-            </value>
-            <value name="PITCH">
-              <shadow type="math_number" id="Ad.[@07QtZcb;(:^,Kjg">
-                <field name="NUM">0</field>
-              </shadow>
-            </value>
-            <value name="ROLL">
-              <shadow type="math_number" id="50,,4i.p6KR:e3/l|k2i">
-                <field name="NUM">0</field>
-              </shadow>
-            </value>
-            <value name="YAW">
-              <shadow type="math_number" id="(:O}={3*LZ#KDiC@;b[F">
-                <field name="NUM">0</field>
-              </shadow>
-            </value>
-            <value name="THRUST">
-              <shadow type="math_number" id="EC6yU;oHb#y-8?W`B/JC">
-                <field name="NUM">1</field>
-              </shadow>
-            </value>
-            <value name="ID">
-              <shadow type="math_number" id="k.X^bynEZH2dyG1=Y6lr">
-                <field name="NUM">0</field>
-              </shadow>
-            </value>
-            <next>
-              <block type="wait" id="#ga)cR4TsF[9t?;zc1H~">
-                <value name="TIME">
-                  <shadow type="math_number" id="zg5fX7[;O,lQt*Txg|tn">
-                    <field name="NUM">0.2</field>
-                  </shadow>
-                </value>
-                <next>
-                  <block type="setpoint" id="j,yf}/lrx*2J}X%^5LG{">
-                    <field name="TYPE">RATES</field>
-                    <field name="FRAME_ID">BODY</field>
-                    <comment pinned="false" h="80" w="160">maximum pitch rate</comment>
-                    <value name="VX">
-                      <shadow type="math_number" id="^WF12EBPyIXhZdpsj;=D">
-                        <field name="NUM">0</field>
-                      </shadow>
-                    </value>
-                    <value name="VY">
-                      <shadow type="math_number" id="C/uBsz]ObzwyHxGqzUP_">
-                        <field name="NUM">0</field>
-                      </shadow>
-                    </value>
-                    <value name="VZ">
-                      <shadow type="math_number" id="4?)O3G/j`UuVTMImLq{T">
-                        <field name="NUM">0</field>
-                      </shadow>
-                    </value>
-                    <value name="PITCH">
-                      <shadow type="math_number" id="sdCVFTmVkc0v2|*3Fy~h">
-                        <field name="NUM">30</field>
-                      </shadow>
-                    </value>
-                    <value name="ROLL">
-                      <shadow type="math_number" id="V1o!AV[`!OnhrUP4~$=!">
-                        <field name="NUM">0</field>
-                      </shadow>
-                    </value>
-                    <value name="YAW">
-                      <shadow type="math_number" id="0e;`*B;ghV*%=ETSvzSM">
-                        <field name="NUM">0</field>
-                      </shadow>
-                    </value>
-                    <value name="THRUST">
-                      <shadow type="math_number" id="K@VAeOH2bEwc#@7_M!@l">
-                        <field name="NUM">0.2</field>
-                      </shadow>
-                    </value>
-                    <value name="ID">
-                      <shadow type="math_number" id="c6ycgS!D}bIK8pl8oAv+">
-                        <field name="NUM">0</field>
-                      </shadow>
-                    </value>
-                    <next>
-                      <block type="controls_whileUntil" id="H_6do.wf6H.E$?5;$uw8">
-                        <field name="MODE">WHILE</field>
-                        <value name="BOOL">
-                          <block type="logic_boolean" id="pu%GH9}wZvUV{RnGk6Se">
-                            <field name="BOOL">TRUE</field>
-                          </block>
-                        </value>
-                        <statement name="DO">
-                          <block type="controls_if" id="l0w?CSzU]S(K9eL-1(1r">
-                            <value name="IF0">
-                              <block type="logic_operation" id="gtXXJ7OFs=1BynL@|R{.">
-                                <field name="OP">OR</field>
-                                <value name="A">
-                                  <block type="logic_compare" id="2%9ns$Jk(8M[8$~9o(=*">
-                                    <field name="OP">GT</field>
-                                    <value name="A">
-                                      <block type="math_single" id="#gr7h89vpJyc8bnbWU9O">
-                                        <field name="OP">ABS</field>
-                                        <value name="NUM">
-                                          <shadow type="math_number" id="5paq?]87z}}+@~NALY}.">
-                                            <field name="NUM">9</field>
-                                          </shadow>
-                                          <block type="get_attitude" id="awk0-~@[jWFVQ+ZPi=Dc">
-                                            <field name="FIELD">PITCH</field>
-                                          </block>
-                                        </value>
-                                      </block>
-                                    </value>
-                                    <value name="B">
-                                      <block type="math_number" id=",4p]fd#kwxy`DtCMtraW">
-                                        <field name="NUM">90</field>
-                                      </block>
-                                    </value>
-                                  </block>
-                                </value>
-                                <value name="B">
-                                  <block type="logic_compare" id="?TPG3^?eg4l~CKM,3DfO">
-                                    <field name="OP">GT</field>
-                                    <value name="A">
-                                      <block type="math_single" id="yS*/-+E3TMpZ]%`1j*Up">
-                                        <field name="OP">ABS</field>
-                                        <value name="NUM">
-                                          <shadow type="math_number">
-                                            <field name="NUM">9</field>
-                                          </shadow>
-                                          <block type="get_attitude" id="~E3RS!on]G(?IZ?SVA#J">
-                                            <field name="FIELD">ROLL</field>
-                                          </block>
-                                        </value>
-                                      </block>
-                                    </value>
-                                    <value name="B">
-                                      <block type="math_number" id="#y??I}u#OmOTK:`!pkNS">
-                                        <field name="NUM">90</field>
-                                      </block>
-                                    </value>
-                                  </block>
-                                </value>
-                              </block>
-                            </value>
-                            <statement name="DO0">
-                              <block type="controls_flow_statements" id="X|V=r3BDKw9fv)u5EY3.">
-                                <field name="FLOW">BREAK</field>
-                              </block>
-                            </statement>
-                          </block>
-                        </statement>
-                        <next>
-                          <block type="procedures_callnoreturn" id="?;sq{#xJ+]bD`yJ#(]o1">
-                            <mutation name="navigate to memorized position"></mutation>
-                          </block>
-                        </next>
-                      </block>
-                    </next>
-                  </block>
-                </next>
-              </block>
-            </next>
-          </block>
-        </next>
-      </block>
-    </statement>
-  </block>
-  <block type="procedures_defnoreturn" id="!Tv!F*.A}=]dW=C6X@-`" x="538" y="62">
-    <field name="NAME">memorize position</field>
-    <comment pinned="false" h="80" w="160">Describe this function...</comment>
-    <statement name="STACK">
-      <block type="variables_set" id="s,o|q7_A/fSV+c0E)b^k">
-        <field name="VAR" id="MEu_bi$Xk5e(#Q`Yu=k0">start_x</field>
-        <value name="VALUE">
-          <block type="get_position" id="f#,.s%hV^19JJ;G/DO):">
-            <field name="FIELD">X</field>
-            <field name="FRAME_ID">MAP</field>
-            <value name="ID">
-              <shadow type="math_number" id="rJRRE#;#JFg}gY+34LVi">
-                <field name="NUM">0</field>
-              </shadow>
-            </value>
-          </block>
-        </value>
-        <next>
-          <block type="variables_set" id="E+2Pfn3JOB{r=DQ3f1b`">
-            <field name="VAR" id="GtS.)8^|(~RL*T4.~#VV">start_y</field>
-            <value name="VALUE">
-              <block type="get_position" id="J#y+_prX(DI;j8gFO81H">
-                <field name="FIELD">Y</field>
-                <field name="FRAME_ID">MAP</field>
-                <value name="ID">
-                  <shadow type="math_number" id="_bnV`SCa:|Vl#sRlYA@}">
-                    <field name="NUM">0</field>
-                  </shadow>
-                </value>
-              </block>
-            </value>
-            <next>
-              <block type="variables_set" id="t*TI?p-h#qC_kC)jM8q0">
-                <field name="VAR" id="jksdQ,]0F47Xan8{_(?+">start_z</field>
-                <value name="VALUE">
-                  <block type="get_position" id="Xh#8yLEMxT|bQ}jYvk6/">
-                    <field name="FIELD">Z</field>
-                    <field name="FRAME_ID">MAP</field>
-                    <value name="ID">
-                      <shadow type="math_number" id="z3FTv/jt^H7dVYx1`$$C">
-                        <field name="NUM">0</field>
-                      </shadow>
-                    </value>
-                  </block>
-                </value>
-              </block>
-            </next>
-          </block>
-        </next>
-      </block>
-    </statement>
-  </block>
-  <block type="procedures_defnoreturn" id="$p3=p^fBz}k5Pg1]svL," x="563" y="213">
-    <field name="NAME">navigate to memorized position</field>
-    <comment pinned="false" h="80" w="160">Describe this function...</comment>
-    <statement name="STACK">
-      <block type="navigate" id="#7b*(J_t9t)V,Arb)3;R">
-        <field name="FRAME_ID">MAP</field>
-        <field name="WAIT">TRUE</field>
-        <value name="X">
-          <shadow type="math_number">
-            <field name="NUM">0</field>
-          </shadow>
-          <block type="variables_get" id="X#O!NcMac+a9WKL;l7?W">
-            <field name="VAR" id="MEu_bi$Xk5e(#Q`Yu=k0">start_x</field>
-          </block>
-        </value>
-        <value name="Y">
-          <shadow type="math_number">
-            <field name="NUM">0</field>
-          </shadow>
-          <block type="variables_get" id="C(=1.r0+01|e3+3qs%SD">
-            <field name="VAR" id="GtS.)8^|(~RL*T4.~#VV">start_y</field>
-          </block>
-        </value>
-        <value name="Z">
-          <shadow type="math_number">
-            <field name="NUM">0</field>
-          </shadow>
-          <block type="variables_get" id="?v;YVETEO%vUqgmiro^/">
-            <field name="VAR" id="jksdQ,]0F47Xan8{_(?+">start_z</field>
-          </block>
-        </value>
-        <value name="ID">
-          <shadow type="math_number" id="ALt.9P$^_]upK}Q9_M,5">
-            <field name="NUM">0</field>
-          </shadow>
-        </value>
-        <value name="SPEED">
-          <shadow type="math_number" id="ObO|OuF^d,1|Bx{Y`O-R">
-            <field name="NUM">5</field>
-          </shadow>
-        </value>
-      </block>
-    </statement>
-  </block>
-  <block type="take_off" id="I1oP(b3wChmK?WgPtULz" x="563" y="612">
-    <field name="WAIT">TRUE</field>
-    <value name="ALT">
-      <shadow type="math_number" id="N1EX*JgURuG783yO+B^X">
-        <field name="NUM">2</field>
-      </shadow>
-    </value>
-    <next>
-      <block type="procedures_callnoreturn" id="(Dc3I{36Kt@1KHN7Ef.m">
-        <mutation name="flip"></mutation>
-        <next>
-          <block type="land" id=";c+lM(v6`13$VK`~jZ|5">
-            <field name="WAIT">TRUE</field>
-          </block>
-        </next>
-      </block>
-    </next>
-  </block>
-</xml>
--- a/clover_blocks/programs/examples/functions.xml
+++ b/clover_blocks/programs/examples/functions.xml
@@ -1,184 +0,0 @@
-<xml xmlns="https://developers.google.com/blockly/xml">
-  <variables>
-    <variable id="MEu_bi$Xk5e(#Q`Yu=k0">start_x</variable>
-    <variable id="GtS.)8^|(~RL*T4.~#VV">start_y</variable>
-    <variable id="jksdQ,]0F47Xan8{_(?+">start_z</variable>
-  </variables>
-  <block type="procedures_defnoreturn" id="X0i)DAnNpakX]gr;CC0}" x="588" y="88">
-    <field name="NAME">memorize position</field>
-    <comment pinned="false" h="80" w="160">Describe this function...</comment>
-    <statement name="STACK">
-      <block type="variables_set" id="Ma4s0pE-i`P:(-|?c88v">
-        <field name="VAR" id="MEu_bi$Xk5e(#Q`Yu=k0">start_x</field>
-        <value name="VALUE">
-          <block type="get_position" id="~uz[sPf[(s,15#x]3tAw">
-            <field name="FIELD">X</field>
-            <field name="FRAME_ID">MAP</field>
-            <value name="ID">
-              <shadow type="math_number" id="V#rB*5s3W^*q75}gScM(">
-                <field name="NUM">0</field>
-              </shadow>
-            </value>
-          </block>
-        </value>
-        <next>
-          <block type="variables_set" id="I07,0;W#gn1CNY697O9n">
-            <field name="VAR" id="GtS.)8^|(~RL*T4.~#VV">start_y</field>
-            <value name="VALUE">
-              <block type="get_position" id="z(5A+nNX@tWO[J/X6Hb(">
-                <field name="FIELD">Y</field>
-                <field name="FRAME_ID">MAP</field>
-                <value name="ID">
-                  <shadow type="math_number" id="6dM^zy88YqX`3ch{IH{1">
-                    <field name="NUM">0</field>
-                  </shadow>
-                </value>
-              </block>
-            </value>
-            <next>
-              <block type="variables_set" id="AGgAbxz?+u_2^h;Xo4oc">
-                <field name="VAR" id="jksdQ,]0F47Xan8{_(?+">start_z</field>
-                <value name="VALUE">
-                  <block type="get_position" id="k~cMq+jvE^,Ip*kKUmw]">
-                    <field name="FIELD">Z</field>
-                    <field name="FRAME_ID">MAP</field>
-                    <value name="ID">
-                      <shadow type="math_number" id="Q8RU-5,2UG*IfCeZg0cy">
-                        <field name="NUM">0</field>
-                      </shadow>
-                    </value>
-                  </block>
-                </value>
-              </block>
-            </next>
-          </block>
-        </next>
-      </block>
-    </statement>
-  </block>
-  <block type="take_off" id=".0#}q|m$C-GD,.fM/9:/" x="137" y="162">
-    <field name="WAIT">TRUE</field>
-    <value name="ALT">
-      <shadow type="math_number" id="H6G+npH8DS90OnBKP;qy">
-        <field name="NUM">2</field>
-      </shadow>
-    </value>
-    <next>
-      <block type="procedures_callnoreturn" id="_r_hGwEy6uksz}DtrwxW">
-        <mutation name="memorize position"></mutation>
-        <next>
-          <block type="navigate" id="e{_?bCxQ}za#e~~A2GmJ">
-            <field name="FRAME_ID">ARUCO_MAP</field>
-            <field name="WAIT">TRUE</field>
-            <value name="X">
-              <shadow type="math_number" id="}P]lFX9(He:*{Rvp8[.q">
-                <field name="NUM">0</field>
-              </shadow>
-              <block type="text_prompt_ext" id="L:~.WP`isNJ/TBBP75.{">
-                <mutation type="NUMBER"></mutation>
-                <field name="TYPE">NUMBER</field>
-                <value name="TEXT">
-                  <shadow type="text" id="v87(;,:4$T8wgbByRv`z">
-                    <field name="TEXT">Input X</field>
-                  </shadow>
-                </value>
-              </block>
-            </value>
-            <value name="Y">
-              <shadow type="math_number" id="eMbKtnF{B^P*p7zRD8SX">
-                <field name="NUM">0</field>
-              </shadow>
-              <block type="text_prompt_ext" id="fp[sLcO^N:q}9k6zYwnN">
-                <mutation type="NUMBER"></mutation>
-                <field name="TYPE">NUMBER</field>
-                <value name="TEXT">
-                  <shadow type="text" id="2Bqk0Rm@h*Vef,,:0c!+">
-                    <field name="TEXT">Input Y</field>
-                  </shadow>
-                </value>
-              </block>
-            </value>
-            <value name="Z">
-              <shadow type="math_number" id="1sLNKuRa}07Ek(eHK;/S">
-                <field name="NUM">2</field>
-              </shadow>
-            </value>
-            <value name="ID">
-              <shadow type="math_number" id="guM,}nHUz^a?_H3(b{FW">
-                <field name="NUM">0</field>
-              </shadow>
-            </value>
-            <value name="SPEED">
-              <shadow type="math_number" id=".EckPx#fD.[:qSk}5/-J">
-                <field name="NUM">0.5</field>
-              </shadow>
-            </value>
-            <next>
-              <block type="text_print" id="d,aouAR0nE80mk6qTAti">
-                <value name="TEXT">
-                  <shadow type="text" id="Yr%NvOOvCnrBP8}m]bn`">
-                    <field name="TEXT">Point reached. Going back.</field>
-                  </shadow>
-                </value>
-                <next>
-                  <block type="procedures_callnoreturn" id="B0o:6T%+wpg{O%s27:bH">
-                    <mutation name="navigate to memorized position"></mutation>
-                    <next>
-                      <block type="land" id="~%g;%HDPtF!lqHY}KO@/">
-                        <field name="WAIT">TRUE</field>
-                      </block>
-                    </next>
-                  </block>
-                </next>
-              </block>
-            </next>
-          </block>
-        </next>
-      </block>
-    </next>
-  </block>
-  <block type="procedures_defnoreturn" id="POzAJb%[yRgBrEn9K*x{" x="587" y="362">
-    <field name="NAME">navigate to memorized position</field>
-    <comment pinned="false" h="80" w="160">Describe this function...</comment>
-    <statement name="STACK">
-      <block type="navigate" id="VX|`@L*bU=aY`4!l=Pbw">
-        <field name="FRAME_ID">BODY</field>
-        <field name="WAIT">TRUE</field>
-        <value name="X">
-          <shadow type="math_number" id="c$==spjA=Vin//l1(1V@">
-            <field name="NUM">0</field>
-          </shadow>
-          <block type="variables_get" id="LxP$oVs*8[{CH4o^RY._">
-            <field name="VAR" id="MEu_bi$Xk5e(#Q`Yu=k0">start_x</field>
-          </block>
-        </value>
-        <value name="Y">
-          <shadow type="math_number" id="a)/FEG4j=.584f/:2$?H">
-            <field name="NUM">0</field>
-          </shadow>
-          <block type="variables_get" id="_-Ib6#usbp0NS}DZ7OGS">
-            <field name="VAR" id="GtS.)8^|(~RL*T4.~#VV">start_y</field>
-          </block>
-        </value>
-        <value name="Z">
-          <shadow type="math_number" id="q*MDfPe,V]rfhC1pnltq">
-            <field name="NUM">0</field>
-          </shadow>
-          <block type="variables_get" id="VY4xW2Y|=*AH3`s2C@)5">
-            <field name="VAR" id="jksdQ,]0F47Xan8{_(?+">start_z</field>
-          </block>
-        </value>
-        <value name="ID">
-          <shadow type="math_number" id="OiuYE,a3PqUW.vst+Qc$">
-            <field name="NUM">0</field>
-          </shadow>
-        </value>
-        <value name="SPEED">
-          <shadow type="math_number" id="X2Ou?:=kt:`aOq,#Q-)+">
-            <field name="NUM">0.5</field>
-          </shadow>
-        </value>
-      </block>
-    </statement>
-  </block>
-</xml>
--- a/clover_blocks/programs/examples/led-compass.xml
+++ b/clover_blocks/programs/examples/led-compass.xml
@@ -1,91 +0,0 @@
-<xml xmlns="https://developers.google.com/blockly/xml">
-  <block type="controls_whileUntil" id="U1it{GcGuSS:=[xiwZr1" x="113" y="113">
-    <field name="MODE">WHILE</field>
-    <value name="BOOL">
-      <block type="logic_boolean" id="]7ZDRwde}/RqjQCX}aVW">
-        <field name="BOOL">TRUE</field>
-      </block>
-    </value>
-    <statement name="DO">
-      <block type="set_effect" id="WI0zqOz/z3].cR/6UWHn">
-        <field name="EFFECT">FILL</field>
-        <value name="COLOR">
-          <shadow type="colour_picker" id="B`6;Xv{s2TFp8Yd=ZpSD">
-            <field name="COLOUR">#000000</field>
-          </shadow>
-        </value>
-        <next>
-          <block type="set_led" id="^Vcs}ki?#ctf7rAchix$">
-            <value name="INDEX">
-              <shadow type="math_number" id="U;VWW$[*LOF7Gf,~?YR7">
-                <field name="NUM">0</field>
-              </shadow>
-              <block type="math_arithmetic" id="AI6PZBd`]_Z%_~4c-%dB">
-                <field name="OP">MULTIPLY</field>
-                <value name="A">
-                  <shadow type="math_number" id="|p}X]`SedK3).F/;}NlB">
-                    <field name="NUM">1</field>
-                  </shadow>
-                  <block type="math_arithmetic" id="-haE#:,cg{-J=NZERA;F">
-                    <field name="OP">DIVIDE</field>
-                    <value name="A">
-                      <shadow type="math_number" id="::st;ot}[r]csqceURu*">
-                        <field name="NUM">1</field>
-                      </shadow>
-                      <block type="math_arithmetic" id="a%+LN)F~=Igg+,p02[qo">
-                        <field name="OP">ADD</field>
-                        <value name="A">
-                          <shadow type="math_number" id="*yIGN((y)/^z0:f5:VDw">
-                            <field name="NUM">1</field>
-                          </shadow>
-                          <block type="get_yaw" id="mf%77q30bEqNfc/3`Mtb">
-                            <field name="FRAME_ID">MAP</field>
-                            <value name="ID">
-                              <shadow type="math_number" id="xb32G.N#ip`|^Xv*MOmY">
-                                <field name="NUM">0</field>
-                              </shadow>
-                            </value>
-                          </block>
-                        </value>
-                        <value name="B">
-                          <shadow type="math_number" id="T/fTrm;j2Azgav;gI{ZW">
-                            <field name="NUM">180</field>
-                          </shadow>
-                        </value>
-                      </block>
-                    </value>
-                    <value name="B">
-                      <shadow type="math_number" id="Wo1/ZCeir,u6/.e1H+BB">
-                        <field name="NUM">360</field>
-                      </shadow>
-                    </value>
-                  </block>
-                </value>
-                <value name="B">
-                  <shadow type="math_number" id="jENTcXz0C5/=)Xpd!}LL">
-                    <field name="NUM">1</field>
-                  </shadow>
-                  <block type="led_count" id="vM@X8s!xa]v}AaK6PWF5"></block>
-                </value>
-              </block>
-            </value>
-            <value name="COLOR">
-              <shadow type="colour_picker" id="+vw3bff.5c[=_w,Xm^C(">
-                <field name="COLOUR">#3366ff</field>
-              </shadow>
-            </value>
-            <next>
-              <block type="wait" id="DT%f$bn1*1El5zsgUW8Y">
-                <value name="TIME">
-                  <shadow type="math_number" id="~Y0hNY[_^#v@aZkE-TH[">
-                    <field name="NUM">0.1</field>
-                  </shadow>
-                </value>
-              </block>
-            </next>
-          </block>
-        </next>
-      </block>
-    </statement>
-  </block>
-</xml>
--- a/clover_blocks/programs/examples/led.xml
+++ b/clover_blocks/programs/examples/led.xml
@@ -1,30 +0,0 @@
-<xml xmlns="https://developers.google.com/blockly/xml">
-  <block type="controls_whileUntil" id="A:_Z_27?K?HlScug%u|5" x="113" y="113">
-    <field name="MODE">WHILE</field>
-    <value name="BOOL">
-      <block type="logic_boolean" id="/rlzePJ6XJcv22J(Q4bs">
-        <field name="BOOL">TRUE</field>
-      </block>
-    </value>
-    <statement name="DO">
-      <block type="set_effect" id="GT~AX(j]r)u^,f_n0agS">
-        <field name="EFFECT">WIPE</field>
-        <value name="COLOR">
-          <shadow type="colour_picker" id="G`I)ZAuIGcnWNyO@N(sH">
-            <field name="COLOUR">#ff0000</field>
-          </shadow>
-          <block type="colour_random" id="}(${|~%[}eJ.QUY?FWi_"></block>
-        </value>
-        <next>
-          <block type="wait" id="Ux(J;l+?5Gq2n^*!jXj^">
-            <value name="TIME">
-              <shadow type="math_number" id=")As9bASkedr9x@M)*)Pf">
-                <field name="NUM">1</field>
-              </shadow>
-            </value>
-          </block>
-        </next>
-      </block>
-    </statement>
-  </block>
-</xml>
--- a/clover_blocks/programs/examples/print-range.xml
+++ b/clover_blocks/programs/examples/print-range.xml
@@ -1,29 +0,0 @@
-<xml xmlns="https://developers.google.com/blockly/xml">
-  <block type="controls_whileUntil" id="^{D$=!y=ahJx0.^n?Ycr" x="88" y="137">
-    <field name="MODE">WHILE</field>
-    <value name="BOOL">
-      <block type="logic_boolean" id="a;~UtFPf9O3B)#QBJxL6">
-        <field name="BOOL">TRUE</field>
-      </block>
-    </value>
-    <statement name="DO">
-      <block type="text_print" id="doqF#i]/ulE{Z0l/{9`K">
-        <value name="TEXT">
-          <shadow type="text" id="Y$c,rIaw^bNs2wIw%[^(">
-            <field name="TEXT">abc</field>
-          </shadow>
-          <block type="rangefinder_distance" id="Pi+WekHZvZ0p}JY-^CLQ"></block>
-        </value>
-        <next>
-          <block type="wait" id="=$,8TaKwL75Pb,_tsCVU">
-            <value name="TIME">
-              <shadow type="math_number" id="/r+hzH#G0Ru#ES+}MxUl">
-                <field name="NUM">0.5</field>
-              </shadow>
-            </value>
-          </block>
-        </next>
-      </block>
-    </statement>
-  </block>
-</xml>
--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
Alexey Rogachevskiy	3460fec25e	travis: Use short commit notation	2020-08-10 00:19:05 +03:00
Alexey Rogachevskiy	fa44e4b42f	travis: Fix IMAGE_VERSION initialization	2020-08-09 01:00:56 +03:00
Alexey Rogachevskiy	4bc985a7f4	travis: Generate zip file after successful build	2020-08-08 21:26:06 +03:00
Alexey Rogachevskiy	c2bfa07a36	travis: Fix parsing errors	2020-08-07 17:36:56 +03:00
Alexey Rogachevskiy	6ba27ef15d	travis: Output compressed image size	2020-08-07 17:28:48 +03:00