Enable strict warnings for aruco_pose

As this flag breaks vision position aiding

.github/workflows/build-image.yaml

@@ -0,0 +1,29 @@
+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 }}

.github/workflows/build.yml

@@ -0,0 +1,23 @@
+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

.github/workflows/docs.yml

@@ -0,0 +1,68 @@
+name: Documentation
+
+on:
+  push:
+    branches: [ '*' ]
+  pull_request:
+    branches: [ master ]
+
+jobs:
+  docs:
+    runs-on: ubuntu-18.04
+    steps:
+      - name: Cancel previous runs
+        uses: styfle/cancel-workflow-action@0.9.1
+        with:
+          access_token: ${{ github.token }}
+      - 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: Deploy
+        uses: JamesIves/github-pages-deploy-action@4.1.3
+        if: ${{ github.event_name == 'push' && github.ref == 'refs/heads/master' }}
+        with:
+          branch: gh-pages
+          folder: _book
+          clean: true
+          single-commit: true # to avoid multiple copies of large pdf files

.github/workflows/editorconfig.yaml

@@ -0,0 +1,18 @@
+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"

.gitignore

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

.markdownlint.json

@@ -21,6 +21,7 @@
             "ROS",
             "ROS Kinetic",
             "ROS Melodic",
+            "ROS Noetic",
             "OpenCV",
             "OpenVPN",
             "Gazebo",
@@ -109,7 +110,8 @@
             "Li-ion",
             "Nvidia",
             "VirtualBox",
-            "VMware"
+            "VMware",
+            "DuoCam"
         ],
         "code_blocks": false
     },

.travis.yml

@@ -1,123 +0,0 @@
-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: 1
-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 -9 ${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
-        - for i in 1 2 3 4; do gitbook pdf ./ _book/clover.pdf && break || sleep 1; done
-        - sudo apt-get 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
-      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: 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|blockly/|clover_blocks/programs/|highlight/|python.js|Assets.xcassets|test_parser_pass.txt|test_node_failure.txt|aruco_pose/vendor|\.stl|\.dxf|\.dae"
-stages:
-  - Build
-# 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/

README.md

@@ -1,6 +1,6 @@
 # clover🍀: create autonomous drones easily
 
-<img src="docs/assets/clover42-main.png" align="right" width="400px" alt="COEX Clover Drone">
+<img src="docs/assets/clover42-main-margin.png" align="right" width="400px" alt="COEX Clover Drone">
 
 Clover is an open source [ROS](https://www.ros.org)-based framework, providing user-friendly tools to control [PX4](https://px4.io)-powered drones. Clover is available as a ROS package, but is shipped mainly as a preconfigured image for Raspberry Pi. Once you've installed Raspberry Pi on your drone and flashed the image to its microSD card, taking the drone up in the air is a matter of minutes.
 
@@ -20,12 +20,13 @@ 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).
 
-[![Build Status](https://travis-ci.org/CopterExpress/clover.svg?branch=master)](https://travis-ci.org/CopterExpress/clover)
+![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)
 
 Image features:
 
 * Raspbian Buster
-* [ROS Melodic](http://wiki.ros.org/melodic)
+* [ROS Noetic](http://wiki.ros.org/noetic)
 * Configured networking
 * OpenCV
 * [`mavros`](http://wiki.ros.org/mavros)
@@ -37,6 +38,10 @@ 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.

apps/ios/cleverrc/README.md

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

aruco_pose/CMakeLists.txt

@@ -4,6 +4,8 @@ project(aruco_pose)
 ## Compile as C++11, supported in ROS Kinetic and newer
 add_compile_options(-std=c++11)
 
+add_compile_options(-Wall -Wextra -Werror)
+
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
@@ -22,13 +24,21 @@ find_package(catkin REQUIRED COMPONENTS
   dynamic_reconfigure
 )
 
-find_package(OpenCV 3 REQUIRED COMPONENTS core imgproc calib3d)
+# 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)
 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 3 REQUIRED COMPONENTS aruco)
+  find_package(OpenCV ${_opencv_version} REQUIRED COMPONENTS aruco)
 endif()
 message(STATUS "OpenCV include dirs: ${OpenCV_INCLUDE_DIRS}")
 message(STATUS "OpenCV libraries: ${OpenCV_LIBRARIES}")
@@ -75,11 +85,10 @@ add_message_files(
 )
 
 ## Generate services in the 'srv' folder
-# add_service_files(
-#   FILES
-#   Service1.srv
-#   Service2.srv
-# )
+add_service_files(
+  FILES
+  SetMarkers.srv
+)
 
 ## Generate actions in the 'action' folder
 # add_action_files(
@@ -111,7 +120,7 @@ generate_messages(
 
 ## Generate dynamic reconfigure parameters in the 'cfg' folder
 generate_dynamic_reconfigure_options(
-  cfg/DetectorParams.cfg
+  cfg/Detector.cfg
 )
 
 ###################################
@@ -172,6 +181,13 @@ 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 ##
 #############
@@ -187,11 +203,11 @@ target_link_libraries(aruco_pose
 # )
 
 ## 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}
-# )
+install(TARGETS ${PROJECT_NAME}
+  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}/
@@ -207,6 +223,14 @@ target_link_libraries(aruco_pose
 #   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 ##
 #############

aruco_pose/README.md

@@ -51,6 +51,7 @@ It's recommended to run it within the same nodelet manager with the camera nodel
 
 * `image_raw` (*sensor_msgs/Image*) – camera image
 * `camera_info` (*sensor_msgs/CameraInfo*) – camera calibration info
+* `map_markers` (*aruco_pose/MarkerArray*) – list of markers to disable TF transform publishing
 
 #### Published
 
@@ -70,7 +71,7 @@ It's recommended to run it within the same nodelet manager with the camera nodel
 
 * `~map` – path to text file with markers list
 * `~frame_id` – published frame id (default: `aruco_map`)
-* `~known_tilt` – debug image width
+* `~known_tilt` – known tilt (pitch and roll) of markers map as a frame
 * `~image_width` – debug image width (default: 2000)
 * `~image_height` – debug image height (default: 2000)
 * `~image_margin` – debug image margin (default: 200)
@@ -97,6 +98,7 @@ See examples in [`map`](map/) directory.
 #### Published
 
 * `~pose` (*geometry_msgs/PoseWithCovarianceStamped*) – estimated map pose
+* `~map` (*aruco_pose/MarkerArray*) – list of markers in the loaded map
 * `~image` (*sensor_msgs/Image*) – planarized map image
 * `~visualization` (*visualization_msgs/MarkerArray*) – markers map visualization for rviz
 * `~debug` (*sensor_msgs/Image*) – debug image with detected markers and map axis

aruco_pose/cfg/Detector.cfg

@@ -10,6 +10,8 @@ gen = ParameterGenerator()
 
 gen.add("enabled", bool_t, 0, "if detection enabled", True)
 
+gen.add("length", double_t, 0, "markers' side length", min=0, max=10)
+
 gen.add("adaptiveThreshConstant",                 double_t, 0,
         "Constant for adaptive thresholding before finding contours",
          p.adaptiveThreshConstant, 0, 100)

aruco_pose/map/cmit.txt

@@ -1,3 +1,4 @@
+# id	length	x	y	z	rot_z	rot_y	rot_x
 0	0.33	0.0	9.0	0	0	0	0	
 1	0.33	1.0	9.0	0	0	0	0	
 2	0.33	2.0	9.0	0	0	0	0	

aruco_pose/map/office.txt

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

aruco_pose/map/office_ceiling.txt

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

aruco_pose/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>aruco_pose</name>
-  <version>0.21.1</version>
+  <version>0.23.0</version>
   <description>Positioning with ArUco markers</description>
 
   <maintainer email="okalachev@gmail.com">Oleg Kalachev</maintainer>

aruco_pose/src/aruco_detect.cpp

@@ -48,6 +48,7 @@
 #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>
@@ -69,8 +70,10 @@ private:
 	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_;
@@ -97,6 +100,7 @@ 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);
@@ -111,17 +115,16 @@ public:
 		image_transport::ImageTransport it(nh_);
 		image_transport::ImageTransport it_priv(nh_priv_);
 
-		map_markers_sub_ = nh_.subscribe("map_markers", 1, &ArucoDetect::mapMarkersCallback, this);
+		dyn_srv_ = std::make_shared<dynamic_reconfigure::Server<aruco_pose::DetectorConfig>>(nh_priv_);
+		dyn_srv_->setCallback(std::bind(&ArucoDetect::paramCallback, this, std::placeholders::_1, std::placeholders::_2));
+
+		set_markers_srv_ = nh_priv_.advertiseService("set_length_override", &ArucoDetect::setMarkers, this);
+
 		debug_pub_ = it_priv.advertise("debug", 1);
 		markers_pub_ = nh_priv_.advertise<aruco_pose::MarkerArray>("markers", 1);
 		vis_markers_pub_ = nh_priv_.advertise<visualization_msgs::MarkerArray>("visualization", 1);
 		img_sub_ = it.subscribeCamera("image_raw", 1, &ArucoDetect::imageCallback, 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);
+		map_markers_sub_ = nh_.subscribe("map_markers", 1, &ArucoDetect::mapMarkersCallback, this);
 
 		NODELET_INFO("ready");
 	}
@@ -175,7 +178,7 @@ private:
 				if (!known_tilt_.empty()) {
 					try {
 						snap_to = tf_buffer_->lookupTransform(msg->header.frame_id, known_tilt_,
-						                                      msg->header.stamp, ros::Duration(0.02));
+						                                      msg->header.stamp, transform_timeout_);
 					} catch (const tf2::TransformException& e) {
 						NODELET_WARN_THROTTLE(5, "can't snap: %s", e.what());
 					}
@@ -349,6 +352,29 @@ 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();
@@ -359,7 +385,8 @@ private:
 
 	void paramCallback(aruco_pose::DetectorConfig &config, uint32_t level)
 	{
-		enabled_ = config.enabled;
+		enabled_ = config.enabled && config.length > 0;
+		length_ = config.length;
 		parameters_->adaptiveThreshConstant = config.adaptiveThreshConstant;
 		parameters_->adaptiveThreshWinSizeMin = config.adaptiveThreshWinSizeMin;
 		parameters_->adaptiveThreshWinSizeMax = config.adaptiveThreshWinSizeMax;

aruco_pose/src/aruco_map.cpp

@@ -89,7 +89,7 @@ public:
 
 		// TODO: why image_transport doesn't work here?
 		img_pub_ = nh_priv_.advertise<sensor_msgs::Image>("image", 1, true);
-		markers_pub_ = nh_priv_.advertise<aruco_pose::MarkerArray>("markers", 1, true);
+		markers_pub_ = nh_priv_.advertise<aruco_pose::MarkerArray>("map", 1, true);
 
 		board_ = cv::makePtr<cv::aruco::Board>();
 		board_->dictionary = cv::aruco::getPredefinedDictionary(
@@ -124,6 +124,11 @@ public:
 		vis_markers_pub_ = nh_priv_.advertise<visualization_msgs::MarkerArray>("visualization", 1, true);
 		debug_pub_ = it_priv.advertise("debug", 1);
 
+		publishMarkersFrames();
+		publishMarkers();
+		publishMapImage();
+		vis_markers_pub_.publish(vis_array_);
+
 		image_sub_.subscribe(nh_, "image_raw", 1);
 		info_sub_.subscribe(nh_, "camera_info", 1);
 		markers_sub_.subscribe(nh_, "markers", 1);
@@ -131,11 +136,6 @@ public:
 		sync_.reset(new message_filters::Synchronizer<SyncPolicy>(SyncPolicy(10), image_sub_, info_sub_, markers_sub_));
 		sync_->registerCallback(boost::bind(&ArucoMap::callback, this, _1, _2, _3));
 
-		publishMarkersFrames();
-		publishMarkers();
-		publishMapImage();
-		vis_markers_pub_.publish(vis_array_);
-
 		NODELET_INFO("ready");
 	}
 

aruco_pose/src/draw.cpp

@@ -3,26 +3,11 @@
 
 #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) {
 
@@ -142,35 +127,194 @@ void _drawPlanarBoard(Board *_board, Size outSize, OutputArray _img, int marginS
 	}
 }
 
-/* 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)
+/**
+ * @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)
 {
-	// If both points are behind the screen, don't draw anything
-	if (p1.z <= 0 && p2.z <= 0) {
-		return;
+	// 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);
 	}
-	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;
-		}
+	else
+	{
+		rmat = rvec_64f.clone();
 	}
-	line(image, p1p, p2p, color, thickness, lineType, shift);
+	// 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?
 }
 
 void _drawAxis(InputOutputArray _image, InputArray _cameraMatrix, InputArray _distCoeffs,
@@ -186,647 +330,23 @@ 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));
-	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())
+	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)
 	{
-		_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)));
+		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);
 	}
-
-	_cvProjectPoints2(&c_objectPoints, &c_rvec, &c_tvec, &c_cameraMatrix, &c_distCoeffs,
-					  &c_imagePoints, pdpdrot, pdpdt, pdpdf, pdpdc, pdpddist, aspectRatio);
-}
-
-namespace _detail
-{
-    template <typename FLOAT>
-    void computeTiltProjectionMatrix(FLOAT tauX,
-                                     FLOAT tauY,
-                                     Matx<FLOAT, 3, 3>* matTilt = 0,
-                                     Matx<FLOAT, 3, 3>* dMatTiltdTauX = 0,
-                                     Matx<FLOAT, 3, 3>* dMatTiltdTauY = 0,
-                                     Matx<FLOAT, 3, 3>* invMatTilt = 0)
-    {
-        FLOAT cTauX = cos(tauX);
-        FLOAT sTauX = sin(tauX);
-        FLOAT cTauY = cos(tauY);
-        FLOAT sTauY = sin(tauY);
-        Matx<FLOAT, 3, 3> matRotX = Matx<FLOAT, 3, 3>(1,0,0,0,cTauX,sTauX,0,-sTauX,cTauX);
-        Matx<FLOAT, 3, 3> matRotY = Matx<FLOAT, 3, 3>(cTauY,0,-sTauY,0,1,0,sTauY,0,cTauY);
-        Matx<FLOAT, 3, 3> matRotXY = matRotY * matRotX;
-        Matx<FLOAT, 3, 3> matProjZ = Matx<FLOAT, 3, 3>(matRotXY(2,2),0,-matRotXY(0,2),0,matRotXY(2,2),-matRotXY(1,2),0,0,1);
-        if (matTilt)
-        {
-            // Matrix for trapezoidal distortion of tilted image sensor
-            *matTilt = matProjZ * matRotXY;
-        }
-        if (dMatTiltdTauX)
-        {
-            // Derivative with respect to tauX
-            Matx<FLOAT, 3, 3> dMatRotXYdTauX = matRotY * Matx<FLOAT, 3, 3>(0,0,0,0,-sTauX,cTauX,0,-cTauX,-sTauX);
-            Matx<FLOAT, 3, 3> dMatProjZdTauX = Matx<FLOAT, 3, 3>(dMatRotXYdTauX(2,2),0,-dMatRotXYdTauX(0,2),
-                                                                 0,dMatRotXYdTauX(2,2),-dMatRotXYdTauX(1,2),0,0,0);
-            *dMatTiltdTauX = (matProjZ * dMatRotXYdTauX) + (dMatProjZdTauX * matRotXY);
-        }
-        if (dMatTiltdTauY)
-        {
-            // Derivative with respect to tauY
-            Matx<FLOAT, 3, 3> dMatRotXYdTauY = Matx<FLOAT, 3, 3>(-sTauY,0,-cTauY,0,0,0,cTauY,0,-sTauY) * matRotX;
-            Matx<FLOAT, 3, 3> dMatProjZdTauY = Matx<FLOAT, 3, 3>(dMatRotXYdTauY(2,2),0,-dMatRotXYdTauY(0,2),
-                                                                 0,dMatRotXYdTauY(2,2),-dMatRotXYdTauY(1,2),0,0,0);
-            *dMatTiltdTauY = (matProjZ * dMatRotXYdTauY) + (dMatProjZdTauY * matRotXY);
-        }
-        if (invMatTilt)
-        {
-            FLOAT inv = 1./matRotXY(2,2);
-            Matx<FLOAT, 3, 3> invMatProjZ = Matx<FLOAT, 3, 3>(inv,0,inv*matRotXY(0,2),0,inv,inv*matRotXY(1,2),0,0,1);
-            *invMatTilt = matRotXY.t()*invMatProjZ;
-        }
-    }
-}
-
-static const char* cvDistCoeffErr = "Distortion coefficients must be 1x4, 4x1, 1x5, 5x1, 1x8, 8x1, 1x12, 12x1, 1x14 or 14x1 floating-point vector";
-
-static void _cvProjectPoints2Internal( const CvMat* objectPoints,
-                                      const CvMat* r_vec,
-                                      const CvMat* t_vec,
-                                      const CvMat* A,
-                                      const CvMat* distCoeffs,
-                                      CvMat* imagePoints, CvMat* dpdr CV_DEFAULT(NULL),
-                                      CvMat* dpdt CV_DEFAULT(NULL), CvMat* dpdf CV_DEFAULT(NULL),
-                                      CvMat* dpdc CV_DEFAULT(NULL), CvMat* dpdk CV_DEFAULT(NULL),
-                                      CvMat* dpdo CV_DEFAULT(NULL),
-                                      double aspectRatio CV_DEFAULT(0) )
-{
-    Ptr<CvMat> matM, _m;
-    Ptr<CvMat> _dpdr, _dpdt, _dpdc, _dpdf, _dpdk;
-    Ptr<CvMat> _dpdo;
-
-    int i, j, count;
-    int calc_derivatives;
-    const CvPoint3D64f* M;
-    CvPoint3D64f* m;
-    double r[3], R[9], dRdr[27], t[3], a[9], k[14] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, fx, fy, cx, cy;
-    Matx33d matTilt = Matx33d::eye();
-    Matx33d dMatTiltdTauX(0,0,0,0,0,0,0,-1,0);
-    Matx33d dMatTiltdTauY(0,0,0,0,0,0,1,0,0);
-    CvMat _r, _t, _a = cvMat( 3, 3, CV_64F, a ), _k;
-    CvMat matR = cvMat( 3, 3, CV_64F, R ), _dRdr = cvMat( 3, 9, CV_64F, dRdr );
-    double *dpdr_p = 0, *dpdt_p = 0, *dpdk_p = 0, *dpdf_p = 0, *dpdc_p = 0;
-    double* dpdo_p = 0;
-    int dpdr_step = 0, dpdt_step = 0, dpdk_step = 0, dpdf_step = 0, dpdc_step = 0;
-    int dpdo_step = 0;
-    bool fixedAspectRatio = aspectRatio > FLT_EPSILON;
-
-    if( !CV_IS_MAT(objectPoints) || !CV_IS_MAT(r_vec) ||
-        !CV_IS_MAT(t_vec) || !CV_IS_MAT(A) ||
-        /*!CV_IS_MAT(distCoeffs) ||*/ !CV_IS_MAT(imagePoints) )
-        CV_Error( CV_StsBadArg, "One of required arguments is not a valid matrix" );
-
-    int total = objectPoints->rows * objectPoints->cols * CV_MAT_CN(objectPoints->type);
-    if(total % 3 != 0)
-    {
-        //we have stopped support of homogeneous coordinates because it cause ambiguity in interpretation of the input data
-        CV_Error( CV_StsBadArg, "Homogeneous coordinates are not supported" );
-    }
-    count = total / 3;
-
-    if( CV_IS_CONT_MAT(objectPoints->type) &&
-        (CV_MAT_DEPTH(objectPoints->type) == CV_32F || CV_MAT_DEPTH(objectPoints->type) == CV_64F)&&
-        ((objectPoints->rows == 1 && CV_MAT_CN(objectPoints->type) == 3) ||
-         (objectPoints->rows == count && CV_MAT_CN(objectPoints->type)*objectPoints->cols == 3) ||
-         (objectPoints->rows == 3 && CV_MAT_CN(objectPoints->type) == 1 && objectPoints->cols == count)))
-    {
-        matM.reset(cvCreateMat( objectPoints->rows, objectPoints->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(objectPoints->type)) ));
-        cvConvert(objectPoints, matM);
-    }
-    else
-    {
-//        matM = cvCreateMat( 1, count, CV_64FC3 );
-//        cvConvertPointsHomogeneous( objectPoints, matM );
-        CV_Error( CV_StsBadArg, "Homogeneous coordinates are not supported" );
-    }
-
-    if( CV_IS_CONT_MAT(imagePoints->type) &&
-        (CV_MAT_DEPTH(imagePoints->type) == CV_32F || CV_MAT_DEPTH(imagePoints->type) == CV_64F) &&
-        ((imagePoints->rows == 1 && CV_MAT_CN(imagePoints->type) == 3) ||
-         (imagePoints->rows == count && CV_MAT_CN(imagePoints->type)*imagePoints->cols == 3) ||
-         (imagePoints->rows == 3 && CV_MAT_CN(imagePoints->type) == 1 && imagePoints->cols == count)))
-    {
-        _m.reset(cvCreateMat( imagePoints->rows, imagePoints->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(imagePoints->type)) ));
-        cvConvert(imagePoints, _m);
-    }
-    else
-    {
-//        _m = cvCreateMat( 1, count, CV_64FC2 );
-        CV_Error( CV_StsBadArg, "Homogeneous coordinates are not supported" );
-    }
-
-    M = (CvPoint3D64f*)matM->data.db;
-    m = (CvPoint3D64f*)_m->data.db;
-
-    if( (CV_MAT_DEPTH(r_vec->type) != CV_64F && CV_MAT_DEPTH(r_vec->type) != CV_32F) ||
-        (((r_vec->rows != 1 && r_vec->cols != 1) ||
-          r_vec->rows*r_vec->cols*CV_MAT_CN(r_vec->type) != 3) &&
-         ((r_vec->rows != 3 && r_vec->cols != 3) || CV_MAT_CN(r_vec->type) != 1)))
-        CV_Error( CV_StsBadArg, "Rotation must be represented by 1x3 or 3x1 "
-                                "floating-point rotation vector, or 3x3 rotation matrix" );
-
-    if( r_vec->rows == 3 && r_vec->cols == 3 )
-    {
-        _r = cvMat( 3, 1, CV_64FC1, r );
-        cvRodrigues2( r_vec, &_r );
-        cvRodrigues2( &_r, &matR, &_dRdr );
-        cvCopy( r_vec, &matR );
-    }
-    else
-    {
-        _r = cvMat( r_vec->rows, r_vec->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(r_vec->type)), r );
-        cvConvert( r_vec, &_r );
-        cvRodrigues2( &_r, &matR, &_dRdr );
-    }
-
-    if( (CV_MAT_DEPTH(t_vec->type) != CV_64F && CV_MAT_DEPTH(t_vec->type) != CV_32F) ||
-        (t_vec->rows != 1 && t_vec->cols != 1) ||
-        t_vec->rows*t_vec->cols*CV_MAT_CN(t_vec->type) != 3 )
-        CV_Error( CV_StsBadArg,
-                  "Translation vector must be 1x3 or 3x1 floating-point vector" );
-
-    _t = cvMat( t_vec->rows, t_vec->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(t_vec->type)), t );
-    cvConvert( t_vec, &_t );
-
-    if( (CV_MAT_TYPE(A->type) != CV_64FC1 && CV_MAT_TYPE(A->type) != CV_32FC1) ||
-        A->rows != 3 || A->cols != 3 )
-        CV_Error( CV_StsBadArg, "Instrinsic parameters must be 3x3 floating-point matrix" );
-
-    cvConvert( A, &_a );
-    fx = a[0]; fy = a[4];
-    cx = a[2]; cy = a[5];
-
-    if( fixedAspectRatio )
-        fx = fy*aspectRatio;
-
-    if( distCoeffs )
-    {
-        if( !CV_IS_MAT(distCoeffs) ||
-            (CV_MAT_DEPTH(distCoeffs->type) != CV_64F &&
-             CV_MAT_DEPTH(distCoeffs->type) != CV_32F) ||
-            (distCoeffs->rows != 1 && distCoeffs->cols != 1) ||
-            (distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 4 &&
-             distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 5 &&
-             distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 8 &&
-             distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 12 &&
-             distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 14) )
-            CV_Error( CV_StsBadArg, cvDistCoeffErr );
-
-        _k = cvMat( distCoeffs->rows, distCoeffs->cols,
-                    CV_MAKETYPE(CV_64F,CV_MAT_CN(distCoeffs->type)), k );
-        cvConvert( distCoeffs, &_k );
-        if(k[12] != 0 || k[13] != 0)
-        {
-            _detail::computeTiltProjectionMatrix(k[12], k[13],
-                                                &matTilt, &dMatTiltdTauX, &dMatTiltdTauY);
-        }
-    }
-
-    if( dpdr )
-    {
-        if( !CV_IS_MAT(dpdr) ||
-            (CV_MAT_TYPE(dpdr->type) != CV_32FC1 &&
-             CV_MAT_TYPE(dpdr->type) != CV_64FC1) ||
-            dpdr->rows != count*2 || dpdr->cols != 3 )
-            CV_Error( CV_StsBadArg, "dp/drot must be 2Nx3 floating-point matrix" );
-
-        if( CV_MAT_TYPE(dpdr->type) == CV_64FC1 )
-        {
-            _dpdr.reset(cvCloneMat(dpdr));
-        }
-        else
-            _dpdr.reset(cvCreateMat( 2*count, 3, CV_64FC1 ));
-        dpdr_p = _dpdr->data.db;
-        dpdr_step = _dpdr->step/sizeof(dpdr_p[0]);
-    }
-
-    if( dpdt )
-    {
-        if( !CV_IS_MAT(dpdt) ||
-            (CV_MAT_TYPE(dpdt->type) != CV_32FC1 &&
-             CV_MAT_TYPE(dpdt->type) != CV_64FC1) ||
-            dpdt->rows != count*2 || dpdt->cols != 3 )
-            CV_Error( CV_StsBadArg, "dp/dT must be 2Nx3 floating-point matrix" );
-
-        if( CV_MAT_TYPE(dpdt->type) == CV_64FC1 )
-        {
-            _dpdt.reset(cvCloneMat(dpdt));
-        }
-        else
-            _dpdt.reset(cvCreateMat( 2*count, 3, CV_64FC1 ));
-        dpdt_p = _dpdt->data.db;
-        dpdt_step = _dpdt->step/sizeof(dpdt_p[0]);
-    }
-
-    if( dpdf )
-    {
-        if( !CV_IS_MAT(dpdf) ||
-            (CV_MAT_TYPE(dpdf->type) != CV_32FC1 && CV_MAT_TYPE(dpdf->type) != CV_64FC1) ||
-            dpdf->rows != count*2 || dpdf->cols != 2 )
-            CV_Error( CV_StsBadArg, "dp/df must be 2Nx2 floating-point matrix" );
-
-        if( CV_MAT_TYPE(dpdf->type) == CV_64FC1 )
-        {
-            _dpdf.reset(cvCloneMat(dpdf));
-        }
-        else
-            _dpdf.reset(cvCreateMat( 2*count, 2, CV_64FC1 ));
-        dpdf_p = _dpdf->data.db;
-        dpdf_step = _dpdf->step/sizeof(dpdf_p[0]);
-    }
-
-    if( dpdc )
-    {
-        if( !CV_IS_MAT(dpdc) ||
-            (CV_MAT_TYPE(dpdc->type) != CV_32FC1 && CV_MAT_TYPE(dpdc->type) != CV_64FC1) ||
-            dpdc->rows != count*2 || dpdc->cols != 2 )
-            CV_Error( CV_StsBadArg, "dp/dc must be 2Nx2 floating-point matrix" );
-
-        if( CV_MAT_TYPE(dpdc->type) == CV_64FC1 )
-        {
-            _dpdc.reset(cvCloneMat(dpdc));
-        }
-        else
-            _dpdc.reset(cvCreateMat( 2*count, 2, CV_64FC1 ));
-        dpdc_p = _dpdc->data.db;
-        dpdc_step = _dpdc->step/sizeof(dpdc_p[0]);
-    }
-
-    if( dpdk )
-    {
-        if( !CV_IS_MAT(dpdk) ||
-            (CV_MAT_TYPE(dpdk->type) != CV_32FC1 && CV_MAT_TYPE(dpdk->type) != CV_64FC1) ||
-            dpdk->rows != count*2 || (dpdk->cols != 14 && dpdk->cols != 12 && dpdk->cols != 8 && dpdk->cols != 5 && dpdk->cols != 4 && dpdk->cols != 2) )
-            CV_Error( CV_StsBadArg, "dp/df must be 2Nx14, 2Nx12, 2Nx8, 2Nx5, 2Nx4 or 2Nx2 floating-point matrix" );
-
-        if( !distCoeffs )
-            CV_Error( CV_StsNullPtr, "distCoeffs is NULL while dpdk is not" );
-
-        if( CV_MAT_TYPE(dpdk->type) == CV_64FC1 )
-        {
-            _dpdk.reset(cvCloneMat(dpdk));
-        }
-        else
-            _dpdk.reset(cvCreateMat( dpdk->rows, dpdk->cols, CV_64FC1 ));
-        dpdk_p = _dpdk->data.db;
-        dpdk_step = _dpdk->step/sizeof(dpdk_p[0]);
-    }
-
-    if( dpdo )
-    {
-        if( !CV_IS_MAT( dpdo ) || ( CV_MAT_TYPE( dpdo->type ) != CV_32FC1
-                                    && CV_MAT_TYPE( dpdo->type ) != CV_64FC1 )
-            || dpdo->rows != count * 2 || dpdo->cols != count * 3 )
-            CV_Error( CV_StsBadArg, "dp/do must be 2Nx3N floating-point matrix" );
-
-        if( CV_MAT_TYPE( dpdo->type ) == CV_64FC1 )
-        {
-            _dpdo.reset( cvCloneMat( dpdo ) );
-        }
-        else
-            _dpdo.reset( cvCreateMat( 2 * count, 3 * count, CV_64FC1 ) );
-        cvZero(_dpdo);
-        dpdo_p = _dpdo->data.db;
-        dpdo_step = _dpdo->step / sizeof( dpdo_p[0] );
-    }
-
-    calc_derivatives = dpdr || dpdt || dpdf || dpdc || dpdk || dpdo;
-
-    for( i = 0; i < count; i++ )
-    {
-        double X = M[i].x, Y = M[i].y, Z = M[i].z;
-        double x = R[0]*X + R[1]*Y + R[2]*Z + t[0];
-        double y = R[3]*X + R[4]*Y + R[5]*Z + t[1];
-        double z = R[6]*X + R[7]*Y + R[8]*Z + t[2];
-        double r2, r4, r6, a1, a2, a3, cdist, icdist2;
-        double xd, yd, xd0, yd0, invProj;
-        Vec3d vecTilt;
-        Vec3d dVecTilt;
-        Matx22d dMatTilt;
-        Vec2d dXdYd;
-
-        double z0 = z;
-        z = z ? 1./z : 1;
-        x *= z; y *= z;
-
-        r2 = x*x + y*y;
-        r4 = r2*r2;
-        r6 = r4*r2;
-        a1 = 2*x*y;
-        a2 = r2 + 2*x*x;
-        a3 = r2 + 2*y*y;
-        cdist = 1 + k[0]*r2 + k[1]*r4 + k[4]*r6;
-        icdist2 = 1./(1 + k[5]*r2 + k[6]*r4 + k[7]*r6);
-        xd0 = x*cdist*icdist2 + k[2]*a1 + k[3]*a2 + k[8]*r2+k[9]*r4;
-        yd0 = y*cdist*icdist2 + k[2]*a3 + k[3]*a1 + k[10]*r2+k[11]*r4;
-
-        // additional distortion by projecting onto a tilt plane
-        vecTilt = matTilt*Vec3d(xd0, yd0, 1);
-        invProj = vecTilt(2) ? 1./vecTilt(2) : 1;
-        xd = invProj * vecTilt(0);
-        yd = invProj * vecTilt(1);
-
-        m[i].x = xd*fx + cx;
-        m[i].y = yd*fy + cy;
-        m[i].z = z; // Just put the projected Z coordinate here, we mainly care about the sign
-
-        if( calc_derivatives )
-        {
-            if( dpdc_p )
-            {
-                dpdc_p[0] = 1; dpdc_p[1] = 0; // dp_xdc_x; dp_xdc_y
-                dpdc_p[dpdc_step] = 0;
-                dpdc_p[dpdc_step+1] = 1;
-                dpdc_p += dpdc_step*2;
-            }
-
-            if( dpdf_p )
-            {
-                if( fixedAspectRatio )
-                {
-                    dpdf_p[0] = 0; dpdf_p[1] = xd*aspectRatio; // dp_xdf_x; dp_xdf_y
-                    dpdf_p[dpdf_step] = 0;
-                    dpdf_p[dpdf_step+1] = yd;
-                }
-                else
-                {
-                    dpdf_p[0] = xd; dpdf_p[1] = 0;
-                    dpdf_p[dpdf_step] = 0;
-                    dpdf_p[dpdf_step+1] = yd;
-                }
-                dpdf_p += dpdf_step*2;
-            }
-            for (int row = 0; row < 2; ++row)
-                for (int col = 0; col < 2; ++col)
-                    dMatTilt(row,col) = matTilt(row,col)*vecTilt(2)
-                                        - matTilt(2,col)*vecTilt(row);
-            double invProjSquare = (invProj*invProj);
-            dMatTilt *= invProjSquare;
-            if( dpdk_p )
-            {
-                dXdYd = dMatTilt*Vec2d(x*icdist2*r2, y*icdist2*r2);
-                dpdk_p[0] = fx*dXdYd(0);
-                dpdk_p[dpdk_step] = fy*dXdYd(1);
-                dXdYd = dMatTilt*Vec2d(x*icdist2*r4, y*icdist2*r4);
-                dpdk_p[1] = fx*dXdYd(0);
-                dpdk_p[dpdk_step+1] = fy*dXdYd(1);
-                if( _dpdk->cols > 2 )
-                {
-                    dXdYd = dMatTilt*Vec2d(a1, a3);
-                    dpdk_p[2] = fx*dXdYd(0);
-                    dpdk_p[dpdk_step+2] = fy*dXdYd(1);
-                    dXdYd = dMatTilt*Vec2d(a2, a1);
-                    dpdk_p[3] = fx*dXdYd(0);
-                    dpdk_p[dpdk_step+3] = fy*dXdYd(1);
-                    if( _dpdk->cols > 4 )
-                    {
-                        dXdYd = dMatTilt*Vec2d(x*icdist2*r6, y*icdist2*r6);
-                        dpdk_p[4] = fx*dXdYd(0);
-                        dpdk_p[dpdk_step+4] = fy*dXdYd(1);
-
-                        if( _dpdk->cols > 5 )
-                        {
-                            dXdYd = dMatTilt*Vec2d(
-                                    x*cdist*(-icdist2)*icdist2*r2, y*cdist*(-icdist2)*icdist2*r2);
-                            dpdk_p[5] = fx*dXdYd(0);
-                            dpdk_p[dpdk_step+5] = fy*dXdYd(1);
-                            dXdYd = dMatTilt*Vec2d(
-                                    x*cdist*(-icdist2)*icdist2*r4, y*cdist*(-icdist2)*icdist2*r4);
-                            dpdk_p[6] = fx*dXdYd(0);
-                            dpdk_p[dpdk_step+6] = fy*dXdYd(1);
-                            dXdYd = dMatTilt*Vec2d(
-                                    x*cdist*(-icdist2)*icdist2*r6, y*cdist*(-icdist2)*icdist2*r6);
-                            dpdk_p[7] = fx*dXdYd(0);
-                            dpdk_p[dpdk_step+7] = fy*dXdYd(1);
-                            if( _dpdk->cols > 8 )
-                            {
-                                dXdYd = dMatTilt*Vec2d(r2, 0);
-                                dpdk_p[8] = fx*dXdYd(0); //s1
-                                dpdk_p[dpdk_step+8] = fy*dXdYd(1); //s1
-                                dXdYd = dMatTilt*Vec2d(r4, 0);
-                                dpdk_p[9] = fx*dXdYd(0); //s2
-                                dpdk_p[dpdk_step+9] = fy*dXdYd(1); //s2
-                                dXdYd = dMatTilt*Vec2d(0, r2);
-                                dpdk_p[10] = fx*dXdYd(0);//s3
-                                dpdk_p[dpdk_step+10] = fy*dXdYd(1); //s3
-                                dXdYd = dMatTilt*Vec2d(0, r4);
-                                dpdk_p[11] = fx*dXdYd(0);//s4
-                                dpdk_p[dpdk_step+11] = fy*dXdYd(1); //s4
-                                if( _dpdk->cols > 12 )
-                                {
-                                    dVecTilt = dMatTiltdTauX * Vec3d(xd0, yd0, 1);
-                                    dpdk_p[12] = fx * invProjSquare * (
-                                            dVecTilt(0) * vecTilt(2) - dVecTilt(2) * vecTilt(0));
-                                    dpdk_p[dpdk_step+12] = fy*invProjSquare * (
-                                            dVecTilt(1) * vecTilt(2) - dVecTilt(2) * vecTilt(1));
-                                    dVecTilt = dMatTiltdTauY * Vec3d(xd0, yd0, 1);
-                                    dpdk_p[13] = fx * invProjSquare * (
-                                            dVecTilt(0) * vecTilt(2) - dVecTilt(2) * vecTilt(0));
-                                    dpdk_p[dpdk_step+13] = fy * invProjSquare * (
-                                            dVecTilt(1) * vecTilt(2) - dVecTilt(2) * vecTilt(1));
-                                }
-                            }
-                        }
-                    }
-                }
-                dpdk_p += dpdk_step*2;
-            }
-
-            if( dpdt_p )
-            {
-                double dxdt[] = { z, 0, -x*z }, dydt[] = { 0, z, -y*z };
-                for( j = 0; j < 3; j++ )
-                {
-                    double dr2dt = 2*x*dxdt[j] + 2*y*dydt[j];
-                    double dcdist_dt = k[0]*dr2dt + 2*k[1]*r2*dr2dt + 3*k[4]*r4*dr2dt;
-                    double dicdist2_dt = -icdist2*icdist2*(k[5]*dr2dt + 2*k[6]*r2*dr2dt + 3*k[7]*r4*dr2dt);
-                    double da1dt = 2*(x*dydt[j] + y*dxdt[j]);
-                    double dmxdt = (dxdt[j]*cdist*icdist2 + x*dcdist_dt*icdist2 + x*cdist*dicdist2_dt +
-                                    k[2]*da1dt + k[3]*(dr2dt + 4*x*dxdt[j]) + k[8]*dr2dt + 2*r2*k[9]*dr2dt);
-                    double dmydt = (dydt[j]*cdist*icdist2 + y*dcdist_dt*icdist2 + y*cdist*dicdist2_dt +
-                                    k[2]*(dr2dt + 4*y*dydt[j]) + k[3]*da1dt + k[10]*dr2dt + 2*r2*k[11]*dr2dt);
-                    dXdYd = dMatTilt*Vec2d(dmxdt, dmydt);
-                    dpdt_p[j] = fx*dXdYd(0);
-                    dpdt_p[dpdt_step+j] = fy*dXdYd(1);
-                }
-                dpdt_p += dpdt_step*2;
-            }
-
-            if( dpdr_p )
-            {
-                double dx0dr[] =
-                        {
-                                X*dRdr[0] + Y*dRdr[1] + Z*dRdr[2],
-                                X*dRdr[9] + Y*dRdr[10] + Z*dRdr[11],
-                                X*dRdr[18] + Y*dRdr[19] + Z*dRdr[20]
-                        };
-                double dy0dr[] =
-                        {
-                                X*dRdr[3] + Y*dRdr[4] + Z*dRdr[5],
-                                X*dRdr[12] + Y*dRdr[13] + Z*dRdr[14],
-                                X*dRdr[21] + Y*dRdr[22] + Z*dRdr[23]
-                        };
-                double dz0dr[] =
-                        {
-                                X*dRdr[6] + Y*dRdr[7] + Z*dRdr[8],
-                                X*dRdr[15] + Y*dRdr[16] + Z*dRdr[17],
-                                X*dRdr[24] + Y*dRdr[25] + Z*dRdr[26]
-                        };
-                for( j = 0; j < 3; j++ )
-                {
-                    double dxdr = z*(dx0dr[j] - x*dz0dr[j]);
-                    double dydr = z*(dy0dr[j] - y*dz0dr[j]);
-                    double dr2dr = 2*x*dxdr + 2*y*dydr;
-                    double dcdist_dr = (k[0] + 2*k[1]*r2 + 3*k[4]*r4)*dr2dr;
-                    double dicdist2_dr = -icdist2*icdist2*(k[5] + 2*k[6]*r2 + 3*k[7]*r4)*dr2dr;
-                    double da1dr = 2*(x*dydr + y*dxdr);
-                    double dmxdr = (dxdr*cdist*icdist2 + x*dcdist_dr*icdist2 + x*cdist*dicdist2_dr +
-                                    k[2]*da1dr + k[3]*(dr2dr + 4*x*dxdr) + (k[8] + 2*r2*k[9])*dr2dr);
-                    double dmydr = (dydr*cdist*icdist2 + y*dcdist_dr*icdist2 + y*cdist*dicdist2_dr +
-                                    k[2]*(dr2dr + 4*y*dydr) + k[3]*da1dr + (k[10] + 2*r2*k[11])*dr2dr);
-                    dXdYd = dMatTilt*Vec2d(dmxdr, dmydr);
-                    dpdr_p[j] = fx*dXdYd(0);
-                    dpdr_p[dpdr_step+j] = fy*dXdYd(1);
-                }
-                dpdr_p += dpdr_step*2;
-            }
-
-            if( dpdo_p )
-            {
-                double dxdo[] = { z * ( R[0] - x * z * z0 * R[6] ),
-                                  z * ( R[1] - x * z * z0 * R[7] ),
-                                  z * ( R[2] - x * z * z0 * R[8] ) };
-                double dydo[] = { z * ( R[3] - y * z * z0 * R[6] ),
-                                  z * ( R[4] - y * z * z0 * R[7] ),
-                                  z * ( R[5] - y * z * z0 * R[8] ) };
-                for( j = 0; j < 3; j++ )
-                {
-                    double dr2do = 2 * x * dxdo[j] + 2 * y * dydo[j];
-                    double dr4do = 2 * r2 * dr2do;
-                    double dr6do = 3 * r4 * dr2do;
-                    double da1do = 2 * y * dxdo[j] + 2 * x * dydo[j];
-                    double da2do = dr2do + 4 * x * dxdo[j];
-                    double da3do = dr2do + 4 * y * dydo[j];
-                    double dcdist_do
-                            = k[0] * dr2do + k[1] * dr4do + k[4] * dr6do;
-                    double dicdist2_do = -icdist2 * icdist2
-                                         * ( k[5] * dr2do + k[6] * dr4do + k[7] * dr6do );
-                    double dxd0_do = cdist * icdist2 * dxdo[j]
-                                     + x * icdist2 * dcdist_do + x * cdist * dicdist2_do
-                                     + k[2] * da1do + k[3] * da2do + k[8] * dr2do
-                                     + k[9] * dr4do;
-                    double dyd0_do = cdist * icdist2 * dydo[j]
-                                     + y * icdist2 * dcdist_do + y * cdist * dicdist2_do
-                                     + k[2] * da3do + k[3] * da1do + k[10] * dr2do
-                                     + k[11] * dr4do;
-                    dXdYd = dMatTilt * Vec2d( dxd0_do, dyd0_do );
-                    dpdo_p[i * 3 + j] = fx * dXdYd( 0 );
-                    dpdo_p[dpdo_step + i * 3 + j] = fy * dXdYd( 1 );
-                }
-                dpdo_p += dpdo_step * 2;
-            }
-        }
-    }
-
-    if( _m != imagePoints )
-        cvConvert( _m, imagePoints );
-
-    if( _dpdr != dpdr )
-        cvConvert( _dpdr, dpdr );
-
-    if( _dpdt != dpdt )
-        cvConvert( _dpdt, dpdt );
-
-    if( _dpdf != dpdf )
-        cvConvert( _dpdf, dpdf );
-
-    if( _dpdc != dpdc )
-        cvConvert( _dpdc, dpdc );
-
-    if( _dpdk != dpdk )
-        cvConvert( _dpdk, dpdk );
-
-    if( _dpdo != dpdo )
-        cvConvert( _dpdo, dpdo );
-}
-
-static void _cvProjectPoints2( const CvMat* objectPoints,
-                        const CvMat* r_vec,
-                        const CvMat* t_vec,
-                        const CvMat* A,
-                        const CvMat* distCoeffs,
-                        CvMat* imagePoints, CvMat* dpdr,
-                        CvMat* dpdt, CvMat* dpdf,
-                        CvMat* dpdc, CvMat* dpdk,
-                        double aspectRatio )
-{
-    _cvProjectPoints2Internal( objectPoints, r_vec, t_vec, A, distCoeffs, imagePoints, dpdr, dpdt,
-                               dpdf, dpdc, dpdk, NULL, aspectRatio );
 }

aruco_pose/src/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]
+  genmap.py <length> <x> <y> <dist_x> <dist_y> [<first>] [<x0>] [<y0>] [--top-left | --bottom-left] [-o <filename>]
   genmap.py (-h | --help)
 
 Options:
@@ -27,13 +27,16 @@ 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 > $(catkin_find aruco_pose map)/test_map.txt
+  rosrun aruco_pose genmap.py 0.33 2 4 1 1 0 -o test_map.txt
 """
 
 from __future__ import print_function
 
+import sys
+from os import path
 from docopt import docopt
 
 
@@ -49,14 +52,19 @@ 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
 
-print('# id\tlength\tx\ty\tz\trot_z\trot_y\trot_x')
+output.write('# id\tlength\tx\ty\tz\trot_z\trot_y\trot_x\n')
 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
-        print('{}\t{}\t{}\t{}\t{}\t{}\t{}\t{}'.format(first, length, pos_x, pos_y, 0, 0, 0, 0))
+        output.write('{}\t{}\t{}\t{}\t{}\t{}\t{}\t{}\n'.format(first, length, pos_x, pos_y, 0, 0, 0, 0))
         first += 1

aruco_pose/srv/SetMarkers.srv

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

aruco_pose/test/basic.py

@@ -143,7 +143,7 @@ def test_map_image(node):
     assert img.encoding in ('mono8', 'rgb8')
 
 def test_map_markers(node):
-    markers = rospy.wait_for_message('aruco_map/markers', MarkerArray, timeout=5)
+    markers = rospy.wait_for_message('aruco_map/map', MarkerArray, timeout=5)
     assert markers.markers[0].id == 1
     assert markers.markers[1].id == 2
     assert markers.markers[2].id == 3

aruco_pose/vendor/VendorOpenCV.cmake

@@ -7,6 +7,7 @@ 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
@@ -23,7 +24,7 @@ target_compile_definitions(_opencv_aruco PRIVATE
   CV_OVERRIDE=override
 )
 target_compile_options(_opencv_aruco PRIVATE
-  -fpic -fPIC
+  -fpic -fPIC -fvisibility=hidden
 )
 
 target_include_directories(_opencv_aruco PUBLIC

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.data(), pts, sizeof(struct pt) * sz);
+    memcpy(_tmp_stack, 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_.data(), 0, sizeof(rvalloc_[0]) * rvalloc_.size()); // TODO Add AutoBuffer zero fill
-    struct remove_vertex *rvalloc = rvalloc_.data();
+    memset(rvalloc_, 0, sizeof(rvalloc_[0]) * rvalloc_.size()); // TODO Add AutoBuffer zero fill
+    struct remove_vertex *rvalloc = rvalloc_;
     cv::AutoBuffer<struct segment, 0> segs_(std::max(1, sz)); // TODO Add AutoBuffer zero fill
-    memset(segs_.data(), 0, sizeof(segs_[0]) * segs_.size());
-    struct segment *segs = segs_.data();
+    memset(segs_, 0, sizeof(segs_[0]) * segs_.size());
+    struct segment *segs = segs_;
 
     // 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_.data(), 0, sizeof(lfps_[0]) * lfps_.size()); // TODO Add AutoBuffer zero fill
-    struct line_fit_pt *lfps = lfps_.data();
+    memset(lfps_, 0, sizeof(lfps_[0]) * lfps_.size()); // TODO Add AutoBuffer zero fill
+    struct line_fit_pt *lfps = lfps_;
 
     for (int i = 0; i < sz; i++) {
         struct pt *p;

book.json

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

builder/assets/clever/setup.py

@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
 
 from distutils.core import setup
 

builder/assets/clover.service

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

builder/assets/install_gitbook.sh

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

builder/assets/noetic-rosdep-clover.yaml

@@ -0,0 +1,18 @@
+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]

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/melodic/setup.sh; ROS_HOSTNAME=`hostname`.local exec roscore"
+ExecStart=/bin/sh -c ". /opt/ros/noetic/setup.sh; ROS_HOSTNAME=`hostname`.local exec roscore"
 Restart=on-failure
 RestartSec=3
 

builder/image-build.sh

@@ -15,7 +15,8 @@
 
 set -e # Exit immidiately on non-zero result
 
-SOURCE_IMAGE="https://downloads.raspberrypi.org/raspios_lite_armhf/images/raspios_lite_armhf-2021-01-12/2021-01-11-raspios-buster-armhf-lite.zip"
+# 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"
 
 export DEBIAN_FRONTEND=${DEBIAN_FRONTEND:='noninteractive'}
 export LANG=${LANG:='C.UTF-8'}
@@ -104,8 +105,6 @@ ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/butterf
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/monkey.service' '/lib/systemd/system/'
 # software install
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} exec ${SCRIPTS_DIR}'/image-software.sh'
-# examples
-${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/examples' '/home/pi/'  # TODO: symlink?
 # network setup
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} exec ${SCRIPTS_DIR}'/image-network.sh'
 # avahi setup
@@ -114,15 +113,11 @@ ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/avahi-s
 # If RPi then use a one thread to build a ROS package on RPi, else use all
 [[ $(arch) == 'armv7l' ]] && NUMBER_THREADS=1 || NUMBER_THREADS=$(nproc --all)
 # Clover
-${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/clover.service' '/lib/systemd/system/'
-${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/roscore.service' '/lib/systemd/system/'
-${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/melodic-rosdep-clover.yaml' '/etc/ros/rosdep/'
+${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/noetic-rosdep-clover.yaml' '/etc/ros/rosdep/'
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/ros_python_paths' '/etc/sudoers.d/'
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/pigpiod.service' '/lib/systemd/system/'
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/launch.nanorc' '/usr/share/nano/'
 # ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/kinetic-ros-clover.rosinstall' '/home/pi/ros_catkin_ws/'
-# Add PX4 udev rules
-${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${REPO_DIR}'/clover/config/99-px4fmu.rules' '/lib/udev/rules.d/'
 # Add rename script
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} exec ${SCRIPTS_DIR}'/image-ros.sh' ${REPO_URL} ${IMAGE_VERSION} false false ${NUMBER_THREADS}
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} exec ${SCRIPTS_DIR}'/image-validate.sh'

builder/image-ros.sh

@@ -13,7 +13,7 @@
 # copies or substantial portions of the Software.
 #
 
-set -e # Exit immidiately on non-zero result
+set -ex # exit on error, echo commands
 
 REPO=$1
 REF=$2
@@ -21,6 +21,9 @@ 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
@@ -68,7 +71,8 @@ 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
-echo "yaml file:///etc/ros/rosdep/melodic-rosdep-clover.yaml" >> /etc/ros/rosdep/sources.list.d/20-default.list
+# 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
 my_travis_retry rosdep update
 
 echo_stamp "Populate rosdep for ROS user"
@@ -76,22 +80,40 @@ 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" # TODO: bring back
-# cd /home/pi/catkin_ws/src/clover
-# git config remote.origin.fetch "+refs/heads/*:refs/remotes/origin/*"
-echo_stamp "Remove .git from Clover to reduce the size"
-rm -rf /home/pi/catkin_ws/src/clover/.git # TODO: remove
+# 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 melodic --os=debian:buster \
+my_travis_retry rosdep install -y --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} --os=debian:buster \
   --skip-keys=gazebo_ros --skip-keys=gazebo_plugins
-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
+my_travis_retry pip3 install wheel
+my_travis_retry pip3 install -r /home/pi/catkin_ws/src/clover/clover/requirements.txt
+source /opt/ros/${ROS_DISTRO}/setup.bash
 # Don't build simulation plugins for actual drone
-catkin_make -j2 -DCMAKE_BUILD_TYPE=Release -DCATKIN_BLACKLIST_PACKAGES=clover_gazebo_plugins
+catkin_make -j2 -DCMAKE_BUILD_TYPE=RelWithDebInfo
+source devel/setup.bash
 
 echo_stamp "Install clever package (for backwards compatibility)"
 cd /home/pi/catkin_ws/src/clover/builder/assets/clever
@@ -100,30 +122,27 @@ rm -rf build  # remove build artifacts
 
 echo_stamp "Build Clover documentation"
 cd /home/pi/catkin_ws/src/clover
-NPM_CONFIG_UNSAFE_PERM=true npm install gitbook-cli -g
-NPM_CONFIG_UNSAFE_PERM=true gitbook install
+builder/assets/install_gitbook.sh
+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-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
+    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
 
 # 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
@@ -132,15 +151,26 @@ catkin_make run_tests #&& catkin_test_results
 echo_stamp "Change permissions for catkin_ws"
 chown -Rf pi:pi /home/pi/catkin_ws
 
-echo_stamp "Change permissions for examples"
+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/melodic/setup.bash
+source /opt/ros/${ROS_DISTRO}/setup.bash
 source /home/pi/catkin_ws/devel/setup.bash
 EOF
 

builder/image-software.sh

@@ -64,15 +64,14 @@ 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
-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
+
+wget -O - 'http://packages.coex.tech/key.asc' | apt-key add - 
+echo 'deb http://packages.coex.tech buster main' >> /etc/apt/sources.list
 
 echo_stamp "Update apt cache"
 
@@ -99,18 +98,18 @@ tree \
 vim \
 libjpeg8 \
 tcpdump \
-ltrace \
 libpoco-dev \
 libzbar0 \
-python-rosdep \
-python-rosinstall-generator \
-python-wstool \
-python-rosinstall \
+python3-rosdep \
+python3-rosinstall-generator \
+python3-wstool \
+python3-rosinstall \
 build-essential \
 libffi-dev \
 monkey \
 pigpio python-pigpio python3-pigpio \
 i2c-tools \
+espeak espeak-data python-espeak python3-espeak \
 ntpdate \
 python-dev \
 python3-dev \
@@ -138,22 +137,25 @@ 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 pip install --prefer-binary rpi_ws281x
+my_travis_retry pip3 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 https://nodejs.org/dist/v10.15.0/node-v10.15.0-linux-armv6l.tar.gz
+wget --no-verbose https://nodejs.org/dist/v10.15.0/node-v10.15.0-linux-armv6l.tar.gz
 tar -xzf node-v10.15.0-linux-armv6l.tar.gz
 cp -R node-v10.15.0-linux-armv6l/* /usr/local/
 rm -rf node-v10.15.0-linux-armv6l/

builder/image-validate.sh

@@ -16,16 +16,24 @@ set -ex
 
 echo "Run image tests"
 
-export ROS_DISTRO='melodic'
+export ROS_DISTRO='noetic'
 export ROS_IP='127.0.0.1'
-source /opt/ros/melodic/setup.bash
+source /opt/ros/${ROS_DISTRO}/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

builder/standalone-install.sh

@@ -3,25 +3,45 @@
 # Perform a "standalone install" in a Docker container
 set -e
 # Step 1: Install pip
-apt update
-apt install -y curl
-curl https://bootstrap.pypa.io/pip/2.7/get-pip.py -o get-pip.py
-python ./get-pip.py
+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
 
 # 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:
-    xenial: ros-kinetic-led-msgs
-    bionic: ros-melodic-led-msgs
-  debian:
-    stretch: ros-kinetic-led-msgs
-    buster: ros-melodic-led-msgs
+    ${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]
 EOF
-apt update
+apt-get update
 rosdep update
 
 # Step 2: Run rosdep to install all dependencies
@@ -37,7 +57,10 @@ cd /root/catkin_ws
 catkin_make
 
 # Step 4: Run tests
-pip install --upgrade pytest
+${PYTHON} -m 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

builder/test/test_qr.py

@@ -0,0 +1,42 @@
+#!/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()

builder/test/tests.py

@@ -1,21 +1,29 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
 
 # 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
+from mavros_msgs.msg import State, StatusText, ExtendedState, RCIn, Mavlink
 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
@@ -28,4 +36,4 @@ import pigpio
 # from espeak import espeak
 from pyzbar import pyzbar
 
-print cv2.getBuildInformation()
+print(cv2.getBuildInformation())

builder/test/tests.sh

@@ -34,6 +34,7 @@ i2cdetect -V
 butterfly -h
 # espeak --version
 mjpg_streamer --version
+systemctl --version
 
 # ros stuff
 
@@ -54,6 +55,12 @@ 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.12" ]]
 
 # validate examples are present
 [[ $(ls /home/pi/examples/*) ]]

builder/test/tests_clever.py

@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
 
 # test backwards compatibility
 

check_unused_assets.py

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

clover/CMakeLists.txt

@@ -1,9 +1,11 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.0)
 project(clover)
 
 ## Compile as C++11, supported in ROS Kinetic and newer
 add_compile_options(-std=c++11)
 
+add_compile_options(-Wall -Wextra -Werror)
+
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
@@ -24,13 +26,22 @@ 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)
 
-find_package(OpenCV 3 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
   COMPONENTS
     calib3d
     imgproc
@@ -118,10 +129,9 @@ generate_messages(
 ##     and list every .cfg file to be processed
 
 ## Generate dynamic reconfigure parameters in the 'cfg' folder
-# generate_dynamic_reconfigure_options(
-#   cfg/DynReconf1.cfg
-#   cfg/DynReconf2.cfg
-# )
+generate_dynamic_reconfigure_options(
+  cfg/Flow.cfg
+)
 
 ###################################
 ## catkin specific configuration ##
@@ -203,6 +213,8 @@ add_dependencies(clover_led ${PROJECT_NAME}_generate_messages_cpp)
 
 add_dependencies(shell ${PROJECT_NAME}_generate_messages_cpp)
 
+add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencfg)
+
 ## Rename C++ executable without prefix
 ## The above recommended prefix causes long target names, the following renames the
 ## target back to the shorter version for ease of user use
@@ -233,12 +245,12 @@ target_link_libraries(${PROJECT_NAME}
 #   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 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 cpp header files for installation
 # install(DIRECTORY include/${PROJECT_NAME}/
@@ -254,13 +266,25 @@ target_link_libraries(${PROJECT_NAME}
 #   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
-    config/99-px4fmu.rules
+    udev/99-px4fmu.rules
     DESTINATION /lib/udev/rules.d
   )
 else()

clover/README.md

@@ -4,7 +4,7 @@ A bundle for autonomous navigation and drone control.
 
 ## Manual installation
 
-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).
+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).
 
 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/config
+cd ~/catkin_ws/src/clover/clover/udev
 sudo cp 99-px4fmu.rules /lib/udev/rules.d
 ```
 

clover/cfg/Flow.cfg

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

clover/examples/flight.py

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

clover/examples/flight_marker.py

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

clover/examples/get_telemetry.py

@@ -0,0 +1,11 @@
+# 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())

clover/examples/gps.py

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

clover/examples/navigate_wait.py

@@ -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)
 
-# Take off 1 meter
+print('Take off 1 meter')
 navigate_wait(z=1, frame_id='body', auto_arm=True)
 
-# Fly forward 1 m
+print('Fly forward 1 m')
 navigate_wait(x=1, frame_id='body')
 
-# Land
+print('Land')
 land()

clover/examples/subscriber.py

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

clover/launch/aruco.launch

@@ -2,30 +2,41 @@
     <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="$(arg aruco_detect)" type="nodelet" args="load aruco_pose/aruco_detect nodelet_manager" output="screen" clear_params="true">
+    <node name="aruco_detect" pkg="nodelet" if="$(eval aruco_detect and not disable)" type="nodelet" args="load aruco_pose/aruco_detect main_camera_nodelet_manager" output="screen" clear_params="true" respawn="true">
         <remap from="image_raw" to="main_camera/image_raw"/>
         <remap from="camera_info" to="main_camera/camera_info"/>
-        <remap from="map_markers" to="aruco_map/markers" if="$(arg aruco_map)"/>
+        <remap from="map_markers" to="aruco_map/map"/>
         <param name="estimate_poses" value="true"/>
         <param name="send_tf" value="true"/>
-        <param name="known_tilt" value="map"/>
-        <param name="length" value="0.33"/>
+        <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"/>
         <!-- 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="$(arg aruco_map)" args="load aruco_pose/aruco_map nodelet_manager" output="screen" clear_params="true">
+    <node name="aruco_map" pkg="nodelet" type="nodelet" if="$(eval aruco_map and not disable)" args="load aruco_pose/aruco_map main_camera_nodelet_manager" output="screen" clear_params="true" respawn="true">
         <remap from="image_raw" to="main_camera/image_raw"/>
         <remap from="camera_info" to="main_camera/camera_info"/>
         <remap from="markers" to="aruco_detect/markers"/>
-        <param name="map" value="$(find aruco_pose)/map/map.txt"/>
-        <param name="known_tilt" value="map"/>
+        <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="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)"/>
@@ -34,11 +45,11 @@
     </node>
 
     <!-- vpe publisher from aruco markers -->
-    <node name="vpe_publisher" pkg="clover" type="vpe_publisher" if="$(arg aruco_vpe)" output="screen" clear_params="true">
-        <remap from="~pose_cov" to="aruco_map/pose"/>
+    <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)"/>
         <remap from="~vpe" to="mavros/vision_pose/pose"/>
-        <param name="frame_id" value="aruco_map_detected"/>
-        <param name="publish_zero" value="true"/>
+        <param name="frame_id" value="aruco_map_detected" if="$(arg aruco_vpe)"/>
+        <param name="force_init" value="$(arg force_init)"/>
         <param name="offset_frame_id" value="aruco_map"/>
     </node>
 </launch>

clover/launch/clover.launch

@@ -11,8 +11,8 @@
     <arg name="rangefinder_vl53l1x" default="true"/>
     <arg name="led" default="true"/>
     <arg name="blocks" default="false"/>
-    <arg name="rc" default="true"/>
-    <arg name="shell" default="true"/>
+    <arg name="rc" default="false"/>
+    <arg name="force_init" value="true"/> <!-- force estimator to init by publishing zero pose -->
 
     <arg name="simulator" default="false"/> <!-- flag that we are operating on a simulated drone -->
 
@@ -34,28 +34,23 @@
     </node>
 
     <!-- aruco markers -->
-    <include file="$(find clover)/launch/aruco.launch" if="$(arg aruco)"/>
+    <include file="$(find clover)/launch/aruco.launch" if="$(eval aruco or force_init)">
+        <arg name="force_init" value="$(arg force_init)"/>
+        <arg name="disable" value="$(eval not aruco)"/>
+    </include>
 
     <!-- optical flow -->
-    <node pkg="nodelet" type="nodelet" name="optical_flow" args="load clover/optical_flow nodelet_manager" if="$(arg optical_flow)" clear_params="true" output="screen">
+    <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">
         <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/body" value="map"/>
-        <param name="reference_frames/base_link" value="map"/>
-        <param name="reference_frames/navigate_target" value="map"/>
         <param name="reference_frames/main_camera_optical" value="map"/>
     </node>
 
@@ -91,9 +86,6 @@
         <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"/>

clover/launch/led.launch

@@ -36,7 +36,7 @@
             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 }
+            error: { effect: flash, r: 255, g: 0, b: 0, ignore: [ "[lpe] vision position timeout" ]}
         </rosparam>
     </node>
 </launch>

clover/launch/main_camera.launch

@@ -3,6 +3,7 @@
 
     <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"/>
@@ -17,9 +18,14 @@
     <!-- 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 nodelet_manager" clear_params="true" unless="$(arg simulator)">
-        <param name="device_path" value="/dev/video0"/> <!-- v4l2 device -->
+    <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)"/>
         <param name="frame_id" value="main_camera_optical"/>
         <param name="camera_info_url" value="file://$(find clover)/camera_info/fisheye_cam.yaml"/>
 

clover/launch/mavros.launch

@@ -6,13 +6,16 @@
     <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" 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" launch-prefix="$(arg prefix)" 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="/dev/px4fmu" if="$(eval fcu_conn == 'usb')"/>
+        <param name="fcu_url" value="$(arg usb_device)" 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')"/>
@@ -36,7 +39,7 @@
         <rosparam command="load" file="$(find clover)/launch/mavros_config.yaml"/>
 
         <!-- remap rangefinder -->
-        <remap from="mavros/distance_sensor/rangefinder_sub" to="rangefinder/range"/>
+        <remap from="mavros/distance_sensor/rangefinder_sub" to="$(arg distance_sensor_remap)" if="$(eval bool(distance_sensor_remap))"/>
 
         <rosparam param="plugin_whitelist">
             - altitude

clover/launch/mavros_config.yaml

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

clover/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
-<package format="2">
+<package format="3">
   <name>clover</name>
-  <version>0.21.1</version>
+  <version>0.23.0</version>
   <description>The Clover package</description>
 
   <maintainer email="okalachev@gmail.com">Oleg Kalachev</maintainer>
@@ -37,7 +37,9 @@
   <depend>rosbridge_server</depend>
   <depend>web_video_server</depend>
   <depend>tf2_web_republisher</depend>
-  <depend>python-lxml</depend>
+  <depend condition="$ROS_PYTHON_VERSION == 2">python-lxml</depend>
+  <depend condition="$ROS_PYTHON_VERSION == 3">python3-lxml</depend>
+  <depend>dynamic_reconfigure</depend>
   <exec_depend>python-pymavlink</exec_depend>
   <!-- Use test_depend for packages you need only for testing: -->
   <!--   <test_depend>gtest</test_depend> -->

clover/src/autotest/autotest_aruco.py

@@ -0,0 +1,87 @@
+#!/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()

clover/src/autotest/autotest_flight.py

@@ -0,0 +1,100 @@
+#!/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()

clover/src/autotest/autotest_led.py

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

clover/src/autotest/util.py

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

clover/src/led.cpp

@@ -12,6 +12,7 @@
 
 #include <ros/ros.h>
 #include <string>
+#include <vector>
 #include <boost/algorithm/string.hpp>
 
 #include <clover/SetLEDEffect.h>
@@ -29,6 +30,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;
@@ -274,6 +276,10 @@ 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");
 	}
 }
@@ -302,6 +308,7 @@ 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);

clover/src/optical_flow.cpp

@@ -22,11 +22,13 @@
 #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;
 
@@ -38,6 +40,7 @@ public:
 	{}
 
 private:
+	bool enabled_;
 	ros::Publisher flow_pub_, velo_pub_, shift_pub_;
 	ros::Time prev_stamp_;
 	std::string fcu_frame_id_, local_frame_id_;
@@ -53,6 +56,8 @@ 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()
 	{
@@ -69,20 +74,25 @@ 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");
 	}
 
@@ -109,6 +119,8 @@ 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;
@@ -152,7 +164,7 @@ private:
 			cv::Point2d shift = cv::phaseCorrelate(prev_, curr_, hann_, &response);
 
 			// Publish raw shift in pixels
-			static geometry_msgs::Vector3Stamped shift_vec;
+			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;
@@ -178,8 +190,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
-			static geometry_msgs::Vector3Stamped flow_camera, flow_fcu;
+			// Convert to FCU frame
+			geometry_msgs::Vector3Stamped flow_camera, flow_fcu;
 			flow_camera.header.frame_id = msg->header.frame_id;
 			flow_camera.header.stamp = msg->header.stamp;
 			flow_camera.vector.x = flow_y; // +y means counter-clockwise rotation around Y axis
@@ -195,18 +207,21 @@ 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);
-					static geometry_msgs::Vector3Stamped flow_gyro_fcu;
+					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) {
-					// Invalidate previous frame
-					prev_.release();
-					return;
+					// Transform not available, keep NANs in flow gyro
 				}
 			}
 
@@ -218,6 +233,9 @@ private:
 			flow_.quality = (uint8_t)(response * 255);
 			flow_pub_.publish(flow_);
 
+			prev_ = curr_.clone();
+			prev_stamp_ = msg->header.stamp;
+
 			// Publish debug image
 			if (img_pub_.getNumSubscribers() > 0) {
 				// publish debug image
@@ -231,15 +249,12 @@ private:
 			}
 
 			// Publish estimated angular velocity
-			static geometry_msgs::TwistStamped velo;
+			geometry_msgs::TwistStamped velo;
 			velo.header.stamp = msg->header.stamp;
 			velo.header.frame_id = fcu_frame_id_;
-			velo.twist.angular.x = flow_.integrated_x / integration_time.toSec();
-			velo.twist.angular.y = flow_.integrated_y / integration_time.toSec();
+			velo.twist.angular.x = flow_fcu.vector.x / integration_time.toSec();
+			velo.twist.angular.y = flow_fcu.vector.y / integration_time.toSec();
 			velo_pub_.publish(velo);
-
-			prev_ = curr_.clone();
-			prev_stamp_ = msg->header.stamp;
 		}
 	}
 
@@ -260,6 +275,14 @@ private:
 
 		return flow;
 	}
+
+	void paramCallback(clover::FlowConfig &config, [[maybe_unused]] uint32_t level)
+	{
+		enabled_ = config.enabled;
+		if (!enabled_) {
+			prev_ = Mat(); // clear previous frame
+		}
+	}
 };
 
 PLUGINLIB_EXPORT_CLASS(OpticalFlow, nodelet::Nodelet)

clover/src/selfcheck.py

@@ -30,6 +30,7 @@ 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
@@ -43,6 +44,10 @@ from mavros import mavlink
 
 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)
@@ -138,7 +143,7 @@ def mavlink_exec(cmd, timeout=3.0):
         timeout=3,
         baudrate=0,
         count=len(cmd),
-        data=map(ord, cmd.ljust(70, '\0')))
+        data=[ord(c) for c in cmd.ljust(70, '\0')])
     msg.pack(link)
     ros_msg = mavlink.convert_to_rosmsg(msg)
     mavlink_pub.publish(ros_msg)
@@ -193,24 +198,27 @@ 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')
 
-        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
+        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: '):])
 
-        if not is_clover_firmware:
-            failure('not running Clover PX4 firmware, https://clover.coex.tech/firmware')
+        if not clover_fw:
+            info('not Clover PX4 firmware, check https://clover.coex.tech/firmware')
 
         est = get_param('SYS_MC_EST_GROUP')
         if est == 1:
@@ -483,6 +491,12 @@ def check_local_position():
             failure('roll is %.2f deg; place copter horizontally or redo level horizon calib',
                     math.degrees(roll))
 
+        if not tf_buffer.can_transform('base_link', pose.header.frame_id, rospy.get_rostime(), rospy.Duration(0.5)):
+            failure('can\'t transform from %s to base_link (timeout 0.5 s): is TF enabled?', pose.header.frame_id)
+
+        if not tf_buffer.can_transform('body', pose.header.frame_id, rospy.get_rostime(), rospy.Duration(0.5)):
+            failure('can\'t transform from %s to body (timeout 0.5 s)', pose.header.frame_id)
+
     except rospy.ROSException:
         failure('no local position')
 
@@ -609,25 +623,25 @@ def check_rangefinder():
 
 @check('Boot duration')
 def check_boot_duration():
-    output = subprocess.check_output('systemd-analyze')
+    output = subprocess.check_output('systemd-analyze').decode()
     r = re.compile(r'([\d\.]+)s\s*$', flags=re.MULTILINE)
     duration = float(r.search(output).groups()[0])
-    if duration > 15:
+    if duration > 20:
         failure('long Raspbian boot duration: %ss (systemd-analyze for analyzing)', duration)
 
 
 @check('CPU usage')
 def check_cpu_usage():
-    WHITELIST = 'nodelet',
+    WHITELIST = 'nodelet', 'gzclient', 'gzserver'
     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)
+    output = subprocess.check_output(CMD, shell=True).decode()
     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 float(cpu) > 30:
+        if cmd.strip() not in WHITELIST and locale.atof(cpu) > 30:
             failure('high CPU usage (%s%%) detected: %s (PID %s)',
                     cpu.strip(), cmd.strip(), pid.strip())
 
@@ -636,7 +650,7 @@ def check_cpu_usage():
 def check_clover_service():
     try:
         output = subprocess.check_output('systemctl show -p ActiveState --value clover.service'.split(),
-                                         stderr=subprocess.STDOUT)
+                                         stderr=subprocess.STDOUT).decode()
     except subprocess.CalledProcessError as e:
         failure('systemctl returned %s: %s', e.returncode, e.output)
         return
@@ -646,13 +660,22 @@ def check_clover_service():
     elif 'failed' in output:
         failure('service failed to run, check your launch-files')
 
-    r = re.compile(r'^(.*)\[(FATAL|ERROR)\] \[\d+.\d+\]: (.*?)(\x1b(.*))?$')
+    BLACKLIST = 'Unexpected command 520', 'Time jump detected', 'different index:'
+
+    r = re.compile(r'^(.*)\[(FATAL|ERROR| WARN)\] \[\d+.\d+\]: (.*?)(\x1b(.*))?$')
     error_count = OrderedDict()
     try:
         for line in open('/tmp/clover.err', 'r'):
+            skip = False
+            for substr in BLACKLIST:
+                if substr in line:
+                    skip = True
+            if skip:
+                continue
+
             node_error = r.search(line)
             if node_error:
-                msg = node_error.groups()[1] + ': ' + node_error.groups()[2]
+                msg = node_error.groups()[1].strip() + ': ' + node_error.groups()[2]
                 if msg in error_count:
                     error_count[msg] += 1
                 else:
@@ -723,6 +746,14 @@ def check_network():
 
 @check('RPi health')
 def check_rpi_health():
+    try:
+        import shutil
+        total, used, free = shutil.disk_usage('/')
+        if free < 1024 * 1024 * 1024:
+            failure('disk space is less than 1 GB; consider removing logs (~/.ros/log/)')
+    except Exception as e:
+        info('could not check the disk free space: %s', str(e))
+
     # `vcgencmd get_throttled` output codes taken from
     # https://github.com/raspberrypi/documentation/blob/JamesH65-patch-vcgencmd-vcdbg-docs/raspbian/applications/vcgencmd.md#get_throttled
     # TODO: support more base platforms?
@@ -751,9 +782,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'])
+        output = subprocess.check_output(['vcgencmd', 'get_throttled']).decode()
     except OSError:
-        failure('could not call vcgencmd binary; not a Raspberry Pi?')
+        info('could not call vcgencmd binary; not a Raspberry Pi?')
         return
 
     throttle_mask = int(output.split('=')[1], base=16)

clover/src/simple_offboard.cpp

@@ -61,6 +61,7 @@ std::shared_ptr<tf2_ros::TransformBroadcaster> transform_broadcaster;
 std::shared_ptr<tf2_ros::StaticTransformBroadcaster> static_transform_broadcaster;
 
 // Parameters
+string mavros;
 string local_frame;
 string fcu_frame;
 ros::Duration transform_timeout;
@@ -181,9 +182,10 @@ inline bool waitTransform(const string& target, const string& source,
 		ros::spinOnce();
 		r.sleep();
 	}
+	return false;
 }
 
-#define TIMEOUT(msg, timeout) (ros::Time::now() - msg.header.stamp > timeout)
+#define TIMEOUT(msg, timeout) (msg.header.stamp.isZero() || (ros::Time::now() - msg.header.stamp > timeout))
 
 bool getTelemetry(GetTelemetry::Request& req, GetTelemetry::Response& res)
 {
@@ -441,6 +443,10 @@ 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;
@@ -497,10 +503,10 @@ inline void checkManualControl()
 
 	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
+		[[maybe_unused]] const uint8_t SWITCH_POS_NONE = 0; // switch is not mapped
+		[[maybe_unused]] const uint8_t SWITCH_POS_ON = 1; // switch activated
+		[[maybe_unused]] const uint8_t SWITCH_POS_MIDDLE = 2; // middle position
+		[[maybe_unused]] 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;
@@ -685,7 +691,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 yaw
+			// destination point and/or attitude
 			PoseStamped ps;
 			ps.header.frame_id = frame_id;
 			ps.header.stamp = stamp;
@@ -694,7 +700,12 @@ 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 (std::isnan(yaw)) {
+			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)) {
 				setpoint_yaw_type = YAW_RATE;
 				setpoint_yaw_rate = yaw_rate;
 			} else if (std::isinf(yaw) && yaw > 0) {
@@ -712,7 +723,7 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl
 		}
 
 		if (sp_type == VELOCITY) {
-			static Vector3Stamped vel;
+			Vector3Stamped vel;
 			vel.header.frame_id = frame_id;
 			vel.header.stamp = stamp;
 			vel.vector.x = vx;
@@ -797,7 +808,7 @@ 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);
 }
 
-bool land(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)
+bool land([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)
 {
 	try {
 		if (busy)
@@ -843,6 +854,7 @@ bool land(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)
 		busy = false;
 		return true;
 	}
+	return false;
 }
 
 int main(int argc, char **argv)
@@ -855,8 +867,9 @@ int main(int argc, char **argv)
 	static_transform_broadcaster = std::make_shared<tf2_ros::StaticTransformBroadcaster>();
 
 	// Params
-	nh.param<string>("mavros/local_position/tf/frame_id", local_frame, "map");
-	nh.param<string>("mavros/local_position/tf/child_frame_id", fcu_frame, "base_link");
+	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_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);
@@ -867,6 +880,13 @@ int main(int argc, char **argv)
 	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));
@@ -881,25 +901,25 @@ 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 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);
 
 	// 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);

clover/src/vpe_publisher.cpp

@@ -38,9 +38,9 @@ TransformStamped offset;
 
 void publishZero(const ros::TimerEvent& e)
 {
-	if (e.current_real - vpe.header.stamp < publish_zero_timout) return; // have vpe
+	if (!vpe.header.stamp.isZero() && e.current_real - vpe.header.stamp < publish_zero_timout) return; // have vpe
 
-	if (e.current_real - pose.header.stamp < publish_zero_timout) { // have local position
+	if (!pose.header.stamp.isZero() && e.current_real - pose.header.stamp < publish_zero_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");
-	static geometry_msgs::PoseStamped zero;
+	geometry_msgs::PoseStamped zero;
 	zero.header.frame_id = local_frame_id;
 	zero.header.stamp = e.current_real;
 	zero.pose.orientation.w = 1;
@@ -109,7 +109,7 @@ void callback(const T& msg)
 	}
 }
 
-bool reset(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)
+bool reset([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)
 {
 	reset_flag = true;
 	res.success = true;
@@ -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("publish_zero", false)) {
+	if (nh_priv.param("force_init", false) || nh_priv.param("publish_zero", false)) { // publish_zero is old name
 		// publish zero to initialize the local position
 		zero_timer = nh.createTimer(ros::Duration(0.1), &publishZero);
-		publish_zero_timout = ros::Duration(nh_priv.param("publish_zero_timout", 5.0));
-		publish_zero_duration = ros::Duration(nh_priv.param("publish_zero_duration", 5.0));
+		publish_zero_timout = ros::Duration(nh_priv.param("force_init_timeout", 5.0));
+		publish_zero_duration = ros::Duration(nh_priv.param("force_init_duration", 5.0));
 		local_position_sub = nh.subscribe("mavros/local_position/pose", 1, &localPositionCallback);
 	}
 

clover/src/waitfile

@@ -0,0 +1,9 @@
+#!/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}"

clover/test/basic.py

@@ -26,21 +26,36 @@ def test_simple_offboard_services_available():
     rospy.wait_for_service('land', timeout=5)
 
 def test_web_video_server(node):
-    import urllib2
-    urllib2.urlopen("http://localhost:8080").read()
+    try:
+        # Python 2
+        import urllib2 as urllib
+    except ModuleNotFoundError:
+        # Python 3
+        import urllib.request as urllib
+    urllib.urlopen("http://localhost:8080").read()
 
-def test_shell(node):
-    execute = rospy.ServiceProxy('exec', srv.Execute)
-    execute.wait_for_service(5)
+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)
 
-    res = execute(cmd='echo foo')
-    assert res.code == 0
-    assert res.output == 'foo\n'
+    from std_msgs.msg import String
+    from clover_blocks.srv import Run
 
-    res = execute(cmd='foo')
-    assert res.code == 32512
-    assert res.output == ''
+    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='ls foo')
-    assert res.code == 512
-    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'

clover/test/basic.test

@@ -23,10 +23,7 @@
 
     <node pkg="tf2_ros" type="static_transform_publisher" name="map_flipped_frame" args="0 0 0 3.1415926 3.1415926 0 map map_flipped" required="true"/>
 
-    <node name="simple_offboard" pkg="clover" type="simple_offboard" required="true" output="screen">
-        <param name="reference_frames/body" value="map"/>
-        <param name="reference_frames/base_link" value="map"/>
-    </node>
+    <node name="simple_offboard" pkg="clover" type="simple_offboard" required="true" output="screen"/>
 
     <node name="tf2_web_republisher" pkg="tf2_web_republisher" type="tf2_web_republisher" required="true"/>
 
@@ -38,6 +35,8 @@
         <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>

clover/udev/99-px4fmu.rules

@@ -12,4 +12,6 @@ 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"
 

clover/www/clover.log

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

clover/www/clover_version

@@ -0,0 +1 @@
+/etc/clover_version

clover/www/console.html

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

clover/www/index.html

@@ -4,24 +4,33 @@
 
 <ul>
 	<li><a href="docs">View documentation</a> (snapshot of <a href="https://clover.coex.tech">clover.coex.tech</a>)</li>
+	<li><a href="topics.html">View topics</a></li>
 	<li><a href="" id="wvs">View image topics</a> (<code>web_video_server</code>)</li>
 	<li><a href="" id="butterfly">Open web terminal</a> (<code>Butterfly</code>)</li>
-	<li><a href="viz.html">View 3D visualization</a> (<code>ros3djs</code>)</li>
-	<li><a href="aruco_map.html">3D visualization for markers map</a> (<code>ros3djs</code>)</li>
+	<li>View <a href="viz.html">View 3D visualization</a>, <a href="aruco_map.html">3D visualization for markers map</a> (<code>ros3djs</code>)</li>
 	<li><a href="../clover_blocks/">Blocks programming</a> (<code>Blockly</code>)</li>
+	<li><a href="console.html">Clover console</a> (<code>/var/log/clover.log</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
-	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;
+	fetch('clover_version').then(function(response) {
+		if (response.status !== 200) return;
+		response.text().then(function(text) {
+			document.querySelector('.version').innerHTML = 'Version: ' + text;
+		});
 	});
 </script>

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

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

clover/www/js/topics.js

@@ -0,0 +1,83 @@
+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);
+		});
+	}
+}

clover/www/topics.html

@@ -0,0 +1,28 @@
+<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>

clover_blocks/CMakeLists.txt

@@ -73,6 +73,13 @@ 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 ##
 #############

clover_blocks/README.md

@@ -10,7 +10,7 @@ Internal package documentation is given below.
 
 ## Frontend
 
-The frontend files are located in [`www`](./www/) subdirectory. The frontend application uses [`roblib.js`](http://wiki.ros.org/roslibjs) library for communicating with backend node and other ROS resources.
+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
 
@@ -30,7 +30,8 @@ The frontend files are located in [`www`](./www/) subdirectory. The frontend app
 Parameters read by frontend:
 
 * `~navigate_tolerance` (*float*) – distance tolerance in meters, used for navigate-like blocks (default: 0.2).
-* `~yaw_tolerance` (*float*) – yaw angle tolerance in degrees, used in set_yaw block (default: 20).
+* `~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).
 

clover_blocks/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>clover_blocks</name>
-  <version>0.21.1</version>
+  <version>0.23.0</version>
   <description>Blockly programming support for Clover</description>
   <maintainer email="okalachev@gmail.com">Oleg Kalachev</maintainer>
   <license>MIT</license>

clover_blocks/src/clover_blocks

@@ -11,7 +11,8 @@
 from __future__ import print_function
 
 import rospy
-import os
+import os, sys
+import traceback
 import threading
 import re
 import uuid
@@ -111,12 +112,17 @@ def run(req):
                 'print': _print,
                 'raw_input': _input}
             try:
-                exec req.code in g
+                exec(req.code, g)
             except Stop:
                 rospy.loginfo('Program forced to stop')
             except Exception as e:
                 rospy.logerr(str(e))
-                error_pub.publish(str(e))
+                traceback.print_exc()
+                etype, value, tb = sys.exc_info()
+                fmt = traceback.format_exception(etype, value, tb)
+                fmt.pop(1) # remove 'clover_blocks' file frame
+                exc_info = ''.join(fmt)
+                error_pub.publish(str(e) + '\n\n' + exc_info)
 
             rospy.loginfo('Program terminated')
             running_lock.release()
@@ -140,6 +146,7 @@ def stop(req):
     return {'success': True}
 
 
+# TODO: find dir in installed package
 programs_path = rospy.get_param('~programs_dir', os.path.dirname(os.path.abspath(__file__)) + '/../programs')
 
 

clover_blocks/www/blocks.js

@@ -31,6 +31,14 @@ function considerFrameId(e) {
 			this.getInput('Y').fieldRow[0].setValue('y');
 			this.getInput('Z').fieldRow[0].setValue('z');
 		}
+		if (this.getInput('LAT')) { // block has global coordinates
+			let global = frameId.startsWith('GLOBAL');
+			this.getInput('LAT').setVisible(global);
+			this.getInput('LON').setVisible(global);
+			this.getInput('X').setVisible(!global);
+			this.getInput('Y').setVisible(!global);
+			this.getInput('Z').fieldRow[0].setValue(frameId == 'GLOBAL' ? 'altitude' : 'z');
+		}
 	}
 	if (this.getInput('ID')) { // block has marker id field
 		this.getInput('ID').setVisible(frameId == 'ARUCO'); // toggle id field
@@ -65,6 +73,9 @@ function updateSetpointBlock(e) {
 
 Blockly.Blocks['navigate'] = {
 	init: function () {
+		let navFrameId = frameIds.slice();
+		navFrameId.push(['global', 'GLOBAL_LOCAL'])
+		navFrameId.push(['global, WGS 84 alt.', 'GLOBAL'])
 		this.appendDummyInput()
 			.appendField("navigate to point");
 		this.appendValueInput("X")
@@ -73,12 +84,20 @@ Blockly.Blocks['navigate'] = {
 		this.appendValueInput("Y")
 			.setCheck("Number")
 			.appendField("left");
+		this.appendValueInput("LAT")
+			.setCheck("Number")
+			.appendField("latitude")
+			.setVisible(false);
+		this.appendValueInput("LON")
+			.setCheck("Number")
+			.appendField("longitude")
+			.setVisible(false)
 		this.appendValueInput("Z")
 			.setCheck("Number")
 			.appendField("up");
 		this.appendDummyInput()
 			.appendField("relative to")
-			.appendField(new Blockly.FieldDropdown(frameIds), "FRAME_ID");
+			.appendField(new Blockly.FieldDropdown(navFrameId), "FRAME_ID");
 		this.appendValueInput("ID")
 			.setCheck("Number")
 			.appendField("with ID")
@@ -268,7 +287,7 @@ Blockly.Blocks['mode'] = {
 			.appendField("current flight mode");
 		this.setOutput(true, "String");
 		this.setColour(COLOR_STATE);
-		this.setTooltip("");
+		this.setTooltip("Returns current flight mode (POSCTL, OFFBOARD, etc).");
 		this.setHelpUrl(DOCS_URL + '#' + this.type);
 	}
 };
@@ -375,7 +394,7 @@ Blockly.Blocks['take_off'] = {
 		this.setPreviousStatement(true, null);
 		this.setNextStatement(true, null);
 		this.setColour(COLOR_FLIGHT);
-		this.setTooltip("Take off on desired altitude in meters");
+		this.setTooltip("Take off on desired altitude in meters.");
 		this.setHelpUrl(DOCS_URL + '#' + this.type);
 	}
 };
@@ -391,7 +410,7 @@ Blockly.Blocks['land'] = {
 		this.setPreviousStatement(true, null);
 		this.setNextStatement(true, null);
 		this.setColour(COLOR_FLIGHT);
-		this.setTooltip("");
+		this.setTooltip("Land the drone.");
 		this.setHelpUrl(DOCS_URL + '#' + this.type);
 	}
 };
@@ -400,10 +419,10 @@ Blockly.Blocks['global_position'] = {
 	init: function () {
 		this.appendDummyInput()
 			.appendField("current")
-			.appendField(new Blockly.FieldDropdown([["latitude", "LATITUDE"], ["longitude", "LONGITUDE"], ["altitude", "ALTITUDE"]]), "NAME");
+			.appendField(new Blockly.FieldDropdown([["latitude", "LAT"], ["longitude", "LON"], ["altitude", "ALT"]]), "FIELD");
 		this.setOutput(true, "Number");
-		this.setColour(230);
-		this.setTooltip("");
+		this.setColour(COLOR_STATE);
+		this.setTooltip("Returns current global position (latitude, longitude, altitude above the WGS 84 ellipsoid).");
 		this.setHelpUrl(DOCS_URL + '#' + this.type);
 	}
 };

clover_blocks/www/index.html

@@ -52,6 +52,8 @@
 				<value name="X"><shadow type="math_number"><field name="NUM">0</field></shadow></value>
 				<value name="Y"><shadow type="math_number"><field name="NUM">0</field></shadow></value>
 				<value name="Z"><shadow type="math_number"><field name="NUM">0</field></shadow></value>
+				<value name="LAT"><shadow type="math_number"><field name="NUM">47.397503</field></shadow></value>
+				<value name="LON"><shadow type="math_number"><field name="NUM">8.544945</field></shadow></value>
 				<value name="SPEED"><shadow type="math_number"><field name="NUM">0.5</field></shadow></value>
 				<value name="ID"><shadow type="math_number"><field name="NUM">0</field></shadow></value>
 			</block>
@@ -85,6 +87,7 @@
 				<value name="ID"><shadow type="math_number"><field name="NUM">0</field></shadow></value>
 			</block>
 			<block type="get_attitude"></block>
+			<block type="global_position"></block>
 			<block type="distance">
 				<value name="X"><shadow type="math_number"><field name="NUM">0</field></shadow></value>
 				<value name="Y"><shadow type="math_number"><field name="NUM">0</field></shadow></value>

clover_blocks/www/main.js

@@ -39,7 +39,8 @@ var workspace = Blockly.inject('blockly', {
 function readParams() {
 	return Promise.all([
 		ros.readParam('navigate_tolerance', true, 0.2),
-		ros.readParam('yaw_tolerance', true, 20),
+		ros.readParam('navigate_global_tolerance', true, 1),
+		ros.readParam('yaw_tolerance', true, 1),
 		ros.readParam('sleep_time', true, 0.2),
 		ros.readParam('confirm_run', true, true),
 	]);
@@ -210,7 +211,7 @@ function loadPrograms() {
 		updateChanged();
 	}, function(err) {
 		document.querySelector('.backend-fail').style.display = 'inline';
-		alert(`Error loading programs list.\n\nHave you enabled clover_blocks in clover.launch?`);
+		alert(`Error loading programs list.\n\nHave you enabled 'blocks' in clover.launch?`);
 		runButton.disabled = true;
 	})
 }

clover_blocks/www/python.js

@@ -33,6 +33,18 @@ const NAVIGATE_WAIT = () => `\ndef navigate_wait(x=0, y=0, z=0, speed=0.5, frame
             return
         rospy.sleep(${params.sleep_time})\n`;
 
+const NAVIGATE_GLOBAL_WAIT = () => `\ndef navigate_global_wait(lat, lon, z, speed=0.5):
+    res = navigate_global(lat=lat, lon=lon, z=z, yaw=float('inf'), speed=speed)
+
+    if not res.success:
+        raise Exception(res.message)
+
+    while not rospy.is_shutdown():
+        telem = get_telemetry(frame_id='navigate_target')
+        if math.sqrt(telem.x ** 2 + telem.y ** 2 + telem.z ** 2) < ${params.navigate_global_tolerance}:
+            return
+        rospy.sleep(${params.sleep_time})\n`;
+
 const LAND_WAIT = () => `\ndef land_wait():
     land()
     while get_telemetry().armed:
@@ -68,6 +80,9 @@ function generateROSDefinitions() {
 	if (rosDefinitions.offboard) {
 		code += `get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry)\n`;
 		code += `navigate = rospy.ServiceProxy('navigate', srv.Navigate)\n`;
+		if (rosDefinitions.navigateGlobal) {
+		code += `navigate_global = rospy.ServiceProxy('navigate_global', srv.NavigateGlobal)\n`;
+		}
 		if (rosDefinitions.setVelocity) {
 			code += `set_velocity = rospy.ServiceProxy('set_velocity', srv.SetVelocity)\n`;
 		}
@@ -94,6 +109,10 @@ function generateROSDefinitions() {
 		Blockly.Python.definitions_['import_math'] = 'import math';
 		code += NAVIGATE_WAIT();
 	}
+	if (rosDefinitions.navigateGlobalWait) {
+		Blockly.Python.definitions_['import_math'] = 'import math';
+		code += NAVIGATE_GLOBAL_WAIT();
+	}
 	if (rosDefinitions.landWait) {
 		code += LAND_WAIT();
 	}
@@ -161,24 +180,48 @@ Blockly.Python.navigate = function(block) {
 	let x = Blockly.Python.valueToCode(block, 'X', Blockly.Python.ORDER_NONE);
 	let y = Blockly.Python.valueToCode(block, 'Y', Blockly.Python.ORDER_NONE);
 	let z = Blockly.Python.valueToCode(block, 'Z', Blockly.Python.ORDER_NONE);
-	let frameId = buildFrameId(block);
+	let lat = Blockly.Python.valueToCode(block, 'LAT', Blockly.Python.ORDER_NONE);
+	let lon = Blockly.Python.valueToCode(block, 'LON', Blockly.Python.ORDER_NONE);
+	let wait = block.getFieldValue('WAIT') == 'TRUE';
+	let frameId = block.getFieldValue('FRAME_ID');
 	let speed = Blockly.Python.valueToCode(block, 'SPEED', Blockly.Python.ORDER_NONE);
 
-	let params = [`x=${x}`, `y=${y}`, `z=${z}`, `frame_id=${frameId}`, `speed=${speed}`];
-
 	simpleOffboard();
 
-	if (block.getFieldValue('WAIT') == 'TRUE') {
-		rosDefinitions.navigateWait = true;
+	// global coordinates
+	if (frameId.startsWith('GLOBAL')) {
+		rosDefinitions.navigateGlobal = true;
 		simpleOffboard();
 
-		return `navigate_wait(${params.join(', ')})\n`;
+		if (frameId == 'GLOBAL') {
+			z = `${z} + get_telemetry().alt - get_telemetry().z`;
+		}
+
+		if (wait) {
+			rosDefinitions.navigateGlobalWait = true;
+			simpleOffboard();
+			return `navigate_global_wait(lat=${lat}, lon=${lon}, z=${z}, speed=${speed})\n`;
+
+		} else {
+			return `navigate_global(lat=${lat}, lon=${lon}, z=${z}, yaw=float('inf'), speed=${speed})\n`;
+		}
 
 	} else {
-		if (frameId != 'body') {
-			params.push(`yaw=float('nan')`);
+		frameId = buildFrameId(block);
+		let params = [`x=${x}`, `y=${y}`, `z=${z}`, `frame_id=${frameId}`, `speed=${speed}`];
+
+		if (wait) {
+			rosDefinitions.navigateWait = true;
+			simpleOffboard();
+
+			return `navigate_wait(${params.join(', ')})\n`;
+
+		} else {
+			if (frameId != 'body') {
+				params.push(`yaw=float('nan')`);
+			}
+			return `navigate(${params.join(', ')})\n`;
 		}
-		return `navigate(${params.join(', ')})\n`;
 	}
 }
 
@@ -315,6 +358,12 @@ Blockly.Python.get_attitude = function(block) {
 	return [code, Blockly.Python.ORDER_FUNCTION_CALL];
 }
 
+Blockly.Python.global_position = function(block) {
+	simpleOffboard();
+	var code = `get_telemetry().${block.getFieldValue('FIELD').toLowerCase()}`;
+	return [code, Blockly.Python.ORDER_FUNCTION_CALL];
+}
+
 Blockly.Python.distance = function(block) {
 	rosDefinitions.distance = true;
 	simpleOffboard();
@@ -391,7 +440,7 @@ Blockly.Python.set_led = function(block) {
 
 	if (/^'(.*)'$/.test(colorCode)) { // is simple string
 		let color = parseColor(colorCode);
-		return `set_leds([LEDState(index=${index}, r=${color.r}, g=${color.g}, b=${color.b})])\n`;
+		return `set_leds([LEDState(index=int(${index}), r=${color.r}, g=${color.g}, b=${color.b})])\n`; // TODO: check for simple int
 	} else {
 		let parseColor = Blockly.Python.provideFunction_('parse_color', [PARSE_COLOR]);
 		return `set_leds([LEDState(index=${index}, **${parseColor}(${colorCode}))])\n`;
@@ -415,7 +464,7 @@ Blockly.Python.led_count = function(block) {
 
 function pigpio() {
 	Blockly.Python.definitions_['import_pigpio'] = 'import pigpio';
-	Blockly.Python.definitions_['init_pigpio'] = 'pi = pigpio.pi()';
+	Blockly.Python.definitions_['init_pigpio'] = 'pi = pigpio.pi()\nif not pi.connected: raise Exception(\'Cannot connect to pigpiod\')';
 }
 
 const GPIO_READ = `\ndef gpio_read(pin):

clover_description/README.md

@@ -14,7 +14,7 @@ You may provide additional parameters for `spawn_drone.launch` as well:
 * `rangefinder` (*boolean*, default: *true*) - controls whether the drone will have a downward-facing laser rangefinder attached;
 * `led` (*boolean*, default: *true*) - controls whether the drone will have a programmable LED strip (requires plugins from `clover_simulation`);
 * `gps` (*boolean*, default: *true*) - controls whether the drone will have a simulated GPS attached (requires plugins from `sitl_gazebo`);
-* `maintain_camera_rate` (*boolean*, default: *false*) - slow down the simultion to maintain camera publishing rate (internally changes the camera plugin from `libgazebo_ros_camera.so` to `libthrottling_camera.so` from [`clover_simulation`](../clover_simulation/README.md#throttling-camera-plugin-throttling_camera)).
+* `maintain_camera_rate` (*boolean*, default: *false*) - slow down the simulation to maintain camera publishing rate (internally changes the camera plugin from `libgazebo_ros_camera.so` to `libthrottling_camera.so` from [`clover_simulation`](../clover_simulation/README.md#throttling-camera-plugin-throttling_camera)).
 
 For example, in order to spawn a drone without a GPS module, you may use the following command:
 

clover_description/launch/view_drone.launch

@@ -1,7 +1,7 @@
 <launch>
-  <arg name="model" default="$(find clover_description)/urdf/drones/clover4.xacro"/>
+  <arg name="model" default="$(find clover_description)/urdf/clover/clover4.xacro"/>
 
-  <param name="robot_description" command="$(find xacro)/xacro.py $(arg model)"/>
+  <param name="robot_description" command="xacro $(arg model)"/>
 
   <node name="rviz" pkg="rviz" type="rviz" required="true"/>
 </launch>

clover_description/package.xml

@@ -1,6 +1,6 @@
 <package format="2">
   <name>clover_description</name>
-  <version>0.21.1</version>
+  <version>0.23.0</version>
   <description>The clover_description package provides URDF models of the Clover series of quadcopters.</description>
 
   <maintainer email="sfalexrog@gmail.com">Alexey Rogachevskiy</maintainer>

clover_description/urdf/clover/clover4.xacro

@@ -1,5 +1,5 @@
 <?xml version="1.0"?>
-<robot name="clover_camera" xmlns:xacro="http://ros.org/wiki/xacro">
+<robot name="clover" xmlns:xacro="http://ros.org/wiki/xacro">
 
   <xacro:arg name="main_camera" default="true"/>
   <xacro:arg name="rangefinder" default="true"/>
@@ -8,10 +8,10 @@
   <xacro:arg name="maintain_camera_rate" default="false"/>
   <xacro:arg name="use_clover_physics" default="false"/>
 
-  <xacro:include filename="$(find clover_description)/urdf/clover/clover4_base.xacro" />
-  <xacro:include filename="$(find clover_description)/urdf/sensors/rpi_cam.urdf.xacro"/>
-  <xacro:include filename="$(find clover_description)/urdf/sensors/distance_sensor.urdf.xacro"/>
-  <xacro:include filename="$(find clover_description)/urdf/leds/led_strip.xacro"/>
+  <xacro:include filename="clover4_base.xacro" />
+  <xacro:include filename="../sensors/rpi_cam.urdf.xacro"/>
+  <xacro:include filename="../sensors/distance_sensor.urdf.xacro"/>
+  <xacro:include filename="../leds/led_strip.xacro"/>
 
   <!-- Create camera plugin -->
   <xacro:if value="$(arg main_camera)">
@@ -36,11 +36,17 @@
   </xacro:if>
 
   <xacro:if value="$(arg gps)">
-    <!-- Instantiate gps plugin. -->
-    <xacro:gps_plugin_macro
-      namespace="${namespace}"
-      gps_noise="true"
-      />
+    <gazebo>
+      <include>
+        <uri>model://gps</uri>
+        <pose>0.1 0 0 0 0 0</pose>
+        <name>gps0</name>
+      </include>
+      <joint name='gps0_joint' type='fixed'>
+        <child>gps0::link</child>
+        <parent>base_link</parent>
+      </joint>  
+    </gazebo>
   </xacro:if>
 
 </robot>

clover_description/urdf/clover/clover4_base.xacro

@@ -1,40 +1,15 @@
 <?xml version="1.0"?>
 
 <robot name="clover" xmlns:xacro="http://ros.org/wiki/xacro">
-  <!-- Properties that can be assigned at build time as arguments.
-  Is there a reason not to make all properties arguments?
-  -->
-  <xacro:arg name='name' default='iris' />
-  <xacro:arg name='mavlink_addr' default='INADDR_ANY' />
-  <xacro:arg name='mavlink_udp_port' default='14560' />
-  <xacro:arg name='mavlink_tcp_port' default='4560' />
-  <xacro:arg name='serial_enabled' default='false' />
-  <xacro:arg name='serial_device' default='/dev/ttyACM0' />
-  <xacro:arg name='baudrate' default='921600' />
-  <xacro:arg name='qgc_addr' default='INADDR_ANY' />
-  <xacro:arg name='qgc_udp_port' default='14550' />
-  <xacro:arg name='sdk_addr' default='INADDR_ANY' />
-  <xacro:arg name='sdk_udp_port' default='14540' />
-  <xacro:arg name='hil_mode' default='false' />
-  <xacro:arg name='hil_state_level' default='false' />
-  <xacro:arg name='send_vision_estimation' default='false' />
-  <xacro:arg name='send_odometry' default='false' />
-  <xacro:arg name='enable_lockstep' default='true' />
-  <xacro:arg name='use_tcp' default='true' />
-  <xacro:arg name='vehicle_is_tailsitter' default='false' />
-  <xacro:arg name='visual_material' default='DarkGrey' />
-  <xacro:arg name='enable_mavlink_interface' default='true' />
   <xacro:arg name='enable_wind' default='false' />
-  <!-- The following causes segfault with multiple vehicles if defaults to true!!! -->
-  <xacro:arg name='enable_ground_truth' default='false' />
   <xacro:arg name='enable_logging' default='false' />
-  <xacro:arg name='log_file' default='iris' />
+  <xacro:arg name='log_file' default='clover' />
 
   <!-- macros for gazebo plugins, sensors -->
-  <xacro:include filename="$(find clover_description)/urdf/component_snippets.urdf.xacro" />
+  <xacro:include filename="../component_snippets.urdf.xacro" />
 
   <!-- Instantiate iris "mechanics" -->
-  <xacro:include filename="$(find clover_description)/urdf/clover/clover4_physics.xacro" />
+  <xacro:include filename="clover4_physics.xacro" />
 
   <xacro:if value="$(arg enable_wind)">
     <xacro:wind_plugin_macro
@@ -49,126 +24,8 @@
         />
   </xacro:if>
 
-  <!-- Instantiate magnetometer plugin. -->
-  <xacro:magnetometer_plugin_macro
-    namespace="${namespace}"
-    pub_rate="20"
-    noise_density="0.0004"
-    random_walk="0.0000064"
-    bias_correlation_time="600"
-    mag_topic="/mag"
-    >
-  </xacro:magnetometer_plugin_macro>
-
-  <!-- Instantiate barometer plugin. -->
-  <xacro:barometer_plugin_macro
-    namespace="${namespace}"
-    pub_rate="10"
-    baro_topic="/baro"
-    >
-  </xacro:barometer_plugin_macro>
-
-  <xacro:if value="$(arg enable_mavlink_interface)">
-  <!-- Instantiate mavlink telemetry interface. -->
-  <xacro:mavlink_interface_macro
-    namespace="${namespace}"
-    imu_sub_topic="/imu"
-    gps_sub_topic="/gps"
-    mag_sub_topic="/mag"
-    baro_sub_topic="/baro"
-    mavlink_addr="$(arg mavlink_addr)"
-    mavlink_udp_port="$(arg mavlink_udp_port)"
-    mavlink_tcp_port="$(arg mavlink_tcp_port)"
-    serial_enabled="$(arg serial_enabled)"
-    serial_device="$(arg serial_device)"
-    baudrate="$(arg baudrate)"
-    qgc_addr="$(arg qgc_addr)"
-    qgc_udp_port="$(arg qgc_udp_port)"
-    sdk_addr="$(arg sdk_addr)"
-    sdk_udp_port="$(arg sdk_udp_port)"
-    hil_mode="$(arg hil_mode)"
-    hil_state_level="$(arg hil_state_level)"
-    vehicle_is_tailsitter="$(arg vehicle_is_tailsitter)"
-    send_vision_estimation="$(arg send_vision_estimation)"
-    send_odometry="$(arg send_odometry)"
-    enable_lockstep="$(arg enable_lockstep)"
-    use_tcp="$(arg use_tcp)"
-    >
-  </xacro:mavlink_interface_macro>
-  </xacro:if>
-
-  <!-- Mount an ADIS16448 IMU. -->
-  <xacro:imu_plugin_macro
-    namespace="${namespace}"
-    imu_suffix=""
-    parent_link="base_link"
-    imu_topic="/imu"
-    mass_imu_sensor="0.015"
-    gyroscope_noise_density="0.0003394"
-    gyroscopoe_random_walk="0.000038785"
-    gyroscope_bias_correlation_time="1000.0"
-    gyroscope_turn_on_bias_sigma="0.0087"
-    accelerometer_noise_density="0.004"
-    accelerometer_random_walk="0.006"
-    accelerometer_bias_correlation_time="300.0"
-    accelerometer_turn_on_bias_sigma="0.1960"
-  >
-    <inertia ixx="0.00001" ixy="0.0" ixz="0.0" iyy="0.00001" iyz="0.0" izz="0.00001" />
-    <origin xyz="0 0 0" rpy="0 0 0" />
-  </xacro:imu_plugin_macro>
-
-  <xacro:if value="$(arg enable_ground_truth)">
-    <!-- Mount an IMU providing ground truth. -->
-    <xacro:imu_plugin_macro
-      namespace="${namespace}"
-      imu_suffix="gt"
-      parent_link="base_link"
-      imu_topic="ground_truth/imu"
-      mass_imu_sensor="0.00001"
-      gyroscope_noise_density="0.0"
-      gyroscopoe_random_walk="0.0"
-      gyroscope_bias_correlation_time="1000.0"
-      gyroscope_turn_on_bias_sigma="0.0"
-      accelerometer_noise_density="0.0"
-      accelerometer_random_walk="0.0"
-      accelerometer_bias_correlation_time="300.0"
-      accelerometer_turn_on_bias_sigma="0.0"
-    >
-      <inertia ixx="0.00001" ixy="0.0" ixz="0.0" iyy="0.00001" iyz="0.0" izz="0.00001" />
-      <origin xyz="0 0 0" rpy="0 0 0" />
-    </xacro:imu_plugin_macro>
-
-    <!-- Mount a generic odometry sensor providing ground truth. -->
-    <xacro:odometry_plugin_macro
-      namespace="${namespace}/ground_truth"
-      odometry_sensor_suffix="gt"
-      parent_link="base_link"
-      pose_topic="pose"
-      pose_with_covariance_topic="pose_with_covariance"
-      position_topic="position"
-      transform_topic="transform"
-      odometry_topic="odometry"
-      parent_frame_id="world"
-      mass_odometry_sensor="0.00001"
-      measurement_divisor="1"
-      measurement_delay="0"
-      unknown_delay="0.0"
-      noise_normal_position="0 0 0"
-      noise_normal_quaternion="0 0 0"
-      noise_normal_linear_velocity="0 0 0"
-      noise_normal_angular_velocity="0 0 0"
-      noise_uniform_position="0 0 0"
-      noise_uniform_quaternion="0 0 0"
-      noise_uniform_linear_velocity="0 0 0"
-      noise_uniform_angular_velocity="0 0 0"
-      enable_odometry_map="false"
-      odometry_map=""
-      image_scale=""
-    >
-      <inertia ixx="0.00001" ixy="0.0" ixz="0.0" iyy="0.00001" iyz="0.0" izz="0.00001" /> <!-- [kg.m^2] [kg.m^2] [kg.m^2] [kg.m^2] [kg.m^2] [kg.m^2] -->
-      <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
-    </xacro:odometry_plugin_macro>
-  </xacro:if>
+  <!-- Gazebo plugins -->
+  <xacro:include filename="$(find clover_description)/urdf/clover/clover4_gazebo.xacro" />
 
   <xacro:if value="$(arg enable_logging)">
     <!-- Instantiate a logger -->

clover_description/urdf/clover/clover4_gazebo.xacro

@@ -0,0 +1,183 @@
+<?xml version="1.0"?>
+
+<!-- Reference: https://github.com/PX4/PX4-SITL_gazebo/blob/7505aee97d2d3112fb2bd95198946345c6fa0b07/models/iris/iris.sdf.jinja#L430 -->
+
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+
+  <!-- IMU link -->
+  <link name="/imu_link">
+    <inertial>
+      <inertia ixx="0.00001" ixy="0.0" ixz="0.0" iyy="0.00001" iyz="0.0" izz="0.00001"/>
+      <mass value="0.015"/>
+      <!-- [kg] -->
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+    </inertial>
+  </link>
+
+  <!-- IMU joint -->
+  <joint name="/imu_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <parent link="base_link"/>
+    <child link="/imu_link"/>
+    <!-- <limit upper="0" lower="0" effort="0" velocity="0" /> -->
+  </joint>
+
+  <gazebo reference="/imu_joint">
+    <disableFixedJointLumping>true</disableFixedJointLumping>
+    <preserveFixedJoint>true</preserveFixedJoint>
+  </gazebo>
+
+  <gazebo>
+    <plugin name='groundtruth_plugin' filename='libgazebo_groundtruth_plugin.so'>
+      <robotNamespace/>
+    </plugin>
+    <plugin name='magnetometer_plugin' filename='libgazebo_magnetometer_plugin.so'>
+      <robotNamespace/>
+      <pubRate>100</pubRate>
+      <noiseDensity>0.0004</noiseDensity>
+      <randomWalk>6.4e-06</randomWalk>
+      <biasCorrelationTime>600</biasCorrelationTime>
+      <magTopic>/mag</magTopic>
+    </plugin>
+    <plugin name='barometer_plugin' filename='libgazebo_barometer_plugin.so'>
+      <robotNamespace/>
+      <pubRate>50</pubRate>
+      <baroTopic>/baro</baroTopic>
+      <baroDriftPaPerSec>0</baroDriftPaPerSec>
+    </plugin>
+    <plugin name='mavlink_interface' filename='libgazebo_mavlink_interface.so'>
+      <robotNamespace/>
+      <imuSubTopic>/imu</imuSubTopic>
+      <magSubTopic>/mag</magSubTopic>
+      <baroSubTopic>/baro</baroSubTopic>
+      <mavlink_addr>INADDR_ANY</mavlink_addr>
+      <mavlink_tcp_port>4560</mavlink_tcp_port>
+      <mavlink_udp_port>14560</mavlink_udp_port>
+      <serialEnabled>false</serialEnabled>
+      <serialDevice>/dev/ttyACM0</serialDevice>
+      <baudRate>921600</baudRate>
+      <qgc_addr>INADDR_ANY</qgc_addr>
+      <qgc_udp_port>14550</qgc_udp_port>
+      <sdk_addr>INADDR_ANY</sdk_addr>
+      <sdk_udp_port>14540</sdk_udp_port>
+      <hil_mode>false</hil_mode>
+      <hil_state_level>0</hil_state_level>
+      <send_vision_estimation>0</send_vision_estimation>
+      <send_odometry>1</send_odometry>
+      <enable_lockstep>1</enable_lockstep>
+      <use_tcp>1</use_tcp>
+      <motorSpeedCommandPubTopic>/gazebo/command/motor_speed</motorSpeedCommandPubTopic>
+      <control_channels>
+        <channel name='rotor1'>
+          <input_index>0</input_index>
+          <input_offset>0</input_offset>
+          <input_scaling>1000</input_scaling>
+          <zero_position_disarmed>0</zero_position_disarmed>
+          <zero_position_armed>100</zero_position_armed>
+          <joint_control_type>velocity</joint_control_type>
+        </channel>
+        <channel name='rotor2'>
+          <input_index>1</input_index>
+          <input_offset>0</input_offset>
+          <input_scaling>1000</input_scaling>
+          <zero_position_disarmed>0</zero_position_disarmed>
+          <zero_position_armed>100</zero_position_armed>
+          <joint_control_type>velocity</joint_control_type>
+        </channel>
+        <channel name='rotor3'>
+          <input_index>2</input_index>
+          <input_offset>0</input_offset>
+          <input_scaling>1000</input_scaling>
+          <zero_position_disarmed>0</zero_position_disarmed>
+          <zero_position_armed>100</zero_position_armed>
+          <joint_control_type>velocity</joint_control_type>
+        </channel>
+        <channel name='rotor4'>
+          <input_index>3</input_index>
+          <input_offset>0</input_offset>
+          <input_scaling>1000</input_scaling>
+          <zero_position_disarmed>0</zero_position_disarmed>
+          <zero_position_armed>100</zero_position_armed>
+          <joint_control_type>velocity</joint_control_type>
+        </channel>
+        <channel name='rotor5'>
+          <input_index>4</input_index>
+          <input_offset>1</input_offset>
+          <input_scaling>324.6</input_scaling>
+          <zero_position_disarmed>0</zero_position_disarmed>
+          <zero_position_armed>0</zero_position_armed>
+          <joint_control_type>velocity</joint_control_type>
+          <joint_control_pid>
+            <p>0.1</p>
+            <i>0</i>
+            <d>0</d>
+            <iMax>0.0</iMax>
+            <iMin>0.0</iMin>
+            <cmdMax>2</cmdMax>
+            <cmdMin>-2</cmdMin>
+          </joint_control_pid>
+          <joint_name>zephyr_delta_wing::propeller_joint</joint_name>
+        </channel>
+        <channel name='rotor6'>
+          <input_index>5</input_index>
+          <input_offset>0</input_offset>
+          <input_scaling>0.524</input_scaling>
+          <zero_position_disarmed>0</zero_position_disarmed>
+          <zero_position_armed>0</zero_position_armed>
+          <joint_control_type>position</joint_control_type>
+          <joint_name>zephyr_delta_wing::flap_left_joint</joint_name>
+          <joint_control_pid>
+            <p>10.0</p>
+            <i>0</i>
+            <d>0</d>
+            <iMax>0</iMax>
+            <iMin>0</iMin>
+            <cmdMax>20</cmdMax>
+            <cmdMin>-20</cmdMin>
+          </joint_control_pid>
+        </channel>
+        <channel name='rotor7'>
+          <input_index>6</input_index>
+          <input_offset>0</input_offset>
+          <input_scaling>0.524</input_scaling>
+          <zero_position_disarmed>0</zero_position_disarmed>
+          <zero_position_armed>0</zero_position_armed>
+          <joint_control_type>position</joint_control_type>
+          <joint_name>zephyr_delta_wing::flap_right_joint</joint_name>
+          <joint_control_pid>
+            <p>10.0</p>
+            <i>0</i>
+            <d>0</d>
+            <iMax>0</iMax>
+            <iMin>0</iMin>
+            <cmdMax>20</cmdMax>
+            <cmdMin>-20</cmdMin>
+          </joint_control_pid>
+        </channel>
+        <channel name='rotor8'>
+          <input_index>7</input_index>
+          <input_offset>0</input_offset>
+          <input_scaling>0.524</input_scaling>
+          <zero_position_disarmed>0</zero_position_disarmed>
+          <zero_position_armed>0</zero_position_armed>
+          <joint_control_type>position</joint_control_type>
+        </channel>
+      </control_channels>
+    </plugin>
+    <static>0</static>
+    <plugin name='rotors_gazebo_imu_plugin' filename='libgazebo_imu_plugin.so'>
+      <robotNamespace/>
+      <linkName>/imu_link</linkName>
+      <imuTopic>/imu</imuTopic>
+      <gyroscopeNoiseDensity>0.00018665</gyroscopeNoiseDensity>
+      <gyroscopeRandomWalk>3.8785e-05</gyroscopeRandomWalk>
+      <gyroscopeBiasCorrelationTime>1000.0</gyroscopeBiasCorrelationTime>
+      <gyroscopeTurnOnBiasSigma>0.0087</gyroscopeTurnOnBiasSigma>
+      <accelerometerNoiseDensity>0.00186</accelerometerNoiseDensity>
+      <accelerometerRandomWalk>0.006</accelerometerRandomWalk>
+      <accelerometerBiasCorrelationTime>300.0</accelerometerBiasCorrelationTime>
+      <accelerometerTurnOnBiasSigma>0.196</accelerometerTurnOnBiasSigma>
+    </plugin>
+  </gazebo>
+
+</robot>

clover_description/urdf/clover/clover4_physics.xacro

@@ -6,7 +6,7 @@
 
 -->
 
-<robot name="iris" xmlns:xacro="http://ros.org/wiki/xacro">
+<robot name="clover" xmlns:xacro="http://ros.org/wiki/xacro">
   <!-- Properties -->
   <xacro:property name="namespace" value="" />
   <xacro:property name="rotor_velocity_slowdown_sim" value="10" />
@@ -35,7 +35,7 @@
   <xacro:property name="sqrt2" value="1.4142135623730951" />
   <xacro:property name="rotor_drag_coefficient" value="1.75e-04" />
   <xacro:property name="rolling_moment_coefficient" value="0.000001" />
-  <xacro:property name="color" value="$(arg visual_material)" />
+  <xacro:property name="color" value="DarkGrey" />
 
   <!-- Property Blocks -->
   <!-- Clover body inertia -->
@@ -84,7 +84,7 @@
   </xacro:unless>
 
   <!-- Included URDF Files -->
-  <xacro:include filename="$(find clover_description)/urdf/clover/clover4_macros.xacro" />
+  <xacro:include filename="clover4_macros.xacro" />
 
   <!-- Instantiate multirotor_base_macro once -->
   <xacro:clover4_base_macro

clover_description/urdf/leds/led_strip.xacro

@@ -64,6 +64,12 @@
         <!-- <gazebo>
             <static>true</static>
         </gazebo> -->
+        <gazebo>
+            <plugin name="${name}_ros_controller" filename="libsim_leds_controller.so">
+                <robotNamespace></robotNamespace>
+                <ledCount>${led_count}</ledCount>
+            </plugin>
+        </gazebo>
     </xacro:macro>
 
 </robot>

clover_description/urdf/sensors/distance_sensor.urdf.xacro

@@ -1,6 +1,6 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <robot name="rpi_camera" xmlns:xacro="http://ros.org/wiki/xacro">
-  <xacro:include filename="$(find clover_description)/urdf/camera_sensor.urdf.xacro"/>
+  <xacro:include filename="../camera_sensor.urdf.xacro"/>
 
   <xacro:macro name="distance_sensor" params="name:=lidar_vl53l1x parent x:=0 y:=0 z:=0 roll:=0 pitch:=0 yaw:=0 range_min:=0.01 range_max:=4.0 resolution:=0.01 rate:=10">
     <joint name="${name}_joint" type="fixed">

clover_description/urdf/sensors/pinhole_camera.urdf.xacro

@@ -1,5 +1,5 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <robot name="pinhole_camera" xmlns:xacro="http://ros.org/wiki/xacro">
-  <xacro:include filename="$(find clover_description)/urdf/camera_sensor.urdf.xacro"/>
+  <xacro:include filename="../camera_sensor.urdf.xacro"/>
   <xacro:camera_sensor name="main_camera" width="640" height="480" rate="30" horizontal_fov="${120.0*pi/180.0}"/>
 </robot>

clover_description/urdf/sensors/rpi_cam.urdf.xacro

@@ -1,6 +1,6 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <robot name="rpi_camera" xmlns:xacro="http://ros.org/wiki/xacro">
-  <xacro:include filename="$(find clover_description)/urdf/camera_sensor.urdf.xacro"/>
+  <xacro:include filename="../camera_sensor.urdf.xacro"/>
 
   <xacro:macro name="rpi_cam" params="name:=rpi_cam parent x:=0 y:=0 z:=0 roll:=0 pitch:=0 yaw:=0 width:=320 height:=240 rate:=40 do_throttling:=false">
     <joint name="${name}_joint" type="fixed">

clover_simulation/CMakeLists.txt

@@ -37,6 +37,14 @@ target_compile_options(sim_leds PRIVATE -std=c++11)
 
 add_dependencies(sim_leds ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
+add_library(sim_leds_controller src/sim_leds_controller.cpp)
+
+target_include_directories(sim_leds_controller PRIVATE ${catkin_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS})
+target_link_libraries(sim_leds_controller PRIVATE ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES})
+target_compile_options(sim_leds_controller PRIVATE -std=c++11)
+
+add_dependencies(sim_leds_controller ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
 # Gazebo throttling camera plugin
 # for some reason, CMake does not support per-target link directories, and Gazebo does not put
 # CameraPlugin into ${GAZEBO_LIBRARIES}
@@ -52,7 +60,7 @@ target_compile_options(throttling_camera PRIVATE -std=c++11)
 add_dependencies(throttling_camera ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
 install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
-install(DIRECTORY meshes DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+install(DIRECTORY models DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
 install(DIRECTORY resources DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
 
 catkin_install_python(PROGRAMS scripts/aruco_gen

clover_simulation/README.md

@@ -55,7 +55,7 @@ A visual Gazebo plugin is used for the LED strip. An example of the plugin usage
 The plugin accepts the following parameters during instantiation:
 
 * `robotNamespace` (*string*, default: "") - a ROS namespace for the plugin;
-* `ledCount` (*integer*, required) - total numer of LEDs in a strip.
+* `ledCount` (*integer*, required) - total number of LEDs in a strip.
 
 The plugin will provide the following service:
 

clover_simulation/airframes/4500_clover

@@ -0,0 +1,36 @@
+#!/bin/sh
+#
+# @name COEX Clover Simulator
+#
+# @type Quadrotor X
+#
+
+. ${R}etc/init.d-posix/airframes/10016_iris # base on iris
+
+param set ATT_W_EXT_HDG 0.5
+param set ATT_EXT_HDG_M 1
+
+param set COM_DISARM_LAND 1.0
+
+param set LPE_FLW_SCALE 1.0
+param set LPE_FLW_R 0.2
+param set LPE_FLW_RR 0.0
+param set LPE_FLW_QMIN 10
+param set LPE_VIS_DELAY 0.0
+param set LPE_VIS_Z 0.1
+param set LPE_FUSION 86
+
+param set SENS_FLOW_ROT 0
+param set SENS_FLOW_MINHGT 0.0
+param set SENS_FLOW_MAXHGT 4.0
+param set SENS_FLOW_MAXR 10.0
+
+param set EKF2_AID_MASK 26 # flow + vis pos + vis yaw
+param set EKF2_OF_DELAY 0
+param set EKF2_OF_QMIN 10
+param set EKF2_OF_N_MIN 0.05
+param set EKF2_OF_N_MAX 0.2
+param set EKF2_HGT_MODE 2
+param set EKF2_EVA_NOISE 0.1
+param set EKF2_EVP_NOISE 0.1
+param set EKF2_EV_DELAY 0