Typo

Generate fisheye_cam camera info for all the
different resolutions

.gitattributes

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

.github/workflows/build-image.yaml

@@ -0,0 +1,40 @@
+name: RPi image
+
+on:
+  push:
+    branches: [ '*' ]
+  pull_request:
+    branches: [ master ]
+  release:
+    types: [ created ]
+  workflow_dispatch:
+
+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 -lh . && unzip -l clover_*.zip
+      - name: Compress image using 7-Zip
+        run: |
+          cd images && sudo chmod -R 777 . && 7z a -mx=9 $(echo *_*).7z *_* && ls -lh . && 7z l *_*.7z
+      - name: Upload image
+        uses: softprops/action-gh-release@v1
+        if: ${{ github.event_name == 'release' }}
+        with:
+          files: images/*_*.zip
+          prerelease: true
+        env:
+          GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }}
+      - name: Upload image to artifacts
+        if: ${{ github.event_name == 'workflow_dispatch' || github.ref != 'refs/heads/master' }}
+        uses: actions/upload-artifact@v4
+        with:
+          name: image
+          path: images/*_*.7z
+          retention-days: 1

.github/workflows/build.yml

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

.github/workflows/docs.yml

@@ -0,0 +1,89 @@
+name: Documentation
+
+on:
+  push:
+    branches: [ '*' ]
+  pull_request:
+    branches: [ '*' ]
+  workflow_dispatch:
+
+permissions:
+  contents: read
+  pages: write
+  id-token: write
+
+defaults:
+  run:
+    shell: bash
+
+jobs:
+  docs:
+    runs-on: ubuntu-22.04
+    steps:
+      - uses: actions/checkout@v2
+      - name: Use Node.js
+        uses: actions/setup-node@v1
+        with: { node-version: '10' }
+      - name: Setup tools
+        run: |
+          sudo sh -c "echo ttf-mscorefonts-installer msttcorefonts/accepted-mscorefonts-eula select true | debconf-set-selections"
+          sudo apt-get update && sudo apt-get install -y calibre msttcorefonts
+          builder/assets/install_gitbook.sh
+          npm install markdownlint-cli@0.28.1 -g # FIXME: https://github.com/DavidAnson/markdownlint/issues/435
+          npm install svgexport -g
+          gitbook -V
+          markdownlint -V
+      - name: Run markdownlint
+        run: markdownlint docs
+      - name: Check Assets
+        run: |
+          ./check_assets_size.py
+          ./check_unused_assets.py
+      - name: Build GitBook
+        run: |
+         gitbook install
+         gitbook build
+      - name: Generate PDF
+        id: generate-pdf
+        env:
+          GITBOOK_SKIP_PDF: ${{ secrets.GITBOOK_SKIP_PDF }}
+        continue-on-error: ${{ env.GITBOOK_SKIP_PDF != '' }}
+        if: ${{ github.event_name == 'push' }}
+        run: |
+          for i in 1 2 3 4; do gitbook pdf ./ _book/clover.pdf && break || sleep 1; done
+          sudo apt-get -q install ghostscript
+          gs -sDEVICE=pdfwrite -dCompatibilityLevel=1.4 -dPDFSETTINGS=/default -dNOPAUSE -dQUIET -dBATCH -dDetectDuplicateImages -dCompressFonts=true -r150 -sOutputFile=_book/clover_ru_compressed.pdf _book/clover_ru.pdf
+          gs -sDEVICE=pdfwrite -dCompatibilityLevel=1.4 -dPDFSETTINGS=/default -dNOPAUSE -dQUIET -dBATCH -dDetectDuplicateImages -dCompressFonts=true -r150 -sOutputFile=_book/clover_en_compressed.pdf _book/clover_en.pdf
+          rm _book/clover_ru.pdf && mv _book/clover_ru_compressed.pdf _book/clover_ru.pdf
+          rm _book/clover_en.pdf && mv _book/clover_en_compressed.pdf _book/clover_en.pdf
+          ls -lah _book/clover*.pdf
+          echo '::set-output name=GITBOOK_PDF_OK::1'
+      - name: Download older PDFs
+        if: ${{ !steps.generate-pdf.outputs.GITBOOK_PDF_OK }}
+        run: |
+          rm -f _book/clover*.pdf
+          wget --no-verbose https://clover.coex.tech/clover_ru.pdf -P _book/
+          wget --no-verbose https://clover.coex.tech/clover_en.pdf -P _book/
+      - name: Upload artifact
+        # if: ${{ github.event_name == 'push' && github.ref == 'refs/heads/master' }}
+        uses: actions/upload-pages-artifact@v1
+        with:
+          path: _book
+
+  deploy-docs:
+    if: ${{ github.event_name == 'push' && github.ref == 'refs/heads/master' }}
+    concurrency:
+        group: "pages"
+        cancel-in-progress: true
+    environment:
+      name: github-pages
+      url: ${{ steps.deployment.outputs.page_url }}
+    runs-on: ubuntu-latest
+    needs: docs
+    steps:
+      - name: Deploy to GitHub Pages
+        env:
+          FREEZE_DOCS: ${{ secrets.FREEZE_DOCS }}
+        if: ${{ !env.FREEZE_DOCS }}
+        id: deployment
+        uses: actions/deploy-pages@v1

.github/workflows/editorconfig.yaml

@@ -0,0 +1,19 @@
+name: Editorconfig
+
+on:
+  push:
+    branches: [ '*' ]
+  pull_request:
+    branches: [ master ]
+  workflow_dispatch:
+
+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

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

.markdownlint.json

@@ -21,7 +21,9 @@
             "ROS",
             "ROS Kinetic",
             "ROS Melodic",
+            "ROS Noetic",
             "OpenCV",
+            "OpenVPN",
             "Gazebo",
             "GitHub",
             "FPV",
@@ -106,9 +108,14 @@
             "UDP",
             "QR",
             "Li-ion",
-            "Nvidia"
+            "Nvidia",
+            "VirtualBox",
+            "VMware",
+            "DuoCam"
         ],
-        "code_blocks": false
+        "code_blocks": false,
+        "html_elements": false
     },
-    "MD045": false
+    "MD045": false,
+    "MD051": false
 }

.travis.yml

@@ -1,121 +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"
-    - if [[ -z ${TRAVIS_TAG} ]]; then IMAGE_VERSION="${TRAVIS_COMMIT}}"; else IMAGE_VERSION="${TRAVIS_TAG}"; fi
-    - IMAGE_NAME="$(basename -s '.git' ${TARGET_REPO})_${IMAGE_VERSION}.img"
-git:
-  depth: 50
-jobs:
-  fast_finish: true
-  include:
-    - stage: Build
-      name: "Raspberry Pi Image Build"
-      cache:
-        directories:
-        - imgcache
-      before_script:
-        - docker pull ${DOCKER}
-# Check if there are any cached images, copy them to our "images" directory
-        - if [ -n "$(ls -A imgcache/*.zip)" ]; then mkdir -p images && cp imgcache/*.zip images; fi
-      script:
-        - if [[ -z ${TRAVIS_TAG} && "${TRAVIS_PULL_REQUEST}" != "false" ]]; then
-            echo "Commit range is ${TRAVIS_COMMIT_RANGE}" &&
-            if [ $(git diff --name-only ${TRAVIS_COMMIT_RANGE} | grep -v ^"docs/" | wc -l) -eq 0 ]; then
-              echo " === Docs-only change; skipping build ===" &&
-              export SKIP_BUILD=true;
-            fi;
-          fi
-        - if [ -z ${SKIP_BUILD} ]; then
-            docker run --privileged --rm -v /dev:/dev -v $(pwd):/builder/repo -e TRAVIS_TAG="${TRAVIS_TAG}" ${DOCKER};
-          fi
-      before_cache:
-        - cp images/*.zip imgcache
-      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"
-        - sudo chmod -R 777 *
-        - cd images && zip ${IMAGE_NAME}.zip ${IMAGE_NAME}
-      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:
-        - npm install gitbook-cli -g
-        - npm install markdownlint-cli -g
-        - gitbook -V
-        - markdownlint -V
-      script:
-        - markdownlint docs
-        - ./check_assets_size.py
-        - ./check_unused_assets.py
-        - gitbook install
-        - gitbook build
-      deploy:
-        provider: pages
-        local_dir: _book
-        skip_cleanup: true
-        token: ${GITHUB_OAUTH_TOKEN}
-        keep_history: true
-        target_branch: master
-        repo: CopterExpress/clover.coex.tech
-        fqdn: clover.coex.tech
-        verbose: true
-        on:
-          branch: master
-    - stage: Annotate
-      name: Auto-generate changelog
-      language: python
-      python: 3.6
-      install:
-        - pip install GitPython PyGithub
-      script:
-        - PYTHONUNBUFFERED=1 python ./gen_changelog.py
-    - stage: Build
-      name: Editorconfig-lint
-      language: generic
-      before_script:
-        - wget https://github.com/okalachev/editorconfig-checker/releases/download/1.2.1-disable-spaces-amount/ec-linux-amd64
-        - chmod +x ec-linux-amd64
-      script:
-        - ./ec-linux-amd64 -spaces-after-tabs -e "roslib.js|ros3d.js|eventemitter2.js|draw.cpp|BinUtils.swift|\.idea|apps/android/app|Assets.xcassets|test_parser_pass.txt|test_node_failure.txt|aruco_pose/vendor|\.stl|\.dxf"
-stages:
-  - Build
-  - Annotate
-# More info there
-# https://github.com/travis-ci/travis-ci/issues/6893
-# https://docs.travis-ci.com/user/customizing-the-build/
-# https://docs.travis-ci.com/user/deployment/releases
-# https://docs.travis-ci.com/user/environment-variables/

README.md

@@ -1,12 +1,14 @@
-# COEX Clover Drone Kit
+# clover🍀: create autonomous drones easily
 
-<img src="docs/assets/clever4-front-white.png" align="right" width="400px" alt="Clover Drone">
+<img src="docs/assets/clover42-main-margin.png" align="right" width="400px" alt="COEX Clover Drone">
 
-Clover is an educational programmable drone kit consisting of an unassembled quadcopter, open source software and documentation. The kit includes Pixracer-compatible autopilot running PX4 firmware, Raspberry Pi 4 as companion computer, a camera for computer vision navigation as well as additional sensors and peripheral devices.
+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.
 
-The main documentation is available [on Gitbook](https://clover.coex.tech/).
+COEX Clover Drone is an educational programmable drone kit, suited perfectly for running clover software. The kit is shipped unassembled and includes Pixracer-compatible autopilot running PX4 firmware, Raspberry Pi 4 as a companion computer, a camera for computer vision navigation as well as additional sensors and peripheral devices. Batteries included.
 
-Official website: <a href="https://coex.tech/clover">coex.tech/clover</a>.
+The main documentation is available at [https://clover.coex.tech](https://clover.coex.tech/). Official website: [coex.tech/clover](https://coex.tech/clover).
+
+[__Support us on Kickstarter!__](https://www.kickstarter.com/projects/copterexpress/cloverdrone)
 
 ## Video compilation
 
@@ -18,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/actions/workflow/status/CopterExpress/clover/build-image.yaml?branch=master)
+![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)
@@ -35,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/CHANGELOG.rst

@@ -0,0 +1,8 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package aruco_pose
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.21.1 (2020-11-17)
+-------------------
+* First release of aruco_pose package to ROS
+* Contributors: Alamoris, Alexey Rogachevskiy, Arthur Golubtsov, Ilya Petrov, Oleg Kalachev

aruco_pose/CMakeLists.txt

@@ -1,8 +1,6 @@
 cmake_minimum_required(VERSION 3.0)
 project(aruco_pose)
 
-add_definitions(-std=c++11 -Wall -g)
-
 ## Compile as C++11, supported in ROS Kinetic and newer
 add_compile_options(-std=c++11)
 
@@ -24,13 +22,21 @@ find_package(catkin REQUIRED COMPONENTS
   dynamic_reconfigure
 )
 
-find_package(OpenCV 3 REQUIRED COMPONENTS core imgproc calib3d)
-if ("${OpenCV_VERSION_MINOR}" LESS "3")
+# 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}")
@@ -77,11 +83,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(
@@ -113,7 +118,8 @@ generate_messages(
 
 ## Generate dynamic reconfigure parameters in the 'cfg' folder
 generate_dynamic_reconfigure_options(
-  cfg/DetectorParams.cfg
+  cfg/Detector.cfg
+  cfg/Map.cfg
 )
 
 ###################################
@@ -174,6 +180,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 ##
 #############
@@ -189,11 +202,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}/
@@ -209,6 +222,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 ##
 #############
@@ -229,4 +250,6 @@ if (CATKIN_ENABLE_TESTING)
   add_rostest(test/test_parser_empty_map.test)
   add_rostest(test/test_node_failure.test)
   add_rostest(test/largemap.test)
+  add_rostest(test/crash_opencv.test)
+  add_rostest(test/duplicate.test)
 endif()

aruco_pose/README.md

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

aruco_pose/cfg/Detector.cfg

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

aruco_pose/cfg/Map.cfg

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

aruco_pose/map/cmit.txt

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

aruco_pose/map/office.txt

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

aruco_pose/map/office_ceiling.txt

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

aruco_pose/package.xml

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

aruco_pose/src/aruco_detect.cpp

@@ -48,7 +48,9 @@
 #include <aruco_pose/Marker.h>
 #include <aruco_pose/MarkerArray.h>
 #include <aruco_pose/DetectorConfig.h>
+#include <aruco_pose/SetMarkers.h>
 
+#include "draw.h"
 #include "utils.h"
 #include <memory>
 #include <functional>
@@ -58,21 +60,24 @@ using cv::Mat;
 
 class ArucoDetect : public nodelet::Nodelet {
 private:
-	ros::NodeHandle nh_, nh_priv_;
-	tf2_ros::TransformBroadcaster br_;
-	tf2_ros::Buffer tf_buffer_;
-	tf2_ros::TransformListener tf_listener_{tf_buffer_};
+	std::unique_ptr<tf2_ros::TransformBroadcaster> br_;
+	std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
+	std::unique_ptr<tf2_ros::TransformListener> tf_listener_;
 	std::shared_ptr<dynamic_reconfigure::Server<aruco_pose::DetectorConfig>> dyn_srv_;
+	bool enabled_ = true;
 	cv::Ptr<cv::aruco::Dictionary> dictionary_;
 	cv::Ptr<cv::aruco::DetectorParameters> parameters_;
 	image_transport::Publisher debug_pub_;
 	image_transport::CameraSubscriber img_sub_;
 	ros::Publisher markers_pub_, vis_markers_pub_;
 	ros::Subscriber map_markers_sub_;
-	bool estimate_poses_, send_tf_, auto_flip_;
+	ros::ServiceServer set_markers_srv_;
+	bool estimate_poses_, send_tf_, flip_vertical_, auto_flip_, use_map_markers_;
+	bool waiting_for_map_;
 	double length_;
+	ros::Duration transform_timeout_;
 	std::unordered_map<int, double> length_override_;
-	std::string frame_id_prefix_, known_tilt_;
+	std::string frame_id_prefix_, known_vertical_;
 	Mat camera_matrix_, dist_coeffs_;
 	aruco_pose::MarkerArray array_;
 	std::unordered_set<int> map_markers_ids_;
@@ -81,45 +86,50 @@ private:
 public:
 	virtual void onInit()
 	{
-		nh_ = getNodeHandle();
-		nh_priv_ = getPrivateNodeHandle();
+		ros::NodeHandle& nh_ = getNodeHandle();
+		ros::NodeHandle& nh_priv_ = getPrivateNodeHandle();
+
+		br_.reset(new tf2_ros::TransformBroadcaster());
+		tf_buffer_.reset(new tf2_ros::Buffer());
+		tf_listener_.reset(new tf2_ros::TransformListener(*tf_buffer_, nh_));
 
 		int dictionary;
-		nh_priv_.param("dictionary", dictionary, 2);
-		nh_priv_.param("estimate_poses", estimate_poses_, true);
-		nh_priv_.param("send_tf", send_tf_, true);
+		dictionary = nh_priv_.param("dictionary", 2);
+		estimate_poses_ = nh_priv_.param("estimate_poses", true);
+		send_tf_ = nh_priv_.param("send_tf", true);
+		use_map_markers_ = nh_priv_.param("use_map_markers", false);
+		waiting_for_map_ = use_map_markers_;
 		if (estimate_poses_ && !nh_priv_.getParam("length", length_)) {
 			NODELET_FATAL("can't estimate marker's poses as ~length parameter is not defined");
 			ros::shutdown();
 		}
-		readLengthOverride();
+		readLengthOverride(nh_priv_);
+		transform_timeout_ = ros::Duration(nh_priv_.param("transform_timeout", 0.02));
 
-		nh_priv_.param<std::string>("known_tilt", known_tilt_, "");
-		nh_priv_.param("auto_flip", auto_flip_, false);
+		known_vertical_ = nh_priv_.param("known_vertical", nh_priv_.param("known_tilt", std::string(""))); // known_tilt is an old name
+		flip_vertical_ = nh_priv_.param<bool>("flip_vertical", false);
+		auto_flip_ = nh_priv_.param("auto_flip", false);
 
-		nh_priv_.param<std::string>("frame_id_prefix", frame_id_prefix_, "aruco_");
+		frame_id_prefix_ = nh_priv_.param<std::string>("frame_id_prefix", "aruco_");
 
 		camera_matrix_ = cv::Mat::zeros(3, 3, CV_64F);
-		dist_coeffs_ = cv::Mat::zeros(8, 1, CV_64F);
 
 		dictionary_ = cv::aruco::getPredefinedDictionary(static_cast<cv::aruco::PREDEFINED_DICTIONARY_NAME>(dictionary));
 		parameters_ = cv::aruco::DetectorParameters::create();
-		parameters_->cornerRefinementMethod = cv::aruco::CORNER_REFINE_SUBPIX;
 
 		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");
 	}
@@ -127,13 +137,16 @@ public:
 private:
 	void imageCallback(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr &cinfo)
 	{
-		Mat image = cv_bridge::toCvShare(msg, "bgr8")->image;
+		if (!enabled_) return;
+		if (waiting_for_map_) return;
+
+		Mat image = cv_bridge::toCvShare(msg)->image;
 
 		vector<int> ids;
 		vector<vector<cv::Point2f>> corners, rejected;
 		vector<cv::Vec3d> rvecs, tvecs;
 		vector<cv::Point3f> obj_points;
-		geometry_msgs::TransformStamped snap_to;
+		geometry_msgs::TransformStamped vertical;
 
 		// Detect markers
 		cv::aruco::detectMarkers(image, dictionary_, corners, ids, parameters_, rejected);
@@ -168,18 +181,20 @@ private:
 					}
 				}
 
-				if (!known_tilt_.empty()) {
+				if (!known_vertical_.empty()) {
 					try {
-						snap_to = tf_buffer_.lookupTransform(msg->header.frame_id, known_tilt_,
-						                                     msg->header.stamp, ros::Duration(0.02));
+						vertical = tf_buffer_->lookupTransform(msg->header.frame_id, known_vertical_,
+						                                       msg->header.stamp, transform_timeout_);
 					} catch (const tf2::TransformException& e) {
-						NODELET_WARN_THROTTLE(5, "can't snap: %s", e.what());
+						NODELET_WARN_THROTTLE(5, "can't retrieve known vertical: %s", e.what());
 					}
 				}
 			}
 
 			array_.markers.reserve(ids.size());
 			aruco_pose::Marker marker;
+			vector<geometry_msgs::TransformStamped> transforms;
+			transforms.reserve(ids.size());
 			geometry_msgs::TransformStamped transform;
 			transform.header.stamp = msg->header.stamp;
 			transform.header.frame_id = msg->header.frame_id;
@@ -192,25 +207,38 @@ private:
 				if (estimate_poses_) {
 					fillPose(marker.pose, rvecs[i], tvecs[i]);
 
-					// snap orientation (if enabled and snap frame available)
-					if (!known_tilt_.empty() && !snap_to.header.frame_id.empty()) {
-						snapOrientation(marker.pose.orientation, snap_to.transform.rotation, auto_flip_);
+					// apply known vertical (if enabled and vertical frame available)
+					if (!known_vertical_.empty() && !vertical.header.frame_id.empty()) {
+						applyVertical(marker.pose.orientation, vertical.transform.rotation, false, auto_flip_);
 					}
 
-					// TODO: check IDs are unique
 					if (send_tf_) {
 						transform.child_frame_id = getChildFrameId(ids[i]);
 
 						// check if such static transform is in the map
 						if (map_markers_ids_.find(ids[i]) == map_markers_ids_.end()) {
-							transform.transform.rotation = marker.pose.orientation;
-							fillTranslation(transform.transform.translation, tvecs[i]);
-							br_.sendTransform(transform);
+							// check if a markers with that id is already added
+							bool send = true;
+							for (auto &t : transforms) {
+								if (t.child_frame_id == transform.child_frame_id) {
+									send = false;
+									break;
+								}
+							}
+							if (send) {
+								transform.transform.rotation = marker.pose.orientation;
+								fillTranslation(transform.transform.translation, tvecs[i]);
+								transforms.push_back(transform);
+							}
 						}
 					}
 				}
 				array_.markers.push_back(marker);
 			}
+
+			if (send_tf_) {
+				br_->sendTransform(transforms);
+			}
 		}
 
 		markers_pub_.publish(array_);
@@ -237,8 +265,7 @@ private:
 			cv::aruco::drawDetectedMarkers(debug, corners, ids); // draw markers
 			if (estimate_poses_)
 				for (unsigned int i = 0; i < ids.size(); i++)
-					cv::aruco::drawAxis(debug, camera_matrix_, dist_coeffs_,
-					                    rvecs[i], tvecs[i], getMarkerLength(ids[i]));
+					_drawAxis(debug, camera_matrix_, dist_coeffs_, rvecs[i], tvecs[i], getMarkerLength(ids[i]));
 
 			cv_bridge::CvImage out_msg;
 			out_msg.header.frame_id = msg->header.frame_id;
@@ -326,10 +353,10 @@ private:
 		return frame_id_prefix_ + std::to_string(id);
 	}
 
-	void readLengthOverride()
+	void readLengthOverride(ros::NodeHandle& nh)
 	{
 		std::map<std::string, double> length_override;
-		nh_priv_.getParam("length_override", length_override);
+		nh.getParam("length_override", length_override);
 		for (auto const& item : length_override) {
 			length_override_[std::stoi(item.first)] = item.second;
 		}
@@ -345,16 +372,47 @@ private:
 		}
 	}
 
+	bool setMarkers(aruco_pose::SetMarkers::Request& req, aruco_pose::SetMarkers::Response& res)
+	{
+		for (auto const& marker : req.markers) {
+			if (marker.id > 999) {
+				res.message = "Invalid marker id: " + std::to_string(marker.id);
+				ROS_ERROR("%s", res.message.c_str());
+				return true;
+			}
+			if (!std::isfinite(marker.length) || marker.length <= 0) {
+				res.message = "Invalid marker " + std::to_string(marker.id) + " length: " + std::to_string(marker.length);
+				ROS_ERROR("%s", res.message.c_str());
+				return true;
+			}
+		}
+
+		for (auto const& marker : req.markers) {
+			length_override_[marker.id] = marker.length;
+		}
+
+		res.success = true;
+		return true;
+	}
+
 	void mapMarkersCallback(const aruco_pose::MarkerArray& msg)
 	{
 		map_markers_ids_.clear();
 		for (auto const& marker : msg.markers) {
 			map_markers_ids_.insert(marker.id);
+			if (use_map_markers_) {
+				if (length_override_.find(marker.id) == length_override_.end()) {
+					length_override_[marker.id] = marker.length;
+				}
+			}
 		}
+		waiting_for_map_ = false;
 	}
 
 	void paramCallback(aruco_pose::DetectorConfig &config, uint32_t level)
 	{
+		enabled_ = config.enabled && config.length > 0;
+		length_ = config.length;
 		parameters_->adaptiveThreshConstant = config.adaptiveThreshConstant;
 		parameters_->adaptiveThreshWinSizeMin = config.adaptiveThreshWinSizeMin;
 		parameters_->adaptiveThreshWinSizeMax = config.adaptiveThreshWinSizeMax;

aruco_pose/src/aruco_map.cpp

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

aruco_pose/src/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>] [--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:
@@ -23,15 +23,20 @@ Options:
   <dist_x>       Distance between markers along X axis
   <dist_y>       Distance between markers along Y axis
   <first>        First marker ID [default: 0]
+  <x0>           X coordinate for the first marker [default: 0]
+  <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
 
 
@@ -39,20 +44,27 @@ arguments = docopt(__doc__)
 
 length = float(arguments['<length>'])
 first = int(arguments['<first>'] if arguments['<first>'] is not None else 0)
+x0 = float(arguments['<x0>'] if arguments['<x0>'] is not None else 0)
+y0 = float(arguments['<y0>'] if arguments['<y0>'] is not None else 0)
 markers_x = int(arguments['<x>'])
 markers_y = int(arguments['<y>'])
 dist_x = float(arguments['<dist_x>'])
 dist_y = float(arguments['<dist_y>'])
 bottom_left = arguments['--bottom-left']
 
-max_y = (markers_y - 1) * dist_y
+if arguments['-o'] is None:
+    output = sys.stdout
+else:
+    output = open(path.join(path.dirname(__file__), '..', 'map', arguments['-o']), 'w')
 
-print('# id\tlength\tx\ty\tz\trot_z\trot_y\trot_x')
+max_y = y0 + (markers_y - 1) * dist_y
+
+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 = x * dist_x
-        pos_y = y * dist_y
+        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{}\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/src/utils.h

@@ -35,9 +35,7 @@ static void parseCameraInfo(const sensor_msgs::CameraInfoConstPtr& cinfo, cv::Ma
 	for (unsigned int i = 0; i < 3; ++i)
 		for (unsigned int j = 0; j < 3; ++j)
 			matrix.at<double>(i, j) = cinfo->K[3 * i + j];
-
-	for (unsigned int k = 0; k < cinfo->D.size(); k++)
-		dist.at<double>(k) = cinfo->D[k];
+	dist = cv::Mat(cinfo->D, true);
 }
 
 inline void rotatePoint(cv::Point3f& p, cv::Point3f origin, float angle)
@@ -108,26 +106,25 @@ inline bool isFlipped(tf::Quaternion& q)
 	return (abs(pitch) > M_PI / 2) || (abs(roll) > M_PI / 2);
 }
 
-/* Set roll and pitch from "from" to "to", keeping yaw */
-inline void snapOrientation(geometry_msgs::Quaternion& to, const geometry_msgs::Quaternion& from, bool auto_flip = false)
+/* Apply a vertical to an orientation */
+inline void applyVertical(geometry_msgs::Quaternion& orientation, const geometry_msgs::Quaternion& vertical,
+                          bool flip_vertical = false, bool auto_flip = false) // editorconfig-checker-disable-line
 {
-	tf::Quaternion _from, _to;
-	tf::quaternionMsgToTF(from, _from);
-	tf::quaternionMsgToTF(to, _to);
+	tf::Quaternion _vertical, _orientation;
+	tf::quaternionMsgToTF(vertical, _vertical);
+	tf::quaternionMsgToTF(orientation, _orientation);
 
-	if (auto_flip) {
-		if (!isFlipped(_from)) {
-			static const tf::Quaternion flip = tf::createQuaternionFromRPY(M_PI, 0, 0);
-			_from *= flip; // flip "from"
-		}
+	if (flip_vertical || (auto_flip && !isFlipped(_orientation))) {
+		static const tf::Quaternion flip = tf::createQuaternionFromRPY(M_PI, 0, 0);
+		_vertical *= flip; // flip vertical
 	}
 
-	auto diff = tf::Matrix3x3(_to).transposeTimes(tf::Matrix3x3(_from));
+	auto diff = tf::Matrix3x3(_orientation).transposeTimes(tf::Matrix3x3(_vertical));
 	double _, yaw;
 	diff.getRPY(_, _, yaw);
 	auto q = tf::createQuaternionFromRPY(0, 0, -yaw);
-	_from = _from * q; // set yaw from "to" to "from"
-	tf::quaternionTFToMsg(_from, to); // set "from" to "to"
+	_vertical = _vertical * q; // set yaw from orientation to vertical
+	tf::quaternionTFToMsg(_vertical, orientation); // set vertical to orientation
 }
 
 inline void transformToPose(const geometry_msgs::Transform& transform, geometry_msgs::Pose& pose)

aruco_pose/srv/SetMarkers.srv

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

aruco_pose/test/basic.py

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

aruco_pose/test/crash_opencv.py

@@ -0,0 +1,18 @@
+import rospy
+import pytest
+
+from visualization_msgs.msg import MarkerArray as VisMarkerArray
+
+
+@pytest.fixture
+def node():
+    return rospy.init_node('aruco_pose_opencv_crash', anonymous=True)
+
+def test_opencv_crashes_img01(node):
+    rospy.wait_for_message('aruco_detect_01/visualization', VisMarkerArray, timeout=5)
+
+def test_opencv_crashes_img02(node):
+    rospy.wait_for_message('aruco_detect_02/visualization', VisMarkerArray, timeout=5)
+
+def test_opencv_crashes_img03(node):
+    rospy.wait_for_message('aruco_detect_03/visualization', VisMarkerArray, timeout=5)

aruco_pose/test/crash_opencv.test

@@ -0,0 +1,51 @@
+<launch>
+    <arg name="corner_method" default="2"/>
+
+    <node pkg="image_publisher" type="image_publisher" name="imgpub_01" args="$(find aruco_pose)/test/crash_image_01.png">
+        <param name="frame_id" value="main_camera_optical"/>
+        <param name="publish_rate" value="10"/>
+        <param name="camera_info_url" value="file://$(find aruco_pose)/test/camera_info.yaml" />
+    </node>
+
+    <node pkg="image_publisher" type="image_publisher" name="imgpub_02" args="$(find aruco_pose)/test/crash_image_02.png">
+        <param name="frame_id" value="main_camera_optical"/>
+        <param name="publish_rate" value="10"/>
+        <param name="camera_info_url" value="file://$(find aruco_pose)/test/camera_info.yaml" />
+    </node>
+
+    <node pkg="image_publisher" type="image_publisher" name="imgpub_03" args="$(find aruco_pose)/test/crash_image_03.png">
+        <param name="frame_id" value="main_camera_optical"/>
+        <param name="publish_rate" value="10"/>
+        <param name="camera_info_url" value="file://$(find aruco_pose)/test/camera_info.yaml" />
+    </node>
+
+    <node pkg="nodelet" type="nodelet" name="nodelet_manager_01" args="manager"/>
+
+    <node pkg="nodelet" clear_params="true" type="nodelet" name="aruco_detect_01" args="load aruco_pose/aruco_detect nodelet_manager_01">
+        <remap from="image_raw" to="imgpub_01/image_raw"/>
+        <remap from="camera_info" to="imgpub_01/camera_info"/>
+        <param name="length" value="0.33"/>
+        <param name="cornerRefinementMethod" value="$(arg corner_method)"/>
+    </node>
+
+    <node pkg="nodelet" type="nodelet" name="nodelet_manager_02" args="manager"/>
+
+    <node pkg="nodelet" clear_params="true" type="nodelet" name="aruco_detect_02" args="load aruco_pose/aruco_detect nodelet_manager_02">
+        <remap from="image_raw" to="imgpub_02/image_raw"/>
+        <remap from="camera_info" to="imgpub_02/camera_info"/>
+        <param name="length" value="0.33"/>
+        <param name="cornerRefinementMethod" value="$(arg corner_method)"/>
+    </node>
+
+    <node pkg="nodelet" type="nodelet" name="nodelet_manager_03" args="manager"/>
+
+    <node pkg="nodelet" clear_params="true" type="nodelet" name="aruco_detect_03" args="load aruco_pose/aruco_detect nodelet_manager_03">
+        <remap from="image_raw" to="imgpub_03/image_raw"/>
+        <remap from="camera_info" to="imgpub_03/camera_info"/>
+        <param name="length" value="0.33"/>
+        <param name="cornerRefinementMethod" value="$(arg corner_method)"/>
+    </node>
+
+    <param name="test_module" value="$(find aruco_pose)/test/crash_opencv.py"/>
+    <test test-name="crash_opencv" pkg="ros_pytest" type="ros_pytest_runner"/>
+</launch>

aruco_pose/test/duplicate.py

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

aruco_pose/test/duplicate.test

@@ -0,0 +1,21 @@
+<launch>
+    <node pkg="image_publisher" type="image_publisher" name="main_camera" args="$(find aruco_pose)/test/duplicate.png">
+        <param name="frame_id" value="main_camera_optical"/>
+        <param name="publish_rate" value="10"/>
+        <param name="camera_info_url" value="file://$(find aruco_pose)/test/camera_info.yaml" />
+    </node>
+
+    <node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager" required="true"/>
+
+    <node pkg="nodelet" clear_params="true" type="nodelet" name="aruco_detect" args="load aruco_pose/aruco_detect nodelet_manager" required="true">
+        <remap from="image_raw" to="main_camera/image_raw"/>
+        <remap from="camera_info" to="main_camera/camera_info"/>
+        <param name="length" value="0.33"/>
+        <param name="estimate_poses" value="true"/>
+        <param name="send_tf" value="true"/>
+        <param name="cornerRefinementMethod" value="1"/>
+    </node>
+
+	<param name="test_module" value="$(find aruco_pose)/test/duplicate.py"/>
+    <test test-name="aruco_pose_test" pkg="ros_pytest" type="ros_pytest_runner"/>
+</launch>

aruco_pose/test/test_node_failure.py

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

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;

aruco_pose/vendor/aruco/src/aruco.cpp

@@ -924,6 +924,8 @@ static void _refineCandidateLines(std::vector<Point>& nContours, std::vector<Poi
 	// calculate the line :: who passes through the grouped points
 	Point3f lines[4];
 	for(int i=0; i<4; i++){
+		// Don't try to "interpolate" single points
+		if (cntPts[i].size() < 2) return;
 		lines[i]=_interpolate2Dline(cntPts[i]);
 	}
 

book.json

@@ -9,7 +9,8 @@
         "richquotes@https://github.com/okalachev/gitbook-plugin-richquotes.git",
         "yametrika",
         "anchors",
-        "validate-links",
+        "collapsible-menu",
+        "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/avahi-services/sftp-ssh.service

@@ -0,0 +1,34 @@
+<?xml version="1.0" standalone='no'?><!--*-nxml-*-->
+<!DOCTYPE service-group SYSTEM "avahi-service.dtd">
+
+<!--
+  This file is part of avahi.
+
+  avahi is free software; you can redistribute it and/or modify it
+  under the terms of the GNU Lesser General Public License as
+  published by the Free Software Foundation; either version 2 of the
+  License, or (at your option) any later version.
+
+  avahi is distributed in the hope that it will be useful, but
+  WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+  General Public License for more details.
+
+  You should have received a copy of the GNU Lesser General Public
+  License along with avahi; if not, write to the Free Software
+  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA
+  02111-1307 USA.
+-->
+
+<!-- See avahi.service(5) for more information about this configuration file -->
+
+<service-group>
+
+  <name replace-wildcards="yes">%h</name>
+
+  <service>
+    <type>_sftp-ssh._tcp</type>
+    <port>22</port>
+  </service>
+
+</service-group>

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

@@ -5,8 +5,12 @@ Requires=roscore.service
 [Service]
 User=pi
 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 \
+                      ROS_HOSTNAME=`hostname`.local ROS_OS_OVERRIDE=debian:bookworm exec stdbuf -o L roslaunch clover clover.launch --wait --screen --skip-log-check \
                       2> >(tee /tmp/clover.err)"
 
+ExecStartPre=+rm /var/log/clover.log
+StandardOutput=file:/var/log/clover.log
+StandardError=file:/var/log/clover.log
+
 [Install]
 WantedBy=multi-user.target

builder/assets/hardware_setup.sh

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

builder/assets/init_rpi.sh

@@ -13,63 +13,31 @@
 # copies or substantial portions of the Software.
 #
 
-set -e # Exit immidiately on non-zero result
-
-echo_stamp() {
-  # TEMPLATE: echo_stamp <TEXT> <TYPE>
-  # TYPE: SUCCESS, ERROR, INFO
-
-  # More info there https://www.shellhacks.com/ru/bash-colors/
-
-  TEXT="$(date '+[%Y-%m-%d %H:%M:%S]') $1"
-  TEXT="\e[1m$TEXT\e[0m" # BOLD
-
-  case "$2" in
-    SUCCESS)
-    TEXT="\e[32m${TEXT}\e[0m";; # GREEN
-    ERROR)
-    TEXT="\e[31m${TEXT}\e[0m";; # RED
-    *)
-    TEXT="\e[34m${TEXT}\e[0m";; # BLUE
-  esac
-  echo -e ${TEXT}
-}
+echo "--- Fix home directory permissions"
+chmod +rx /home/pi
 
 NEW_SSID='clover-'$(head -c 100 /dev/urandom | xxd -ps -c 100 | sed -e "s/[^0-9]//g" | cut -c 1-4)
-echo_stamp "Setting SSID to ${NEW_SSID}"
-# TODO: Use wpa_cli insted direct file edit
-# FIXME: We rely on raspberrypi-net-mods to copy our file to /etc/wpa_supplicant.
-# This is not very reliable, but seems to fix our rfkill problem.
-cat << EOF >> /boot/wpa_supplicant.conf
-ctrl_interface=DIR=/var/run/wpa_supplicant GROUP=netdev
-update_config=1
-country=GB
-network={
-    ssid="${NEW_SSID}"
-    psk="cloverwifi"
-    mode=2
-    proto=WPA RSN
-    key_mgmt=WPA-PSK
-    pairwise=CCMP
-    group=CCMP
-    auth_alg=OPEN
-}
-EOF
+echo "--- Creating Wi-Fi AP with SSID=${NEW_SSID}"
+nmcli con add type wifi ifname wlan0 mode ap con-name clover ssid $NEW_SSID autoconnect true \
+&& nmcli con modify clover 802-11-wireless.band bg \
+&& nmcli con modify clover ipv4.method shared ipv4.address 192.168.11.1/24 \
+&& nmcli con modify clover ipv6.method disabled \
+&& nmcli con modify clover wifi-sec.key-mgmt wpa-psk \
+&& nmcli con modify clover wifi-sec.psk "cloverwifi" \
+&& systemctl disable dnsmasq # disable dnsmasq to avoid conflicts with NetworkManager's dnsmasq
 
 NEW_HOSTNAME=$(echo ${NEW_SSID} | tr '[:upper:]' '[:lower:]')
-echo_stamp "Setting hostname to $NEW_HOSTNAME"
-hostnamectl set-hostname $NEW_HOSTNAME
-sed -i 's/127\.0\.1\.1.*/127.0.1.1\t'${NEW_HOSTNAME}' '${NEW_HOSTNAME}'.local/g' /etc/hosts
+echo "--- Setting hostname to $NEW_HOSTNAME"
+hostnamectl set-hostname $NEW_HOSTNAME \
+&& sed -i 's/127\.0\.1\.1.*/127.0.1.1\t'${NEW_HOSTNAME}' '${NEW_HOSTNAME}'.local/g' /etc/hosts
 # .local (mdns) hostname added to make it accesable when wlan and ethernet interfaces are down
 
-echo_stamp "Enable ROS services"
+echo "--- Enable ROS services"
 systemctl enable roscore
 systemctl enable clover
 
-echo_stamp "Harware setup"
+echo "--- Harware setup"
 /root/hardware_setup.sh
 
-echo_stamp "Remove init scripts"
+echo "--- Remove init scripts"
 rm /root/init_rpi.sh /root/hardware_setup.sh
-
-echo_stamp "End of initialization of the image"

builder/assets/install_gitbook.sh

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

builder/assets/monkey

@@ -20,7 +20,7 @@
     # Example:
     #      DocumentRoot /home/krypton/htdocs
 
-    DocumentRoot /home/pi/catkin_ws/src/clover/clover/www
+    DocumentRoot /home/pi/.ros/www
 
     # Redirect:
     # ---------

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-ros.sh

@@ -0,0 +1,122 @@
+#! /usr/bin/env bash
+
+#
+# Script for building ROS packages from scratch
+#
+# Copyright (C) 2022 Copter Express Technologies
+#
+# Author: Oleg Kalachev <okalachev@gmail.com>
+#
+# Distributed under MIT License (available at https://opensource.org/licenses/MIT).
+# The above copyright notice and this permission notice shall be included in all
+# copies or substantial portions of the Software.
+#
+
+set -ex # exit on error, echo commands
+
+# http://wiki.ros.org/Installation/Source
+
+export ROS_DISTRO=noetic
+. /etc/os-release # set $VERSION_CODENAME to Debian release code name
+export ROS_OS_OVERRIDE=debian:$VERSION_CODENAME
+export ROS_PYTHON_VERSION=3
+export CLOVER_DEPS="tf tf2 tf2_ros tf2_geometry_msgs geometry_msgs sensor_msgs visualization_msgs libgeographiclib-dev mavros mavros_extras cv_camera cv_bridge rosbridge_server web_video_server tf2_web_republisher libxml2 libxslt python3-lxml dynamic_reconfigure image_transport image_proc image_geometry python-pymavlink ros_pytest"
+export CLOVER_DEPS="$CLOVER_DEPS rostest python3-docopt image_publisher"
+
+echo "=== Building ROS from scratch"
+
+#echo "--- Adding sources"
+# echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list
+# curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | sudo apt-key add -
+
+#cp /etc/apt/trusted.gpg /etc/apt/trusted.gpg.d # https://askubuntu.com/a/1408456
+apt-get update
+apt-get install -y python3-distutils build-essential cmake git python3-pip python3-rosinstall-generator python3-vcstools python3-empy libpoco-dev
+
+# install vcstool using pip
+# curl https://bootstrap.pypa.io/get-pip.py -o get-pip.py && python3 get-pip.py && rm get-pip.py
+pip3 install -U --break-system-packages vcstool rosdep rosinstall-generator catkin-pkg future
+
+# sudo rosdep init
+# rm /etc/ros/rosdep/sources.list.d/20-default.list
+rosdep init
+rosdep update --os=debian:bullseye
+
+# rm /etc/ros/rosdep/sources.list.d/20-default.list && rosdep init
+# rosdep --os=debian:$VERSION_CODENAME update
+
+echo "--- Create Catkin workspace to build ROS package"
+mkdir ~/ros_catkin_ws
+cd ~/ros_catkin_ws
+
+echo "--- Download ROS sources"
+rosinstall_generator ros_base $CLOVER_DEPS --rosdistro $ROS_DISTRO --deps --tar > noetic.rosinstall
+mkdir ./src
+vcs import --input noetic.rosinstall ./src
+
+# https://answers.ros.org/question/343367/catkin-package-dependencies-issue-when-installing-ros-melodic-on-raspberry-pi-4/
+#sudo apt remove python-rospkg
+#sudo apt remove python-catkin-pkg
+##sudo apt --fix-broken install
+#sudo apt-get autoremove
+
+#echo "--- Install catkin_pkg"
+#cd
+#git clone https://github.com/ros-infrastructure/catkin_pkg.git
+#cd catkin_pkg
+#python3 setup.py install
+#cd ~/ros_catkin_ws
+
+echo "--- Resolve dependencies"
+rosdep install --from-paths ./src --ignore-packages-from-source --rosdistro $ROS_DISTRO -y --os=debian:bullseye --skip-keys="python3-catkin-pkg-modules libboost-thread python3-rosdep-modules" || true
+
+echo "--- Install missing dependencies"
+apt-get install -y liborocos-kdl1.5 geographiclib-tools libgeographiclib-dev
+
+echo "-- Install geographiclib datasets"
+wget https://raw.githubusercontent.com/mavlink/mavros/master/mavros/scripts/install_geographiclib_datasets.sh
+chmod +x install_geographiclib_datasets.sh
+./install_geographiclib_datasets.sh
+
+echo "--- Apply patches"
+wget https://github.com/ros/rosconsole/pull/58.patch
+patch -p1 -d src/rosconsole < 58.patch
+
+wget https://github.com/ros/ros_comm/pull/2353.patch
+patch -p2 -d src/ros_comm < 2353.patch
+
+wget https://github.com/AJahueyM/web_video_server/commit/5b722eb0822bcc3fe45fefe7b393b87bfe004417.patch
+patch -p1 -d src/web_video_server < 5b722eb0822bcc3fe45fefe7b393b87bfe004417.patch
+
+echo "--- Build ROS"
+# https://github.com/ros/catkin/issues/863#issuecomment-290392074
+./src/catkin/bin/catkin_make_isolated --install -DCMAKE_BUILD_TYPE=Release
+#	-DSETUPTOOLS_DEB_LAYOUT=OFF \
+#	--install-space=/opt/ros/$ROS_DISTRO
+
+# source ~/ros_catkin_ws/install_isolated/setup.bash
+#source /opt/ros/$ROS_DISTRO/setup.bash
+#
+#echo "--- List built ROS packages"
+#set +x
+#rospack list-names | while read line; do echo $line `rosversion $line`; done
+#set -x
+#
+#echo "--- Build Debian packages"
+#apt-get install -y python3-bloom debhelper dpkg-dev libtinyxml-dev
+#
+## add rosdep file to help bloom-generate resolve missing bookworm dependencies
+#echo "yaml file:///etc/ros/rosdep/noetic-rosdep-clover.yaml" >> /etc/ros/rosdep/sources.list.d/20-default.list
+#rosdep update
+#
+#pip3 install setuptools==45.2.0 # https://github.com/ros/catkin/issues/863#issuecomment-1000446018
+#
+#for file in `find . -name "package.xml" -not -path "*/debian/*"`; do
+#	cd $(dirname ${file})
+#	rm -rf debian
+#	bloom-generate rosdebian --os-name debian --os-version $VERSION_CODENAME --ros-distro $ROS_DISTRO --debug
+#	debian/rules binary # fakeroot is not needed as we are root
+#	cd -
+#done
+
+ls

builder/image-build.sh

@@ -13,41 +13,22 @@
 # copies or substantial portions of the Software.
 #
 
-set -e # Exit immidiately on non-zero result
+set -ex # exit on error, echo commands
 
-SOURCE_IMAGE="https://downloads.raspberrypi.org/raspbian_lite/images/raspbian_lite-2020-02-14/2020-02-13-raspbian-buster-lite.zip"
+# https://www.raspberrypi.org/software/operating-systems/#raspberry-pi-os-32-bit
+SOURCE_IMAGE="https://downloads.raspberrypi.com/raspios_lite_armhf/images/raspios_lite_armhf-2024-03-15/2024-03-15-raspios-bookworm-armhf-lite.img.xz"
 
 export DEBIAN_FRONTEND=${DEBIAN_FRONTEND:='noninteractive'}
 export LANG=${LANG:='C.UTF-8'}
 export LC_ALL=${LC_ALL:='C.UTF-8'}
 
-echo_stamp() {
-  # TEMPLATE: echo_stamp <TEXT> <TYPE>
-  # TYPE: SUCCESS, ERROR, INFO
-
-  # More info there https://www.shellhacks.com/ru/bash-colors/
-
-  TEXT="$(date '+[%Y-%m-%d %H:%M:%S]') $1"
-  TEXT="\e[1m$TEXT\e[0m" # BOLD
-
-  case "$2" in
-    SUCCESS)
-    TEXT="\e[32m${TEXT}\e[0m";; # GREEN
-    ERROR)
-    TEXT="\e[31m${TEXT}\e[0m";; # RED
-    *)
-    TEXT="\e[34m${TEXT}\e[0m";; # BLUE
-  esac
-  echo -e ${TEXT}
-}
-
 BUILDER_DIR="/builder"
 REPO_DIR="${BUILDER_DIR}/repo"
 SCRIPTS_DIR="${REPO_DIR}/builder"
 IMAGES_DIR="${REPO_DIR}/images"
 
-[[ ! -d ${SCRIPTS_DIR} ]] && (echo_stamp "Directory ${SCRIPTS_DIR} doesn't exist" "ERROR"; exit 1)
-[[ ! -d ${IMAGES_DIR} ]] && mkdir ${IMAGES_DIR} && echo_stamp "Directory ${IMAGES_DIR} was created successful" "SUCCESS"
+[[ ! -d ${SCRIPTS_DIR} ]] && (echo "Error: directory ${SCRIPTS_DIR} doesn't exist"; exit 1)
+[[ ! -d ${IMAGES_DIR} ]] && mkdir ${IMAGES_DIR} && echo "Directory ${IMAGES_DIR} was created successful"
 
 if [[ -z ${TRAVIS_TAG} ]]; then IMAGE_VERSION="$(cd ${REPO_DIR}; git log --format=%h -1)"; else IMAGE_VERSION="${TRAVIS_TAG}"; fi
 # IMAGE_VERSION="${TRAVIS_TAG:=$(cd ${REPO_DIR}; git log --format=%h -1)}"
@@ -63,15 +44,17 @@ get_image() {
   local RPI_IMAGE_NAME=$(echo ${RPI_ZIP_NAME} | sed 's/zip/img/')
 
   if [ ! -e "${BUILD_DIR}/${RPI_ZIP_NAME}" ]; then
-    echo_stamp "Downloading original Linux distribution"
+    echo "--- Downloading original Linux distribution"
     wget --progress=dot:giga -O ${BUILD_DIR}/${RPI_ZIP_NAME} $2
-    echo_stamp "Downloading complete" "SUCCESS" \
-  else echo_stamp "Linux distribution already donwloaded"; fi
+    echo "--- Downloading complete" "SUCCESS"
+  else
+    echo "Linux distribution already downloaded"
+  fi
 
-  echo_stamp "Unzipping Linux distribution image" \
-  && unzip -p ${BUILD_DIR}/${RPI_ZIP_NAME} ${RPI_IMAGE_NAME} > $1 \
-  && echo_stamp "Unzipping complete" "SUCCESS" \
-  || (echo_stamp "Unzipping was failed!" "ERROR"; exit 1)
+  echo "--- Unzipping Linux distribution image"
+  apt-get update --allow-releaseinfo-change
+  apt-get install -y xz-utils
+  unxz --stdout ${BUILD_DIR}/${RPI_ZIP_NAME} > $1
 }
 
 get_image ${IMAGE_PATH} ${SOURCE_IMAGE}
@@ -104,25 +87,22 @@ ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/butterf
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/monkey.service' '/lib/systemd/system/'
 # software install
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} exec ${SCRIPTS_DIR}'/image-software.sh'
-# examples
-${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/examples' '/home/pi/'
 # network setup
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} exec ${SCRIPTS_DIR}'/image-network.sh'
+# avahi setup
+${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/avahi-services/sftp-ssh.service' '/etc/avahi/services'
 
 # 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-build-ros.sh'
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} exec ${SCRIPTS_DIR}'/image-validate.sh'
 
 ${BUILDER_DIR}/image-resize.sh ${IMAGE_PATH}

builder/image-init.sh

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

builder/image-network.sh

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

builder/image-ros.sh

@@ -13,43 +13,22 @@
 # copies or substantial portions of the Software.
 #
 
-set -e # Exit immidiately on non-zero result
+set -ex # exit on error, echo commands
 
-REPO=$1
-REF=$2
-INSTALL_ROS_PACK_SOURCES=$3
-DISCOVER_ROS_PACK=$4
-NUMBER_THREADS=$5
-
-echo_stamp() {
-  # TEMPLATE: echo_stamp <TEXT> <TYPE>
-  # TYPE: SUCCESS, ERROR, INFO
-
-  # More info there https://www.shellhacks.com/ru/bash-colors/
-
-  TEXT="$(date '+[%Y-%m-%d %H:%M:%S]') $1"
-  TEXT="\e[1m$TEXT\e[0m" # BOLD
-
-  case "$2" in
-    SUCCESS)
-    TEXT="\e[32m${TEXT}\e[0m";; # GREEN
-    ERROR)
-    TEXT="\e[31m${TEXT}\e[0m";; # RED
-    *)
-    TEXT="\e[34m${TEXT}\e[0m";; # BLUE
-  esac
-  echo -e ${TEXT}
-}
+# Current ROS distribution
+ROS_DISTRO=noetic
+. /etc/os-release # set $VERSION_CODENAME to Debian release code name
+export ROS_OS_OVERRIDE=debian:$VERSION_CODENAME
 
 # https://gist.github.com/letmaik/caa0f6cc4375cbfcc1ff26bd4530c2a3
 # https://github.com/travis-ci/travis-build/blob/master/lib/travis/build/templates/header.sh
 my_travis_retry() {
   local result=0
   local count=1
-  local max_count=50
+  local max_count=5
   while [ $count -le $max_count ]; do
     [ $result -ne 0 ] && {
-      echo -e "\nThe command \"$@\" failed. Retrying, $count of $max_count.\n" >&2
+      echo -e "\nThe command \"$*\" failed. Retrying, $count of $max_count.\n" >&2
     }
     # ! { } ignores set -e, see https://stackoverflow.com/a/4073372
     ! { "$@"; result=$?; }
@@ -59,102 +38,136 @@ my_travis_retry() {
   done
 
   [ $count -gt $max_count ] && {
-    echo -e "\nThe command \"$@\" failed $max_count times.\n" >&2
+    echo -e "\nThe command \"$*\" failed $max_count times.\n" >&2
   }
 
   return $result
 }
 
+echo "--- Install rosdep"
+my_travis_retry pip3 install -U rosdep
+
 # TODO: 'kinetic-rosdep-clover.yaml' should add only if we use our repo?
-echo_stamp "Init rosdep"
+echo "--- Init rosdep"
 my_travis_retry rosdep init
-echo "yaml file:///etc/ros/rosdep/melodic-rosdep-clover.yaml" >> /etc/ros/rosdep/sources.list.d/20-default.list
+
+echo "--- Update rosdep"
+echo "yaml file:///etc/ros/rosdep/${ROS_DISTRO}-rosdep-clover.yaml" >> /etc/ros/rosdep/sources.list.d/10-clover.list
 my_travis_retry rosdep update
 
-echo_stamp "Populate rosdep for ROS user"
-my_travis_retry sudo -u pi rosdep update
+echo "--- Populate rosdep for ROS user"
+my_travis_retry sudo -u pi ROS_OS_OVERRIDE=debian:$VERSION_CODENAME rosdep update
 
-resolve_rosdep() {
-  # TEMPLATE: resolve_rosdep <CATKIN_PATH> <ROS_DISTRO> <OS_DISTRO> <OS_VERSION>
-  CATKIN_PATH=$1
-  ROS_DISTRO='melodic'
-  OS_DISTRO='debian'
-  OS_VERSION='buster'
-
-  echo_stamp "Installing dependencies apps with rosdep in ${CATKIN_PATH}"
-  cd ${CATKIN_PATH}
-  my_travis_retry rosdep install -y --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} --os=${OS_DISTRO}:${OS_VERSION}
-}
-
-export ROS_IP='127.0.0.1' # needed for running tests
-
-echo_stamp "Reconfiguring Clover repository for simplier unshallowing"
+# echo "Reconfiguring Clover repository for simplier unshallowing"
 cd /home/pi/catkin_ws/src/clover
 git config remote.origin.fetch "+refs/heads/*:refs/remotes/origin/*"
 
-echo_stamp "Build and install Clover"
-cd /home/pi/catkin_ws
-resolve_rosdep $(pwd)
-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
-catkin_make -j2 -DCMAKE_BUILD_TYPE=Release
+# 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 "--- 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 "Install clever package (for backwards compatibility)"
+#echo "--- Installing libboost-dev" # https://travis-ci.org/github/CopterExpress/clover/jobs/766318908#L6536
+#my_travis_retry apt-get install -y --no-install-recommends libboost-dev libboost-all-dev
+
+echo "--- Build and install Clover"
+cd /home/pi/catkin_ws
+# Don't try to install gazebo_ros
+my_travis_retry rosdep install -y --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} --os=debian:$VERSION_CODENAME \
+  --skip-keys="gazebo_ros gazebo_plugins"
+my_travis_retry pip3 install wheel
+my_travis_retry pip3 install -r /home/pi/catkin_ws/src/clover/clover/requirements.txt
+source /opt/ros/${ROS_DISTRO}/setup.bash
+catkin_make -j2 -DCMAKE_BUILD_TYPE=RelWithDebInfo
+source devel/setup.bash
+
+echo "--- Install clever package (for backwards compatibility)"
 cd /home/pi/catkin_ws/src/clover/builder/assets/clever
 ./setup.py install
 rm -rf build  # remove build artifacts
 
-echo_stamp "Build Clover documentation"
+echo "--- Build Clover documentation"
 cd /home/pi/catkin_ws/src/clover
-NPM_CONFIG_UNSAFE_PERM=true npm install gitbook-cli -g
-NPM_CONFIG_UNSAFE_PERM=true gitbook install
+builder/assets/install_gitbook.sh
+gitbook install
 gitbook build
+# replace assets copy to assets symlink to save space
+rm -rf _book/assets && ln -s ../docs/assets _book/assets
 touch node_modules/CATKIN_IGNORE docs/CATKIN_IGNORE _book/CATKIN_IGNORE clover/www/CATKIN_IGNORE apps/CATKIN_IGNORE # ignore documentation files by catkin
+npm cache clean --force
 
-echo_stamp "Installing additional ROS packages"
-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
+echo "--- Installing additional ROS packages"
+my_travis_retry apt-get install -y --no-install-recommends \
+    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}-libcamera-ros \
+    ros-${ROS_DISTRO}-rosshow \
+    ros-${ROS_DISTRO}-cmake-modules \
+    ros-${ROS_DISTRO}-image-view \
+    ros-${ROS_DISTRO}-nodelet-topic-tools \
+    ros-${ROS_DISTRO}-stereo-msgs \
+    ros-${ROS_DISTRO}-vision-msgs
 
 # TODO move GeographicLib datasets to Mavros debian package
-echo_stamp "Install GeographicLib datasets (needed for mavros)" \
+echo "--- Install GeographicLib datasets (needed for mavros)" \
 && wget -qO- https://raw.githubusercontent.com/mavlink/mavros/master/mavros/scripts/install_geographiclib_datasets.sh | bash
 
-# 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"
+echo "--- Running tests"
+export ROS_IP='127.0.0.1' # needed for running tests
 cd /home/pi/catkin_ws
 # FIXME: Investigate failing tests
 catkin_make run_tests #&& catkin_test_results
 
-echo_stamp "Change permissions for catkin_ws"
+echo "--- Change permissions for catkin_ws"
 chown -Rf pi:pi /home/pi/catkin_ws
 
-echo_stamp "Setup ROS environment"
+echo "--- Update www"
+sudo -u pi sh -c ". devel/setup.sh && rosrun clover www"
+
+echo "--- Make \$HOME/examples symlink"
+ln -s "$(catkin_find clover examples --first-only)" /home/pi
+chown -Rf pi:pi /home/pi/examples
+
+echo "--- Make systemd services symlinks"
+ln -s /home/pi/catkin_ws/src/clover/builder/assets/clover.service /lib/systemd/system/
+ln -s /home/pi/catkin_ws/src/clover/builder/assets/roscore.service /lib/systemd/system/
+# validate
+[ -f /lib/systemd/system/clover.service ]
+[ -f /lib/systemd/system/roscore.service ]
+
+echo "--- Make udev rules symlink"
+ln -s "$(catkin_find clover udev --first-only)"/* /lib/udev/rules.d/
+
+echo "--- Setup ROS environment"
 cat << EOF >> /home/pi/.bashrc
 LANG='C.UTF-8'
 LC_ALL='C.UTF-8'
-ROS_DISTRO='melodic'
 export ROS_HOSTNAME=\`hostname\`.local
-source /opt/ros/melodic/setup.bash
+export ROS_OS_OVERRIDE=debian:bookworm
+source /opt/ros/${ROS_DISTRO}/setup.bash
 source /home/pi/catkin_ws/devel/setup.bash
 EOF
 
-#echo_stamp "Removing local apt mirror"
-# Restore original sources.list
-#mv /var/sources.list.bak /etc/apt/sources.list
-# Clean apt cache
-apt-get clean -qq > /dev/null
-# Remove local mirror repository key
-#apt-key del COEX-MIRROR
+echo "--- Cleanup apt"
+apt-get autoremove --purge -y
+apt-get clean
 
-echo_stamp "END of ROS INSTALLATION"
+echo "--- Cleanup pip"
+pip3 cache purge
+
+echo "--- Cleanup /tmp"
+rm -rf /tmp/*

builder/image-software.sh

@@ -12,27 +12,9 @@
 # copies or substantial portions of the Software.
 #
 
-set -e # Exit immidiately on non-zero result
+set -ex # exit on error, echo commands
 
-echo_stamp() {
-  # TEMPLATE: echo_stamp <TEXT> <TYPE>
-  # TYPE: SUCCESS, ERROR, INFO
-
-  # More info there https://www.shellhacks.com/ru/bash-colors/
-
-  TEXT="$(date '+[%Y-%m-%d %H:%M:%S]') $1"
-  TEXT="\e[1m${TEXT}\e[0m" # BOLD
-
-  case "$2" in
-    SUCCESS)
-    TEXT="\e[32m${TEXT}\e[0m";; # GREEN
-    ERROR)
-    TEXT="\e[31m${TEXT}\e[0m";; # RED
-    *)
-    TEXT="\e[34m${TEXT}\e[0m";; # BLUE
-  esac
-  echo -e ${TEXT}
-}
+. /etc/os-release # set $VERSION_CODENAME to Debian release code name
 
 # https://gist.github.com/letmaik/caa0f6cc4375cbfcc1ff26bd4530c2a3
 # https://github.com/travis-ci/travis-build/blob/master/lib/travis/build/templates/header.sh
@@ -41,7 +23,7 @@ my_travis_retry() {
   local count=1
   while [ $count -le 3 ]; do
     [ $result -ne 0 ] && {
-      echo -e "\n${ANSI_RED}The command \"$@\" failed. Retrying, $count of 3.${ANSI_RESET}\n" >&2
+      echo -e "\n${ANSI_RED}The command \"$*\" failed. Retrying, $count of 3.${ANSI_RESET}\n" >&2
     }
     # ! { } ignores set -e, see https://stackoverflow.com/a/4073372
     ! { "$@"; result=$?; }
@@ -51,36 +33,39 @@ my_travis_retry() {
   done
 
   [ $count -gt 3 ] && {
-    echo -e "\n${ANSI_RED}The command \"$@\" failed 3 times.${ANSI_RESET}\n" >&2
+    echo -e "\n${ANSI_RED}The command \"$*\" failed 3 times.${ANSI_RESET}\n" >&2
   }
 
   return $result
 }
 
-echo_stamp "Install apt keys & repos"
+echo "--- Increase apt retries"
+echo "APT::Acquire::Retries \"3\";" > /etc/apt/apt.conf.d/80-retries
+
+echo "--- 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
+# echo "deb http://packages.ros.org/ros/ubuntu buster main" > /etc/apt/sources.list.d/ros-latest.list
 
-echo_stamp "Update apt cache"
+wget -O - 'http://packages.coex.tech/key.asc' | apt-key add -
+echo "deb http://packages.coex.tech $VERSION_CODENAME main" >> /etc/apt/sources.list
+
+echo "--- Update apt cache"
 
 # TODO: FIX ERROR: /usr/bin/apt-key: 596: /usr/bin/apt-key: cannot create /dev/null: Permission denied
 apt-get update
 # && apt upgrade -y
 
-echo_stamp "Software installing"
-apt-get install --no-install-recommends -y \
+# Let's retry fetching those packages several times, just in case
+echo "--- Install software"
+my_travis_retry apt-get install --no-install-recommends -y cmake-data cmake
+my_travis_retry apt-get install --no-install-recommends -y \
 unzip \
 zip \
-ipython \
 ipython3 \
 screen \
 byobu  \
@@ -89,93 +74,84 @@ lsof \
 git \
 dnsmasq  \
 tmux \
+tree \
 vim \
-cmake \
-libjpeg8 \
 tcpdump \
-ltrace \
 libpoco-dev \
 libzbar0 \
-python-rosdep \
-python-rosinstall-generator \
-python-wstool \
-python-rosinstall \
+python3-rosinstall-generator \
+python3-wstool \
+python3-rosinstall \
 build-essential \
 libffi-dev \
 monkey \
-pigpio python-pigpio python3-pigpio \
+pigpio python3-pigpio \
 i2c-tools \
-espeak espeak-data python-espeak \
+espeak espeak-data python3-espeak \
 ntpdate \
-python-dev \
-python3-dev \
-python-systemd \
 mjpg-streamer \
+xxd \
+python3-dev \
+python3-systemd \
 python3-opencv \
-&& echo_stamp "Everything was installed!" "SUCCESS" \
-|| (echo_stamp "Some packages wasn't installed!" "ERROR"; exit 1)
+python3-pip
+#libjpeg8 \
 
 # Deny byobu to check available updates
 sed -i "s/updates_available//" /usr/share/byobu/status/status
 # sed -i "s/updates_available//" /home/pi/.byobu/status
 
-echo_stamp "Installing pip"
-curl https://bootstrap.pypa.io/get-pip.py -o get-pip.py
-python3 get-pip.py
-python get-pip.py
-rm get-pip.py
-#my_travis_retry pip install --upgrade pip
-#my_travis_retry pip3 install --upgrade pip
-
-echo_stamp "Make sure both pip and pip3 are installed"
+echo "--- Make sure pip is installed"
 pip --version
 pip3 --version
 
-echo_stamp "Install and enable Butterfly (web terminal)"
-echo_stamp "Workaround for tornado >= 6.0 breaking butterfly"
-my_travis_retry pip3 install tornado==5.1.1
+echo "--- Enable installing packages with pip"
+mv /usr/lib/python3.11/EXTERNALLY-MANAGED /usr/lib/python3.11/EXTERNALLY-MANAGED.old
+
+echo "--- Install and enable Butterfly (web terminal)"
+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
+echo "--- Install ws281x library"
+my_travis_retry pip3 install --prefer-binary rpi_ws281x
 
-echo_stamp "Setup Monkey"
+echo "--- Setup Monkey"
 mv /etc/monkey/sites/default /etc/monkey/sites/default.orig
 mv /root/monkey /etc/monkey/sites/default
+sed -i 's/SymLink Off/SymLink On/' /etc/monkey/monkey.conf
 systemctl enable monkey.service
 
-echo_stamp "Install Node.js"
+echo "--- Install Node.js"
 cd /home/pi
-wget 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/
 rm node-v10.15.0-linux-armv6l.tar.gz
 
-echo_stamp "Installing ptvsd"
-my_travis_retry pip install ptvsd
-my_travis_retry pip3 install ptvsd
+echo "--- Installing debugpy"
+my_travis_retry pip3 install debugpy
 
-echo_stamp "Installing pyzbar"
-my_travis_retry pip install pyzbar
+echo "--- Installing pyzbar"
 my_travis_retry pip3 install pyzbar
 
-echo_stamp "Add .vimrc"
+echo "--- Add .vimrc"
 cat << EOF > /home/pi/.vimrc
 set mouse-=a
 syntax on
 autocmd BufNewFile,BufRead *.launch set syntax=xml
 EOF
 
-echo_stamp "Change default keyboard layout to US"
+echo "--- Change default keyboard layout to US"
 sed -i 's/XKBLAYOUT="gb"/XKBLAYOUT="us"/g' /etc/default/keyboard
 
-echo_stamp "Attempting to kill dirmngr"
+echo "--- Attempting to kill dirmngr"
 gpgconf --kill dirmngr
 # dirmngr is only used by apt-key, so we can safely kill it.
 # We ignore pkill's exit value as well.
 pkill -9 -f dirmngr || true
-
-echo_stamp "End of software installation"

builder/image-validate.sh

@@ -12,20 +12,33 @@
 # copies or substantial portions of the Software.
 #
 
-set -ex
+set -ex # exit on error, echo commands
 
-echo "Run image tests"
 
-export ROS_DISTRO='melodic'
+echo "--- Run image tests"
+
+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
 
 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
 
+# check documented packages available
+apt-cache show gst-rtsp-launch
+apt-cache show openvpn
+
+echo "Largest packages installed"
+sudo -E sh -c 'apt-get install -y debian-goodies'
+dpigs -H -n 100
+
+echo "Cleanup apt"
+apt-get autoremove --purge -y
+apt-get clean
+
 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

@@ -1,27 +1,47 @@
 #!/bin/bash
 
 # 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/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,35 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
 
 # validate all required modules installed
 
+import os
 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
+from vision_msgs.msg import BoundingBox2D, BoundingBox2DArray, BoundingBox3D, BoundingBox3DArray, \
+    Classification2D, Classification3D, Detection2D, Detection2DArray, Detection3D, Detection3DArray, \
+    ObjectHypothesis, ObjectHypothesisWithPose, VisionInfo
+
+from clover import long_callback
+
+import dynamic_reconfigure.client
 
 import tf2_ros
 import tf2_geometry_msgs
@@ -23,9 +37,15 @@ import tf2_geometry_msgs
 import VL53L1X
 import pymavlink
 from pymavlink import mavutil
-import rpi_ws281x
-import pigpio
-from espeak import espeak
+from image_geometry import PinholeCameraModel, StereoCameraModel
+# from espeak import espeak
 from pyzbar import pyzbar
+import docopt
+import geopy
+import flask
 
-print cv2.getBuildInformation()
+print(cv2.getBuildInformation())
+
+if not os.environ.get('VM'):
+    import rpi_ws281x
+    import pigpio

builder/test/tests.sh

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

builder/test/tests_clever.py

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

builder/test/tests_py3.py

@@ -1,8 +0,0 @@
-#!/usr/bin/env python3
-
-# Make sure our Python 3 software is installed
-
-import cv2
-from pyzbar import pyzbar
-
-print(cv2.getBuildInformation())

check_assets_size.py

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

check_unused_assets.py

@@ -4,7 +4,9 @@ import os
 import sys
 import subprocess
 
-EXCLUDE = ('clever4-front-white.png', '.DS_Store', 'clever4-front-black-large.png')
+EXCLUDE = ('clever4-front-white.png', 'clever4-front-white-large.png', '.DS_Store',
+    'clever4-front-black-large.png','clover42-black.png', 'clover42-main-margin.png')
+
 code = 0
 
 os.chdir('./docs')

clover/CHANGELOG.rst

@@ -0,0 +1,8 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package clover
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+0.21.1 (2020-11-17)
+-------------------
+* First release of clover package to ROS
+* Contributors: Alexey Rogachevskiy, Arthur Golubtsov, Oleg Kalachev

clover/CMakeLists.txt

@@ -1,4 +1,4 @@
-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
@@ -17,6 +17,7 @@ find_package(catkin REQUIRED COMPONENTS
   message_generation
   geometry_msgs
   sensor_msgs
+  led_msgs
   geographic_msgs
   tf
   tf2
@@ -24,13 +25,24 @@ find_package(catkin REQUIRED COMPONENTS
   tf2_ros
   image_transport
   cv_bridge
+  dynamic_reconfigure
 )
 
 list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/cmake")
 
+# https://github.com/mavlink/mavros/blob/7f1a8/mavros/CMakeLists.txt#L42
+set(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH};/usr/share/cmake/geographiclib")
 find_package(GeographicLib REQUIRED)
 
-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
@@ -43,7 +55,7 @@ find_package(OpenCV 3 REQUIRED
 ## Uncomment this if the package has a setup.py. This macro ensures
 ## modules and global scripts declared therein get installed
 ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
-# catkin_python_setup()
+catkin_python_setup()
 
 ################################################
 ## Declare ROS messages, services and actions ##
@@ -70,11 +82,10 @@ find_package(OpenCV 3 REQUIRED
 ##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
 
 ## Generate messages in the 'msg' folder
-# add_message_files(
-#   FILES
-#   Message1.msg
-#   Message2.msg
-# )
+add_message_files(
+  FILES
+  State.msg
+)
 
 ## Generate services in the 'srv' folder
 add_service_files(
@@ -82,6 +93,9 @@ add_service_files(
   GetTelemetry.srv
   Navigate.srv
   NavigateGlobal.srv
+  SetAltitude.srv
+  SetYaw.srv
+  SetYawRate.srv
   SetPosition.srv
   SetVelocity.srv
   SetAttitude.srv
@@ -118,10 +132,9 @@ generate_messages(
 ##     and list every .cfg file to be processed
 
 ## Generate dynamic reconfigure parameters in the 'cfg' folder
-# generate_dynamic_reconfigure_options(
-#   cfg/DynReconf1.cfg
-#   cfg/DynReconf2.cfg
-# )
+generate_dynamic_reconfigure_options(
+  cfg/Flow.cfg
+)
 
 ###################################
 ## catkin specific configuration ##
@@ -166,13 +179,14 @@ add_library(${PROJECT_NAME}
 ## The recommended prefix ensures that target names across packages don't collide
 add_executable(simple_offboard src/simple_offboard.cpp)
 
-add_executable(rc src/rc.cpp)
+# PX4 already has rc and led targets, so we prefix ours with clover_
+add_executable(clover_rc src/rc.cpp)
 
 add_executable(camera_markers src/camera_markers.cpp)
 
 add_executable(vpe_publisher src/vpe_publisher.cpp)
 
-add_executable(led src/led.cpp)
+add_executable(clover_led src/led.cpp)
 
 add_executable(shell src/shell.cpp)
 
@@ -181,22 +195,29 @@ target_link_libraries(simple_offboard
   ${GeographicLib_LIBRARIES}
 )
 
-target_link_libraries(rc ${catkin_LIBRARIES})
+# Don't change actual binary names
+set_target_properties(clover_rc PROPERTIES OUTPUT_NAME rc)
+
+set_target_properties(clover_led PROPERTIES OUTPUT_NAME led)
+
+target_link_libraries(clover_rc ${catkin_LIBRARIES})
 
 target_link_libraries(camera_markers ${catkin_LIBRARIES})
 
 target_link_libraries(vpe_publisher ${catkin_LIBRARIES})
 
-target_link_libraries(led ${catkin_LIBRARIES})
+target_link_libraries(clover_led ${catkin_LIBRARIES})
 
 target_link_libraries(shell ${catkin_LIBRARIES})
 
 add_dependencies(simple_offboard ${PROJECT_NAME}_generate_messages_cpp)
 
-add_dependencies(led ${PROJECT_NAME}_generate_messages_cpp)
+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
@@ -227,12 +248,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}/
@@ -248,13 +269,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()
@@ -277,4 +310,5 @@ endif()
 if (CATKIN_ENABLE_TESTING)
   find_package(rostest REQUIRED)
   add_rostest(test/basic.test)
+  add_rostest(test/offboard.test)
 endif()

clover/README.md

@@ -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
 ```
 
@@ -44,30 +44,12 @@ Alternatively you may change the `fcu_url` property in `mavros.launch` file to p
 
 ## Running
 
-Enable systemd service `roscore` (if not running):
-
-```bash
-sudo systemctl enable /home/<username>/catkin_ws/src/clover/builder/assets/roscore.service
-sudo systemctl start roscore
-```
-
-To start connection to SITL, use:
-
-```bash
-roslaunch clover sitl.launch
-```
-
 To start connection to the flight controller, use:
 
 ```bash
 roslaunch clover clover.launch
 ```
 
+For the simulation information see the [corresponding article](https://clover.coex.tech/en/simulation.html).
+
 > Note that the package is configured to connect to `/dev/px4fmu` by default (see [previous section](#manual-installation)). Install udev rules or specify path to your FCU device in `mavros.launch`.
-
-Also, you can enable and start the systemd service:
-
-```bash
-sudo systemctl enable /home/<username>/catkin_ws/src/clover/deploy/clover.service
-sudo systemctl start clover
-```

clover/camera_info/fisheye_cam_1280x720.yaml

@@ -0,0 +1,65 @@
+# Generated from fisheye_cam.yaml by rescale_camera_info.py
+camera_matrix:
+  cols: 3
+  data:
+  - 415.5985593268293
+  - 0.0
+  - 400.0
+  - 0.0
+  - 312.35267324512984
+  - 225.0
+  - 0.0
+  - 0.0
+  - 1.0
+  rows: 3
+camera_name: main_camera_optical
+distortion_coefficients:
+  cols: 8
+  data:
+  - 0.215356885
+  - -0.117472846
+  - -0.000306197672
+  - -0.000109444025
+  - -0.00453657258
+  - 0.573090623
+  - -0.127574577
+  - -0.0286125589
+  - 0.0
+  - 0.0
+  - 0.0
+  - 0.0
+  - 0.0
+  - 0.0
+  rows: 1
+distortion_model: plumb_bob
+image_height: 720
+image_width: 1280
+projection_matrix:
+  cols: 4
+  data:
+  - 415.5985593268293
+  - 0.0
+  - 405.4752811707317
+  - 0.0
+  - 0.0
+  - 312.35267324512984
+  - 205.91677004464282
+  - 0.0
+  - 0.0
+  - 0.0
+  - 1.0
+  - 0.0
+  rows: 3
+rectification_matrix:
+  cols: 3
+  data:
+  - 1
+  - 0
+  - 0
+  - 0
+  - 1
+  - 0
+  - 0
+  - 0
+  - 1
+  rows: 3

clover/camera_info/fisheye_cam_1920x1080.yaml

@@ -0,0 +1,65 @@
+# Generated from fisheye_cam.yaml by rescale_camera_info.py
+camera_matrix:
+  cols: 3
+  data:
+  - 1246.7956779804879
+  - 0.0
+  - 1200.0
+  - 0.0
+  - 702.7935148015422
+  - 506.25
+  - 0.0
+  - 0.0
+  - 1.0
+  rows: 3
+camera_name: main_camera_optical
+distortion_coefficients:
+  cols: 8
+  data:
+  - 0.215356885
+  - -0.117472846
+  - -0.000306197672
+  - -0.000109444025
+  - -0.00453657258
+  - 0.573090623
+  - -0.127574577
+  - -0.0286125589
+  - 0.0
+  - 0.0
+  - 0.0
+  - 0.0
+  - 0.0
+  - 0.0
+  rows: 1
+distortion_model: plumb_bob
+image_height: 1080
+image_width: 1920
+projection_matrix:
+  cols: 4
+  data:
+  - 1246.7956779804879
+  - 0.0
+  - 1216.4258435121951
+  - 0.0
+  - 0.0
+  - 702.7935148015422
+  - 463.31273260044634
+  - 0.0
+  - 0.0
+  - 0.0
+  - 1.0
+  - 0.0
+  rows: 3
+rectification_matrix:
+  cols: 3
+  data:
+  - 1
+  - 0
+  - 0
+  - 0
+  - 1
+  - 0
+  - 0
+  - 0
+  - 1
+  rows: 3

clover/camera_info/fisheye_cam_2592x1944.yaml

@@ -0,0 +1,65 @@
+# Generated from fisheye_cam.yaml by rescale_camera_info.py
+camera_matrix:
+  cols: 3
+  data:
+  - 5049.522495820976
+  - 0.0
+  - 4860.0
+  - 0.0
+  - 2846.313734946246
+  - 2050.3125
+  - 0.0
+  - 0.0
+  - 1.0
+  rows: 3
+camera_name: main_camera_optical
+distortion_coefficients:
+  cols: 8
+  data:
+  - 0.215356885
+  - -0.117472846
+  - -0.000306197672
+  - -0.000109444025
+  - -0.00453657258
+  - 0.573090623
+  - -0.127574577
+  - -0.0286125589
+  - 0.0
+  - 0.0
+  - 0.0
+  - 0.0
+  - 0.0
+  - 0.0
+  rows: 1
+distortion_model: plumb_bob
+image_height: 1944
+image_width: 2592
+projection_matrix:
+  cols: 4
+  data:
+  - 5049.522495820976
+  - 0.0
+  - 4926.52466622439
+  - 0.0
+  - 0.0
+  - 2846.313734946246
+  - 1876.4165670318075
+  - 0.0
+  - 0.0
+  - 0.0
+  - 1.0
+  - 0.0
+  rows: 3
+rectification_matrix:
+  cols: 3
+  data:
+  - 1
+  - 0
+  - 0
+  - 0
+  - 1
+  - 0
+  - 0
+  - 0
+  - 1
+  rows: 3

clover/camera_info/fisheye_cam_320x240.yaml

@@ -0,0 +1,65 @@
+# Generated from fisheye_cam.yaml by rescale_camera_info.py
+camera_matrix:
+  cols: 3
+  data:
+  - 166.23942373073172
+  - 0.0
+  - 160.0
+  - 0.0
+  - 166.5880923974026
+  - 120.0
+  - 0.0
+  - 0.0
+  - 1.0
+  rows: 3
+camera_name: main_camera_optical
+distortion_coefficients:
+  cols: 8
+  data:
+  - 0.215356885
+  - -0.117472846
+  - -0.000306197672
+  - -0.000109444025
+  - -0.00453657258
+  - 0.573090623
+  - -0.127574577
+  - -0.0286125589
+  - 0.0
+  - 0.0
+  - 0.0
+  - 0.0
+  - 0.0
+  - 0.0
+  rows: 1
+distortion_model: plumb_bob
+image_height: 240
+image_width: 320
+projection_matrix:
+  cols: 4
+  data:
+  - 166.23942373073172
+  - 0.0
+  - 162.19011246829268
+  - 0.0
+  - 0.0
+  - 166.5880923974026
+  - 109.82227735714285
+  - 0.0
+  - 0.0
+  - 0.0
+  - 1.0
+  - 0.0
+  rows: 3
+rectification_matrix:
+  cols: 3
+  data:
+  - 1
+  - 0
+  - 0
+  - 0
+  - 1
+  - 0
+  - 0
+  - 0
+  - 1
+  rows: 3

clover/camera_info/fisheye_cam_3840x2160.yaml

@@ -0,0 +1,65 @@
+# Generated from fisheye_cam.yaml by rescale_camera_info.py
+camera_matrix:
+  cols: 3
+  data:
+  - 30297.13497492585
+  - 0.0
+  - 29160.0
+  - 0.0
+  - 12808.411807258106
+  - 9226.40625
+  - 0.0
+  - 0.0
+  - 1.0
+  rows: 3
+camera_name: main_camera_optical
+distortion_coefficients:
+  cols: 8
+  data:
+  - 0.215356885
+  - -0.117472846
+  - -0.000306197672
+  - -0.000109444025
+  - -0.00453657258
+  - 0.573090623
+  - -0.127574577
+  - -0.0286125589
+  - 0.0
+  - 0.0
+  - 0.0
+  - 0.0
+  - 0.0
+  - 0.0
+  rows: 1
+distortion_model: plumb_bob
+image_height: 2160
+image_width: 3840
+projection_matrix:
+  cols: 4
+  data:
+  - 30297.13497492585
+  - 0.0
+  - 29559.14799734634
+  - 0.0
+  - 0.0
+  - 12808.411807258106
+  - 8443.874551643134
+  - 0.0
+  - 0.0
+  - 0.0
+  - 1.0
+  - 0.0
+  rows: 3
+rectification_matrix:
+  cols: 3
+  data:
+  - 1
+  - 0
+  - 0
+  - 0
+  - 1
+  - 0
+  - 0
+  - 0
+  - 1
+  rows: 3

clover/camera_info/fisheye_cam_4056x3040.yaml

@@ -0,0 +1,65 @@
+# Generated from fisheye_cam.yaml by rescale_camera_info.py
+camera_matrix:
+  cols: 3
+  data:
+  - 192008.0929035926
+  - 0.0
+  - 184801.5
+  - 0.0
+  - 81119.941445968
+  - 58433.90625
+  - 0.0
+  - 0.0
+  - 1.0
+  rows: 3
+camera_name: main_camera_optical
+distortion_coefficients:
+  cols: 8
+  data:
+  - 0.215356885
+  - -0.117472846
+  - -0.000306197672
+  - -0.000109444025
+  - -0.00453657258
+  - 0.573090623
+  - -0.127574577
+  - -0.0286125589
+  - 0.0
+  - 0.0
+  - 0.0
+  - 0.0
+  - 0.0
+  - 0.0
+  rows: 1
+distortion_model: plumb_bob
+image_height: 3040
+image_width: 4056
+projection_matrix:
+  cols: 4
+  data:
+  - 192008.0929035926
+  - 0.0
+  - 187331.10043318244
+  - 0.0
+  - 0.0
+  - 81119.941445968
+  - 53477.87216040651
+  - 0.0
+  - 0.0
+  - 0.0
+  - 1.0
+  - 0.0
+  rows: 3
+rectification_matrix:
+  cols: 3
+  data:
+  - 1
+  - 0
+  - 0
+  - 0
+  - 1
+  - 0
+  - 0
+  - 0
+  - 1
+  rows: 3

clover/camera_info/fisheye_cam_640x480.yaml

@@ -0,0 +1,65 @@
+# Generated from fisheye_cam.yaml by rescale_camera_info.py
+camera_matrix:
+  cols: 3
+  data:
+  - 166.23942373073172
+  - 0.0
+  - 160.0
+  - 0.0
+  - 166.5880923974026
+  - 120.0
+  - 0.0
+  - 0.0
+  - 1.0
+  rows: 3
+camera_name: main_camera_optical
+distortion_coefficients:
+  cols: 8
+  data:
+  - 0.215356885
+  - -0.117472846
+  - -0.000306197672
+  - -0.000109444025
+  - -0.00453657258
+  - 0.573090623
+  - -0.127574577
+  - -0.0286125589
+  - 0.0
+  - 0.0
+  - 0.0
+  - 0.0
+  - 0.0
+  - 0.0
+  rows: 1
+distortion_model: plumb_bob
+image_height: 480
+image_width: 640
+projection_matrix:
+  cols: 4
+  data:
+  - 166.23942373073172
+  - 0.0
+  - 162.19011246829268
+  - 0.0
+  - 0.0
+  - 166.5880923974026
+  - 109.82227735714285
+  - 0.0
+  - 0.0
+  - 0.0
+  - 1.0
+  - 0.0
+  rows: 3
+rectification_matrix:
+  cols: 3
+  data:
+  - 1
+  - 0
+  - 0
+  - 0
+  - 1
+  - 0
+  - 0
+  - 0
+  - 1
+  rows: 3

clover/camera_info/fisheye_cam_800x600.yaml

@@ -0,0 +1,65 @@
+# Generated from fisheye_cam.yaml by rescale_camera_info.py
+camera_matrix:
+  cols: 3
+  data:
+  - 207.79927966341464
+  - 0.0
+  - 200.0
+  - 0.0
+  - 208.23511549675322
+  - 150.0
+  - 0.0
+  - 0.0
+  - 1.0
+  rows: 3
+camera_name: main_camera_optical
+distortion_coefficients:
+  cols: 8
+  data:
+  - 0.215356885
+  - -0.117472846
+  - -0.000306197672
+  - -0.000109444025
+  - -0.00453657258
+  - 0.573090623
+  - -0.127574577
+  - -0.0286125589
+  - 0.0
+  - 0.0
+  - 0.0
+  - 0.0
+  - 0.0
+  - 0.0
+  rows: 1
+distortion_model: plumb_bob
+image_height: 600
+image_width: 800
+projection_matrix:
+  cols: 4
+  data:
+  - 207.79927966341464
+  - 0.0
+  - 202.73764058536585
+  - 0.0
+  - 0.0
+  - 208.23511549675322
+  - 137.27784669642855
+  - 0.0
+  - 0.0
+  - 0.0
+  - 1.0
+  - 0.0
+  rows: 3
+rectification_matrix:
+  cols: 3
+  data:
+  - 1
+  - 0
+  - 0
+  - 0
+  - 1
+  - 0
+  - 0
+  - 0
+  - 1
+  rows: 3

clover/cfg/Flow.cfg

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

clover/cmake/FindGeographicLib.cmake

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

clover/config/99-px4fmu.rules

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

clover/examples/camera.py

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

clover/examples/flight.py

@@ -15,17 +15,17 @@ set_attitude = rospy.ServiceProxy('set_attitude', srv.SetAttitude)
 set_rates = rospy.ServiceProxy('set_rates', srv.SetRates)
 land = rospy.ServiceProxy('land', Trigger)
 
-# Take off and hover 1 m above the ground
+print('Take off and hover 1 m above the ground')
 navigate(x=0, y=0, z=1, frame_id='body', auto_arm=True)
 
-# Wait for 3 seconds
-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

@@ -1,4 +1,4 @@
-# Information: https://clover.coex.tech/en/aruco.html
+# Information: https://clover.coex.tech/aruco
 
 import rospy
 from clover import srv
@@ -15,23 +15,23 @@ set_attitude = rospy.ServiceProxy('set_attitude', srv.SetAttitude)
 set_rates = rospy.ServiceProxy('set_rates', srv.SetRates)
 land = rospy.ServiceProxy('land', Trigger)
 
-# 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/leds.py

@@ -1,4 +1,4 @@
-# Information: https://clover.coex.tech/en/leds.html
+# Information: https://clover.coex.tech/led
 
 import rospy
 from clover.srv import SetLEDEffect
@@ -7,19 +7,25 @@ rospy.init_node('leds')
 
 set_effect = rospy.ServiceProxy('led/set_effect', SetLEDEffect)  # define proxy to ROS-service
 
+print('Fill red')
 set_effect(r=255, g=0, b=0)  # fill strip with red color
 rospy.sleep(2)
 
+print('Fill green')
 set_effect(r=0, g=100, b=0)  # fill strip with green color
 rospy.sleep(2)
 
+print('Fade to blue')
 set_effect(effect='fade', r=0, g=0, b=255)  # fade to blue color
 rospy.sleep(2)
 
+print('Flash red')
 set_effect(effect='flash', r=255, g=0, b=0)  # flash twice with red color
-rospy.sleep(5)
+rospy.sleep(2)
 
+print('Blink white')
 set_effect(effect='blink', r=255, g=255, b=255)  # blink with white color
 rospy.sleep(5)
 
+print('Rainbow')
 set_effect(effect='rainbow')  # show rainbow

clover/examples/navigate_wait.py

@@ -1,4 +1,4 @@
-# Information: https://clover.coex.tech/en/snippets.html#block-nav
+# Information: https://clover.coex.tech/en/snippets.html#navigate_wait
 
 import math
 import rospy
@@ -16,11 +16,8 @@ set_attitude = rospy.ServiceProxy('set_attitude', srv.SetAttitude)
 set_rates = rospy.ServiceProxy('set_rates', srv.SetRates)
 land = rospy.ServiceProxy('land', Trigger)
 
-def navigate_wait(x=0, y=0, z=0, yaw=float('nan'), yaw_rate=0, speed=0.5, \
-        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)
+def navigate_wait(x=0, y=0, z=0, yaw=math.nan, speed=0.5, frame_id='body', tolerance=0.2, auto_arm=False):
+    res = navigate(x=x, y=y, z=z, yaw=yaw, speed=speed, frame_id=frame_id, auto_arm=auto_arm)
 
     if not res.success:
         return res
@@ -31,11 +28,11 @@ def navigate_wait(x=0, y=0, z=0, yaw=float('nan'), yaw_rate=0, speed=0.5, \
             return res
         rospy.sleep(0.2)
 
-# Take off 1 meter
+print('Take off 1 meter')
 navigate_wait(z=1, frame_id='body', auto_arm=True)
 
-# Fly forward 1 m
+print('Fly forward 1 m')
 navigate_wait(x=1, frame_id='body')
 
-# Land
+print('Land')
 land()

clover/examples/red_circle.py

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

clover/examples/subscriber.py

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

clover/launch/aruco.launch

@@ -2,28 +2,43 @@
     <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)"/>
-        <param name="cornerRefinementMethod" value="2"/>
+        <remap from="map_markers" to="aruco_map/map"/>
+        <param name="dictionary" value="2"/> <!-- DICT_4X4_250 -->
         <param name="estimate_poses" value="true"/>
         <param name="send_tf" value="true"/>
-        <param name="known_tilt" value="map"/>
-        <param name="length" value="0.33"/>
+        <param name="use_map_markers" value="$(arg aruco_map)"/>
+        <param name="known_vertical" value="map" if="$(eval placement == 'floor' or placement == 'ceiling')"/>
+        <param name="flip_vertical" value="true" if="$(eval placement == 'ceiling')"/>
+        <param name="length" value="$(arg length)"/>
+        <param name="transform_timeout" value="0.1"/>
+        <!-- aruco detector parameters -->
+        <param name="cornerRefinementMethod" value="2"/> <!-- contour refinement -->
+        <param name="minMarkerPerimeterRate" value="0.075"/> <!-- 0.075 for 320x240, 0.0375 for 640x480 -->
+        <!-- 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_vertical" value="map" if="$(eval placement == 'floor' or placement == 'ceiling')"/>
+        <param name="flip_vertical" value="true" if="$(eval placement == 'ceiling')"/>
         <param name="image_axis" value="true"/>
         <param name="frame_id" value="aruco_map_detected" if="$(arg aruco_vpe)"/>
         <param name="frame_id" value="aruco_map" unless="$(arg aruco_vpe)"/>
@@ -32,11 +47,11 @@
     </node>
 
     <!-- vpe publisher from aruco markers -->
-    <node name="vpe_publisher" pkg="clover" type="vpe_publisher" if="$(arg aruco_vpe)" output="screen" clear_params="true">
-        <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

@@ -10,8 +10,11 @@
     <arg name="aruco" default="false"/>
     <arg name="rangefinder_vl53l1x" default="true"/>
     <arg name="led" default="true"/>
-    <arg name="rc" default="true"/>
-    <arg name="shell" default="true"/>
+    <arg name="blocks" default="false"/>
+    <arg name="rc" default="false"/>
+    <arg name="force_init" default="true"/> <!-- force estimator to init by publishing zero pose -->
+
+    <arg name="simulator" default="false"/> <!-- flag that we are operating on a simulated drone -->
 
     <!-- log formatting -->
     <env name="ROSCONSOLE_FORMAT" value="[${severity}] [${time}]: ${logger}: ${message}"/>
@@ -31,32 +34,30 @@
     </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"/>
+        <param name="disable_on_vpe" value="true"/>
     </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"/>
+        <param name="terrain_frame_mode" value="range"/>
     </node>
 
     <!-- main camera -->
-    <include file="$(find clover)/launch/main_camera.launch" if="$(arg main_camera)"/>
+    <include file="$(find clover)/launch/main_camera.launch" if="$(arg main_camera)">
+        <arg name="simulator" value="$(arg simulator)"/>
+    </include>
 
     <!-- rosbridge -->
     <include file="$(find rosbridge_server)/launch/rosbridge_websocket.launch" if="$(eval rosbridge or rc)"/>
@@ -65,14 +66,22 @@
     <node name="tf2_web_republisher" pkg="tf2_web_republisher" type="tf2_web_republisher" output="screen" if="$(arg rosbridge)"/>
 
     <!-- vl53l1x ToF rangefinder -->
-    <node name="rangefinder" pkg="vl53l1x" type="vl53l1x_node" output="screen" if="$(arg rangefinder_vl53l1x)">
+    <node name="rangefinder" pkg="vl53l1x" type="vl53l1x_node" output="screen" if="$(eval rangefinder_vl53l1x and not simulator)">
         <param name="frame_id" value="rangefinder"/>
         <param name="min_signal" value="0.4"/>
         <param name="pass_statuses" type="yaml" value="[0, 6, 7, 11]"/>
     </node>
 
+    <!-- rangefinder's frame -->
+    <node pkg="tf2_ros" type="static_transform_publisher" name="rangefinder_frame" args="0 0 -0.05 0 1.5707963268 0 base_link rangefinder" if="$(arg rangefinder_vl53l1x)"/>
+
     <!-- led strip -->
-    <include file="$(find clover)/launch/led.launch" if="$(arg led)"/>
+    <include file="$(find clover)/launch/led.launch" if="$(arg led)">
+        <arg name="simulator" value="$(arg simulator)"/>
+    </include>
+
+    <!-- Clover Blocks -->
+    <node name="clover_blocks" pkg="clover_blocks" type="clover_blocks" output="screen" if="$(arg blocks)"/>
 
     <!-- rc backend -->
     <node name="rc" pkg="clover" type="rc" output="screen" if="$(arg rc)" clear_params="true">
@@ -80,6 +89,4 @@
         <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)"/>
 </launch>

clover/launch/led.launch

@@ -2,13 +2,17 @@
     <arg name="ws281x" default="true"/>
     <arg name="led_effect" default="true"/>
     <arg name="led_notify" default="true"/>
+    <arg name="led_count" default="72"/>
+    <arg name="gpio_pin" default="21"/>
+
+    <arg name="simulator" default="false"/>
 
     <!-- For additional help go to https://clover.coex.tech/led -->
 
     <!-- ws281x led strip driver -->
-    <node pkg="ws281x" name="led" type="ws281x_node" clear_params="true" output="screen" if="$(arg ws281x)">
-        <param name="led_count" value="58"/>
-        <param name="gpio_pin" value="21"/>
+    <node pkg="ws281x" name="led" type="ws281x_node" clear_params="true" output="screen" if="$(eval ws281x and not simulator)">
+        <param name="led_count" value="$(arg led_count)"/>
+        <param name="gpio_pin" value="$(arg gpio_pin)"/>
         <param name="brightness" value="64"/>
         <param name="strip_type" value="WS2811_STRIP_GRB"/>
         <param name="target_frequency" value="800000"/>
@@ -17,7 +21,8 @@
     </node>
 
     <!-- high level led effects control, events notification with leds -->
-    <node pkg="clover" name="led_effect" type="led" ns="led" clear_params="true" output="screen" if="$(arg led_effect)">
+    <node pkg="clover" name="led_effect" type="led" clear_params="true" output="screen" if="$(arg led_effect)">
+        <param name="led" value="led"/>
         <param name="blink_rate" value="2"/>
         <param name="fade_period" value="0.5"/>
         <param name="rainbow_period" value="5"/>
@@ -31,8 +36,8 @@
             altctl: { r: 255, g: 255, b: 40 }
             posctl: { r: 50, g: 100, b: 220 }
             offboard: { r: 220, g: 20, b: 250 }
-            low_battery: { threshold: 3.7, effect: blink_fast, r: 255, g: 0, b: 0 }
-            error: { effect: flash, r: 255, g: 0, b: 0 }
+            low_battery: { threshold: 3.6, effect: blink_fast, r: 255, g: 0, b: 0 }
+            error: { effect: flash, r: 255, g: 0, b: 0, ignore: [ "[lpe] vision position timeout" ]}
         </rosparam>
     </node>
 </launch>

clover/launch/main_camera.launch

@@ -3,35 +3,77 @@
 
     <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="type" default="libcamera"/> <!-- camera interface: libcamera, v4l2 -->
+    <arg name="camera_id" default="0"/> <!-- libcamera camera id -->
+    <arg name="device" default="/dev/video0"/> <!-- v4l2 device path -->
+    <arg name="width" default="320"/>
+    <arg name="height" default="240"/>
+    <arg name="fps" default="40"/>
+    <arg name="throttled_topic" default="true"/> <!-- enable throttled image topic -->
+    <arg name="throttled_topic_rate" default="5.0"/> <!-- throttled image topic rate -->
+    <arg name="rectify" default="false"/> <!-- enable rectification -->
+    <arg name="simulator" default="false"/>
 
     <node if="$(eval direction_z == 'down' and direction_y == 'backward')" pkg="tf2_ros" type="static_transform_publisher" name="main_camera_frame" args="0.05 0 -0.07 -1.5707963 0 3.1415926 base_link main_camera_optical"/>
     <node if="$(eval direction_z == 'down' and direction_y == 'forward')" pkg="tf2_ros" type="static_transform_publisher" name="main_camera_frame" args="0.05 0 -0.07 1.5707963 0 3.1415926 base_link main_camera_optical"/>
     <node if="$(eval direction_z == 'up' and direction_y == 'backward')" pkg="tf2_ros" type="static_transform_publisher" name="main_camera_frame" args="0.05 0 0.07 1.5707963 0 0 base_link main_camera_optical"/>
     <node if="$(eval direction_z == 'up' and direction_y == 'forward')" pkg="tf2_ros" type="static_transform_publisher" name="main_camera_frame" args="0.05 0 0.07 -1.5707963 0 0 base_link main_camera_optical"/>
+    <node if="$(eval direction_z == 'forward')" pkg="tf2_ros" type="static_transform_publisher" name="main_camera_frame" args="0.03 0 0.05 -1.5707963 0 -1.5707963 base_link main_camera_optical"/>
+    <node if="$(eval direction_z == 'backward')" pkg="tf2_ros" type="static_transform_publisher" name="main_camera_frame" args="-0.03 0 0.05 1.5707963 0 -1.5707963 base_link main_camera_optical"/>
 
     <!-- Template for custom camera orientation -->
     <!-- Camera position and orientation are represented by base_link -> main_camera_optical transform -->
     <!-- static_transform_publisher arguments: x y z yaw pitch roll frame_id child_frame_id -->
     <!-- <node pkg="tf2_ros" type="static_transform_publisher" name="main_camera_frame" args="0.05 0 -0.07 -1.5707963 0 3.1415926 base_link main_camera_optical"/> -->
 
-    <!-- camera node -->
-    <node pkg="nodelet" type="nodelet" name="main_camera" args="load cv_camera/CvCameraNodelet nodelet_manager" clear_params="true">
-        <param name="device_path" value="/dev/video0"/> <!-- v4l2 device -->
+    <!-- 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 using libcamera -->
+    <node pkg="nodelet" type="nodelet" name="main_camera" args="load libcamera_ros/LibcameraRos main_camera_nodelet_manager" output="screen" clear_params="true" if="$(eval not simulator and type == 'libcamera')" respawn="true">
+        <param name="camera_name" value=""/>
+        <param name="camera_id" value="$(arg camera_id)"/>
+        <param name="frame_id" value="main_camera_optical"/>
+        <param name="calib_url" type="string" value="file://$(find clover)/camera_info/fisheye_cam_$(arg width)x$(arg height).yaml"/>
+        <param name="stream_role" value="still"/>
+        <param name="pixel_format" value="RGB888"/>
+        <param name="use_ros_time" value="true"/>
+        <param name="resolution/width" value="$(arg width)"/>
+        <param name="resolution/height" value="$(arg height)"/>
+
+        <!-- see: https://github.com/ctu-mrs/libcamera_ros/blob/b3645/config/param.yaml#L19 -->
+        <param name="control/fps" value="$(arg fps)"/>
+    </node>
+
+    <!-- old camera node for v4l2 (cv_camera) -->
+    <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" if="$(eval not simulator and type == 'v4l2')" 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"/>
 
         <param name="rate" value="100"/> <!-- poll rate -->
-        <param name="cv_cap_prop_fps" value="40"/> <!-- camera FPS -->
+        <param name="cv_cap_prop_fps" value="$(arg fps)"/> <!-- camera FPS -->
         <param name="capture_delay" value="0.02"/> <!-- approximate delay on frame retrieving -->
         <param name="rescale_camera_info" value="true"/> <!-- automatically rescale camera calibration info -->
-
-        <!-- camera resolution -->
-        <param name="image_width" value="320"/>
-        <param name="image_height" value="240"/>
+        <param name="image_width" value="$(arg width)"/>
+        <param name="image_height" value="$(arg height)"/>
     </node>
 
     <!-- camera visualization markers -->
     <node pkg="clover" type="camera_markers" ns="main_camera" name="main_camera_markers">
         <param name="scale" value="3.0"/>
     </node>
+
+    <!-- image topic throttled -->
+    <node pkg="topic_tools" name="main_camera_throttle" type="throttle" ns="main_camera"
+        args="messages image_raw $(arg throttled_topic_rate) image_raw_throttled" if="$(arg throttled_topic)"/>
+
+    <!-- rectified image topic -->
+    <node pkg="nodelet" type="nodelet" name="rectify" args="load image_proc/rectify main_camera_nodelet_manager" if="$(arg rectify)">
+        <remap from="image_mono" to="main_camera/image_raw"/>
+        <remap from="camera_info" to="main_camera/camera_info"/>
+        <remap from="image_rect" to="main_camera/image_rect"/>
+    </node>
 </launch>

clover/launch/mavros.launch

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

clover/launch/mavros_config.yaml

@@ -1,11 +1,11 @@
 # Config file for mavros
 # Based on https://raw.githubusercontent.com/mavlink/mavros/master/mavros/launch/px4_config.yaml
 
-startup_px4_usb_quirk: true
+startup_px4_usb_quirk: false
 
 conn:
-  heartbeat_rate: 1.0    # send hertbeat rate in Hertz
-  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/launch/simulator.launch

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

clover/launch/sitl.launch

@@ -1,19 +0,0 @@
-<launch>
-    <!-- clover configuration for testing in sitl -->
-    <arg name="ip" default="127.0.0.1"/>
-    <arg name="rosbridge" default="false"/>
-
-    <include file="$(find clover)/launch/clover.launch">
-        <arg name="fcu_conn" value="udp"/>
-        <arg name="fcu_ip" value="$(arg ip)"/>
-        <arg name="gcs_bridge" value="false"/>
-        <arg name="optical_flow" value="false"/>
-        <arg name="web_video_server" default="false"/>
-        <arg name="main_camera" default="false"/>
-        <arg name="rosbridge" value="$(arg rosbridge)"/>
-        <arg name="aruco" default="false"/>
-        <arg name="rangefinder_vl53l1x" default="false"/>
-        <arg name="led" default="false"/>
-        <arg name="rc" default="false"/>
-    </include>
-</launch>

clover/msg/State.msg

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

clover/package.xml

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

clover/requirements.txt

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

clover/setup.py

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