vpe_publisher: allow taking pose data from topic

* aruco_pose: improved processing of axis visualization in the topic /aruco_map/image

* aruco_pose: reworked method for drawing axis arrow

* aruco_pose: fix indentation error

* Fix style

.editorconfig

@@ -9,5 +9,12 @@ charset = utf-8
 indent_style = space
 indent_size = 4
 
-[*.{js,html}]
+[*.{cpp,h,js,html,txt}]
 indent_style = tab
+
+[*.txt]
+tab_width = 8
+
+[CMakeLists.txt]
+indent_style = space
+indent_size = 2

.gitattributes

@@ -1,2 +1,5 @@
-apps/ios/cleverrc/roslib.js linguist-vendored
 apps/ios/cleverrc/BinUtils.swift linguist-vendored
+roslib.js linguist-vendored
+eventemitter2.js linguist-vendored
+ros3d.js linguist-vendored
+three.min.js linguist-vendored

.gitignore

@@ -1,2 +1,6 @@
-/deploy/ros_lib/
 *.pyc
+*.DS_Store
+/images
+node_modules/
+_book/
+package-lock.json

.markdownlint.json

@@ -1,22 +1,112 @@
 {
     "MD003": false,
+    "MD010": {
+        "code_blocks": false
+    },
     "MD013": false,
+    "MD024": false,
+    "MD026" :{
+        "punctuation": ".,;:!"
+    },
     "MD033": false,
     "MD034": false,
+    "MD040": false,
     "MD044": {
         "names": [
+            "COEX",
+            "Copter Express",
+            "Коптер Экспресс",
+            "Клевер",
             "MAVLink",
             "ROS",
+            "ROS Kinetic",
+            "OpenCV",
+            "Gazebo",
+            "GitHub",
+            "FPV",
+            "PPM",
+            "PWM",
             "Python",
             "C++",
+            "JavaScript",
+            "Node.js",
+            "Django",
+            "Flask",
+            "HTTP",
+            "HTTPS",
+            "WebSocket",
+            "RPC",
             "PX4",
+            "ArduPilot",
+            "jMAVSim",
+            "px4.io",
+            "logs.px4.io",
+            "QGroundControl",
+            "QGC",
             "WireShark",
+            "FlightPlot",
+            "OFFBOARD",
+            "ACRO",
+            "RPY",
+            "LPE",
+            "EKF2",
+            "IMU",
+            "VPE",
+            "SITL",
+            "PID",
             "Wi-Fi",
             "Raspberry Pi",
+            "RPi",
+            "Linux",
+            "GNU",
+            "GNU/Linux",
+            "Windows",
+            "Docker",
+            "RFC",
+            "Travis",
+            "travis-ci.org",
+            "travis-ci.com",
+            "macOS",
+            "iOS",
+            "Android",
+            "Bluetooth",
+            "Raspbian",
+            "Raspbian Jesse",
+            "Raspbian Stretch",
+            "Raspbian Buster",
             "Pixhawk",
             "Pixracer",
-            "ArUco"
+            "Arduino",
+            "GPS",
+            "ArUco",
+            "LIRC",
+            "GPIO",
+            "HC-SR04",
+            "RCW-0001",
+            "RealSense",
+            "NUC",
+            "NVIDIA",
+            "Jetson",
+            "Jetson Nano",
+            "STM",
+            "LED",
+            "USB",
+            "FAT32",
+            "uORB",
+            "SSH",
+            "PuTTY",
+            "API",
+            "UART",
+            "GND",
+            "VCC",
+            "SCL",
+            "SDA",
+            "TCP",
+            "UDP",
+            "QR",
+            "Li-ion"
         ],
         "code_blocks": false
-    }
+    },
+    "MD045": false
 }

.travis.yml

@@ -0,0 +1,104 @@
+sudo: required
+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
+        api_key: ${GITHUB_OAUTH_TOKEN}
+        file: ${IMAGE_NAME}.zip
+        skip_cleanup: true
+        on:
+          tags: true
+        draft: true
+        name: ${TRAVIS_TAG}
+    - 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
+        github-token: ${GITHUB_OAUTH_TOKEN}
+        keep-history: true
+        target-branch: master
+        repo: CopterExpress/clever.coex.tech
+        fqdn: clever.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"
+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,20 +1,20 @@
 # CLEVER
 
-<img src="docs/assets/clever3.png" align="right" width="300px" alt="CLEVER drone">
+<img src="docs/assets/clever4-front-white.png" align="right" width="400px" alt="CLEVER drone">
 
-CLEVER is an educational programmable drone kit consisting of an unassembled quadcopter, open source software and documentation. The kit includes Pixhawk/Pixracer autopilot running PX4 firmware, Raspberry Pi 3 as companion computer, a camera for computer vision navigation as well as additional sensors and peripheral devices.
+CLEVER (Russian: *"Клевер"*, meaning *"Clover"*) is an educational programmable drone kit consisting of an unassembled quadcopter, open source software and documentation. The kit includes Pixhawk/Pixracer autopilot running PX4 firmware, Raspberry Pi 3 as companion computer, a camera for computer vision navigation as well as additional sensors and peripheral devices.
 
 Copter Express has implemented a large number of different autonomous drone projects using exactly the same platform: [automated pizza delivery](https://www.youtube.com/watch?v=hmkAoZOtF58) in Samara and Kazan, coffee delivery in Skolkovo Innovation Center, [autonomous quadcopter with charging station](https://www.youtube.com/watch?v=RjX6nUqw1mI) for site monitoring and security, winning drones on [Robocross-2016](https://www.youtube.com/watch?v=dGbDaz_VmYU) and [Robocross-2017](https://youtu.be/AQnd2CRczbQ) competitions and many others.
 
-**The main documentation in Russian is available [on our Gitbook](https://clever.copterexpress.com/).**
+**The main documentation is available [on Gitbook](https://clever.coex.tech/).**
 
 Use it to learn how to assemble, configure, pilot and program autonomous CLEVER drone.
 
-## Preconfigured RPi 3 image
+## Raspberry Pi image
 
 **Preconfigured image for Raspberry Pi 3 with installed and configured software, ready to fly, is available [in the Releases section](https://github.com/CopterExpress/clever/releases).**
 
-[![Build Status](http://builder.coex.space/job/CopterExpress---clever/badge/icon)](http://builder.coex.space/job/CopterExpress---clever/)
+[![Build Status](https://travis-ci.org/CopterExpress/clever.svg?branch=master)](https://travis-ci.org/CopterExpress/clever)
 
 Image includes:
 
@@ -23,55 +23,81 @@ Image includes:
 * Configured networking
 * OpenCV
 * mavros
+* Periphery drivers (`pigpiod`, `rpi_ws281x`, etc)
 * CLEVER software bundle for autonomous drone control
 
-API description (in Russian) for autonomous flights is available [on GitBook](https://copterexpress.gitbooks.io/clever/simple_offboard.html).
+API description (in Russian) for autonomous flights is available [on GitBook](https://clever.coex.tech/simple_offboard.html).
 
 ## Manual installation
 
-Install ROS Kinetic according to the [documentation](http://wiki.ros.org/kinetic/Installation).
+Install ROS Kinetic according to the [documentation](http://wiki.ros.org/kinetic/Installation), then [create a Catkin workspace](http://wiki.ros.org/catkin/Tutorials/create_a_workspace).
 
-Clone repo to directory `/home/pi/catkin_ws/src/clever`:
+Clone this repo to directory `~/catkin_ws/src/clever`:
 
 ```bash
 cd ~/catkin_ws/src
 git clone https://github.com/CopterExpress/clever.git clever
 ```
 
-Build ROS packages:
+All the required ROS packages (including `mavros` and `opencv`) can be installed using `rosdep`:
+
+```bash
+cd ~/catkin_ws/
+rosdep install -y --from-paths src --ignore-src
+```
+
+Build ROS packages (on memory constrained platforms you might be going to need to use `-j1` key):
 
 ```bash
 cd ~/catkin_ws
 catkin_make -j1
 ```
 
-Enable systemd service `roscore` (if not enabled):
+To complete `mavros` install you'll need to install `geographiclib` datasets:
 
 ```bash
-sudo systemctl enable /home/pi/catkin_ws/src/clever/deploy/roscore.service
+curl https://raw.githubusercontent.com/mavlink/mavros/master/mavros/scripts/install_geographiclib_datasets.sh | sudo bash
+```
+
+You may optionally install udev rules to provide `/dev/px4fmu` symlink to your PX4-based flight controller connected over USB. Copy `99-px4fmu.rules` to your `/lib/udev/rules.d` folder:
+
+```bash
+cd ~/catkin_ws/src/clever/clever/config
+sudo cp 99-px4fmu.rules /lib/udev/rules.d
+```
+
+Alternatively you may change the `fcu_url` property in `mavros.launch` file to point to your flight controller device.
+
+## Running
+
+Enable systemd service `roscore` (if not running):
+
+```bash
+sudo systemctl enable /home/<username>/catkin_ws/src/clever/builder/assets/roscore.service
 sudo systemctl start roscore
 ```
 
-Enable systemd service `clever`:
+To start connection to SITL, use:
 
 ```bash
-sudo systemctl enable /home/pi/catkin_ws/src/clever/deploy/clever.service
+roslaunch clever sitl.launch
+```
+
+To start connection to the flight controller, use:
+
+```bash
+roslaunch clever clever.launch
+```
+
+> 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/clever/deploy/clever.service
 sudo systemctl start clever
 ```
 
-### Dependencies
+## License
 
-[ROS Kinetic](http://wiki.ros.org/kinetic).
-
-Necessary ROS packages:
-
-* `opencv3`
-* `mavros`
-* `rosbridge_suite`
-* `web_video_server`
-* `cv_camera`
-* `nodelet`
-* `dynamic_reconfigure`
-* `bondcpp`, branch `master`
-* `roslint`
-* `rosserial`
+While the Clever 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.

_config.yml

@@ -1,2 +0,0 @@
-theme: jekyll-theme-cayman
-tagline: Конструктор программируемого квадрокоптера

apps/android/.gitignore

@@ -0,0 +1,11 @@
+*.iml
+.gradle
+/local.properties
+/.idea/caches/build_file_checksums.ser
+/.idea/libraries
+/.idea/modules.xml
+/.idea/workspace.xml
+.DS_Store
+/build
+/captures
+.externalNativeBuild

apps/android/.idea/assetWizardSettings.xml

@@ -0,0 +1,57 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="WizardSettings">
+    <option name="children">
+      <map>
+        <entry key="imageWizard">
+          <value>
+            <PersistentState>
+              <option name="children">
+                <map>
+                  <entry key="imageAssetPanel">
+                    <value>
+                      <PersistentState>
+                        <option name="children">
+                          <map>
+                            <entry key="launcher">
+                              <value>
+                                <PersistentState>
+                                  <option name="children">
+                                    <map>
+                                      <entry key="foregroundImage">
+                                        <value>
+                                          <PersistentState>
+                                            <option name="values">
+                                              <map>
+                                                <entry key="scalingPercent" value="54" />
+                                              </map>
+                                            </option>
+                                          </PersistentState>
+                                        </value>
+                                      </entry>
+                                    </map>
+                                  </option>
+                                  <option name="values">
+                                    <map>
+                                      <entry key="backgroundAssetType" value="COLOR" />
+                                      <entry key="backgroundColor" value="ffffff" />
+                                      <entry key="foregroundImage" value="C:\Users\Motoy\Desktop\COEX\logo.png" />
+                                    </map>
+                                  </option>
+                                </PersistentState>
+                              </value>
+                            </entry>
+                          </map>
+                        </option>
+                      </PersistentState>
+                    </value>
+                  </entry>
+                </map>
+              </option>
+            </PersistentState>
+          </value>
+        </entry>
+      </map>
+    </option>
+  </component>
+</project>

apps/android/.idea/codeStyles/Project.xml

@@ -0,0 +1,35 @@
+<component name="ProjectCodeStyleConfiguration">
+  <code_scheme name="Project" version="173">
+    <JetCodeStyleSettings>
+      <option name="CODE_STYLE_DEFAULTS" value="KOTLIN_OFFICIAL" />
+    </JetCodeStyleSettings>
+    <Objective-C-extensions>
+      <file>
+        <option name="com.jetbrains.cidr.lang.util.OCDeclarationKind" value="Import" />
+        <option name="com.jetbrains.cidr.lang.util.OCDeclarationKind" value="Macro" />
+        <option name="com.jetbrains.cidr.lang.util.OCDeclarationKind" value="Typedef" />
+        <option name="com.jetbrains.cidr.lang.util.OCDeclarationKind" value="Enum" />
+        <option name="com.jetbrains.cidr.lang.util.OCDeclarationKind" value="Constant" />
+        <option name="com.jetbrains.cidr.lang.util.OCDeclarationKind" value="Global" />
+        <option name="com.jetbrains.cidr.lang.util.OCDeclarationKind" value="Struct" />
+        <option name="com.jetbrains.cidr.lang.util.OCDeclarationKind" value="FunctionPredecl" />
+        <option name="com.jetbrains.cidr.lang.util.OCDeclarationKind" value="Function" />
+      </file>
+      <class>
+        <option name="com.jetbrains.cidr.lang.util.OCDeclarationKind" value="Property" />
+        <option name="com.jetbrains.cidr.lang.util.OCDeclarationKind" value="Synthesize" />
+        <option name="com.jetbrains.cidr.lang.util.OCDeclarationKind" value="InitMethod" />
+        <option name="com.jetbrains.cidr.lang.util.OCDeclarationKind" value="StaticMethod" />
+        <option name="com.jetbrains.cidr.lang.util.OCDeclarationKind" value="InstanceMethod" />
+        <option name="com.jetbrains.cidr.lang.util.OCDeclarationKind" value="DeallocMethod" />
+      </class>
+      <extensions>
+        <pair source="cpp" header="h" fileNamingConvention="NONE" />
+        <pair source="c" header="h" fileNamingConvention="NONE" />
+      </extensions>
+    </Objective-C-extensions>
+    <codeStyleSettings language="kotlin">
+      <option name="CODE_STYLE_DEFAULTS" value="KOTLIN_OFFICIAL" />
+    </codeStyleSettings>
+  </code_scheme>
+</component>

apps/android/.idea/codeStyles/codeStyleConfig.xml

@@ -0,0 +1,5 @@
+<component name="ProjectCodeStyleConfiguration">
+  <state>
+    <option name="USE_PER_PROJECT_SETTINGS" value="true" />
+  </state>
+</component>

apps/android/.idea/gradle.xml

@@ -0,0 +1,18 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="GradleSettings">
+    <option name="linkedExternalProjectsSettings">
+      <GradleProjectSettings>
+        <option name="distributionType" value="DEFAULT_WRAPPED" />
+        <option name="externalProjectPath" value="$PROJECT_DIR$" />
+        <option name="modules">
+          <set>
+            <option value="$PROJECT_DIR$" />
+            <option value="$PROJECT_DIR$/app" />
+          </set>
+        </option>
+        <option name="resolveModulePerSourceSet" value="false" />
+      </GradleProjectSettings>
+    </option>
+  </component>
+</project>

apps/android/.idea/misc.xml

@@ -0,0 +1,38 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="NullableNotNullManager">
+    <option name="myDefaultNullable" value="android.support.annotation.Nullable" />
+    <option name="myDefaultNotNull" value="android.support.annotation.NonNull" />
+    <option name="myNullables">
+      <value>
+        <list size="7">
+          <item index="0" class="java.lang.String" itemvalue="org.jetbrains.annotations.Nullable" />
+          <item index="1" class="java.lang.String" itemvalue="javax.annotation.Nullable" />
+          <item index="2" class="java.lang.String" itemvalue="javax.annotation.CheckForNull" />
+          <item index="3" class="java.lang.String" itemvalue="edu.umd.cs.findbugs.annotations.Nullable" />
+          <item index="4" class="java.lang.String" itemvalue="android.support.annotation.Nullable" />
+          <item index="5" class="java.lang.String" itemvalue="androidx.annotation.Nullable" />
+          <item index="6" class="java.lang.String" itemvalue="androidx.annotation.RecentlyNullable" />
+        </list>
+      </value>
+    </option>
+    <option name="myNotNulls">
+      <value>
+        <list size="6">
+          <item index="0" class="java.lang.String" itemvalue="org.jetbrains.annotations.NotNull" />
+          <item index="1" class="java.lang.String" itemvalue="javax.annotation.Nonnull" />
+          <item index="2" class="java.lang.String" itemvalue="edu.umd.cs.findbugs.annotations.NonNull" />
+          <item index="3" class="java.lang.String" itemvalue="android.support.annotation.NonNull" />
+          <item index="4" class="java.lang.String" itemvalue="androidx.annotation.NonNull" />
+          <item index="5" class="java.lang.String" itemvalue="androidx.annotation.RecentlyNonNull" />
+        </list>
+      </value>
+    </option>
+  </component>
+  <component name="ProjectRootManager" version="2" languageLevel="JDK_1_7" project-jdk-name="1.8" project-jdk-type="JavaSDK">
+    <output url="file://$PROJECT_DIR$/build/classes" />
+  </component>
+  <component name="ProjectType">
+    <option name="id" value="Android" />
+  </component>
+</project>

apps/android/.idea/runConfigurations.xml

@@ -0,0 +1,12 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="RunConfigurationProducerService">
+    <option name="ignoredProducers">
+      <set>
+        <option value="org.jetbrains.plugins.gradle.execution.test.runner.AllInPackageGradleConfigurationProducer" />
+        <option value="org.jetbrains.plugins.gradle.execution.test.runner.TestClassGradleConfigurationProducer" />
+        <option value="org.jetbrains.plugins.gradle.execution.test.runner.TestMethodGradleConfigurationProducer" />
+      </set>
+    </option>
+  </component>
+</project>

apps/android/.idea/vcs.xml

@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="VcsDirectoryMappings">
+    <mapping directory="$PROJECT_DIR$" vcs="Git" />
+  </component>
+</project>

apps/android/app/.gitignore

@@ -0,0 +1 @@
+/build

apps/android/app/build.gradle

@@ -0,0 +1,33 @@
+apply plugin: 'com.android.application'
+
+apply plugin: 'kotlin-android'
+
+apply plugin: 'kotlin-android-extensions'
+
+android {
+    compileSdkVersion 28
+    defaultConfig {
+        applicationId "express.copter.cleverrc"
+        minSdkVersion 19
+        targetSdkVersion 28
+        versionCode 1
+        versionName "1.0"
+        testInstrumentationRunner "android.support.test.runner.AndroidJUnitRunner"
+    }
+    buildTypes {
+        release {
+            minifyEnabled false
+            proguardFiles getDefaultProguardFile('proguard-android.txt'), 'proguard-rules.pro'
+        }
+    }
+}
+
+dependencies {
+    implementation fileTree(dir: 'libs', include: ['*.jar'])
+    implementation"org.jetbrains.kotlin:kotlin-stdlib-jdk7:$kotlin_version"
+    implementation 'com.android.support:appcompat-v7:28.0.0'
+    implementation 'com.android.support.constraint:constraint-layout:1.1.3'
+    testImplementation 'junit:junit:4.12'
+    androidTestImplementation 'com.android.support.test:runner:1.0.2'
+    androidTestImplementation 'com.android.support.test.espresso:espresso-core:3.0.2'
+}

apps/android/app/proguard-rules.pro

@@ -0,0 +1,21 @@
+# Add project specific ProGuard rules here.
+# You can control the set of applied configuration files using the
+# proguardFiles setting in build.gradle.
+#
+# For more details, see
+#   http://developer.android.com/guide/developing/tools/proguard.html
+
+# If your project uses WebView with JS, uncomment the following
+# and specify the fully qualified class name to the JavaScript interface
+# class:
+#-keepclassmembers class fqcn.of.javascript.interface.for.webview {
+#   public *;
+#}
+
+# Uncomment this to preserve the line number information for
+# debugging stack traces.
+#-keepattributes SourceFile,LineNumberTable
+
+# If you keep the line number information, uncomment this to
+# hide the original source file name.
+#-renamesourcefileattribute SourceFile

apps/android/app/src/androidTest/java/express/copter/cleverrc/ExampleInstrumentedTest.kt

@@ -0,0 +1,24 @@
+package express.copter.cleverrc
+
+import android.support.test.InstrumentationRegistry
+import android.support.test.runner.AndroidJUnit4
+
+import org.junit.Test
+import org.junit.runner.RunWith
+
+import org.junit.Assert.*
+
+/**
+ * Instrumented test, which will execute on an Android device.
+ *
+ * See [testing documentation](http://d.android.com/tools/testing).
+ */
+@RunWith(AndroidJUnit4::class)
+class ExampleInstrumentedTest {
+    @Test
+    fun useAppContext() {
+        // Context of the app under test.
+        val appContext = InstrumentationRegistry.getTargetContext()
+        assertEquals("express.copter.cleverrc", appContext.packageName)
+    }
+}

apps/android/app/src/main/AndroidManifest.xml

@@ -0,0 +1,25 @@
+<?xml version="1.0" encoding="utf-8"?>
+<manifest xmlns:android="http://schemas.android.com/apk/res/android"
+          package="express.copter.cleverrc">
+    <uses-permission android:name="android.permission.INTERNET"/>
+
+
+    <application
+            android:allowBackup="true"
+            android:icon="@mipmap/ic_launcher"
+            android:label="@string/app_name"
+            android:roundIcon="@mipmap/ic_launcher_round"
+            android:supportsRtl="true"
+            android:theme="@style/AppTheme">
+        <activity android:name=".MainActivity"
+                  android:screenOrientation="landscape"
+                  android:theme="@style/NoUiAppTheme">
+            <intent-filter>
+                <action android:name="android.intent.action.MAIN"/>
+
+                <category android:name="android.intent.category.LAUNCHER"/>
+            </intent-filter>
+        </activity>
+    </application>
+
+</manifest>

apps/android/app/src/main/assets/clever.svg

@@ -0,0 +1,57 @@
+<?xml version="1.0" encoding="UTF-8" standalone="no"?>
+<svg
+   xmlns:dc="http://purl.org/dc/elements/1.1/"
+   xmlns:cc="http://creativecommons.org/ns#"
+   xmlns:rdf="http://www.w3.org/1999/02/22-rdf-syntax-ns#"
+   xmlns:svg="http://www.w3.org/2000/svg"
+   xmlns="http://www.w3.org/2000/svg"
+   viewBox="0 0 69.988266 69.987198"
+   height="69.987198"
+   width="69.988266"
+   xml:space="preserve"
+   id="svg2"
+   version="1.1"><metadata
+     id="metadata8"><rdf:RDF><cc:Work
+         rdf:about=""><dc:format>image/svg+xml</dc:format><dc:type
+           rdf:resource="http://purl.org/dc/dcmitype/StillImage" /></cc:Work></rdf:RDF></metadata><defs
+     id="defs6"><clipPath
+       id="clipPath18"
+       clipPathUnits="userSpaceOnUse"><path
+         id="path16"
+         d="M 0,52.49 H 52.491 V 0 H 0 Z" /></clipPath></defs><g
+     transform="matrix(1.3333333,0,0,-1.3333333,0,69.9872)"
+     id="g10"><g
+       id="g12"><g
+         clip-path="url(#clipPath18)"
+         id="g14"><g
+           transform="translate(35.6531,35.3361)"
+           id="g20"><path
+             id="path22"
+             style="fill:white;fill-opacity:0.5;fill-rule:nonzero;stroke:none"
+             d="M 0,0 C 0.279,0.322 0.5,0.686 0.657,1.081 0.827,1.513 0.914,1.968 0.917,2.434 0.92,2.924 0.829,3.403 0.647,3.857 0.458,4.329 0.165,4.77 -0.2,5.13 -0.587,5.512 -1.061,5.812 -1.571,5.995 -2.138,6.198 -2.756,6.262 -3.358,6.18 -3.821,6.115 -4.263,5.967 -4.671,5.739 -4.886,5.618 -5.094,5.472 -5.291,5.305 L -5.467,5.151 C -5.527,5.099 -5.587,5.049 -5.649,5.003 -5.775,4.909 -5.906,4.827 -6.04,4.759 -6.173,4.691 -6.314,4.633 -6.458,4.586 -6.575,4.549 -6.696,4.519 -6.819,4.496 -6.917,4.478 -7.017,4.465 -7.115,4.457 c -0.623,-0.052 -1.281,0.082 -1.9,0.385 -0.224,0.111 -0.439,0.241 -0.64,0.387 -0.042,0.03 -0.084,0.063 -0.127,0.095 -0.039,0.031 -0.069,0.054 -0.102,0.09 -0.023,0.025 -0.043,0.05 -0.061,0.077 -0.056,0.082 -0.093,0.176 -0.107,0.271 -0.004,0.022 -0.005,0.044 -0.006,0.066 l -0.002,6.486 c -0.534,0.589 -1.115,1.136 -1.728,1.626 -0.913,0.73 -1.913,1.352 -2.971,1.845 -0.867,0.405 -1.779,0.725 -2.711,0.952 -0.851,0.208 -1.731,0.34 -2.617,0.391 -0.912,0.053 -1.834,0.023 -2.741,-0.092 -0.741,-0.094 -1.479,-0.246 -2.194,-0.453 -1.274,-0.365 -2.494,-0.905 -3.628,-1.604 -0.824,-0.507 -1.603,-1.101 -2.316,-1.764 -0.687,-0.64 -1.317,-1.35 -1.871,-2.109 -0.505,-0.692 -0.951,-1.432 -1.327,-2.2 -0.375,-0.764 -0.684,-1.566 -0.919,-2.383 -0.201,-0.7 -0.351,-1.423 -0.446,-2.147 -0.12,-0.919 -0.153,-1.858 -0.099,-2.79 0.05,-0.838 0.171,-1.673 0.359,-2.482 0.201,-0.856 0.481,-1.7 0.833,-2.507 0.546,-1.253 1.265,-2.423 2.136,-3.479 0.317,-0.384 0.654,-0.753 1.002,-1.098 0.158,-0.157 0.32,-0.31 0.485,-0.459 h 6.121 c 0.051,0.08 0.099,0.161 0.143,0.245 0.037,0.071 0.072,0.144 0.104,0.218 0.026,0.062 0.05,0.128 0.073,0.193 0.132,0.394 0.159,0.784 0.077,1.127 -0.013,0.052 -0.028,0.105 -0.046,0.157 -0.024,0.068 -0.053,0.136 -0.087,0.201 -0.048,0.09 -0.105,0.174 -0.169,0.249 -0.035,0.041 -0.07,0.079 -0.107,0.116 l -0.024,0.026 c -0.025,0.029 -0.046,0.053 -0.066,0.079 -0.014,0.02 -0.02,0.029 -0.027,0.038 l -0.079,0.096 c -0.109,0.134 -0.212,0.277 -0.304,0.42 -0.207,0.318 -0.378,0.659 -0.506,1.013 -0.118,0.32 -0.202,0.651 -0.252,0.985 -0.085,0.571 -0.073,1.146 0.035,1.71 0.098,0.509 0.273,0.999 0.521,1.454 0.258,0.475 0.588,0.903 0.98,1.272 0.419,0.394 0.895,0.711 1.415,0.942 0.557,0.249 1.15,0.393 1.762,0.428 0.659,0.035 1.298,-0.05 1.914,-0.258 0.653,-0.221 1.268,-0.585 1.78,-1.05 0.541,-0.492 0.977,-1.106 1.261,-1.776 0.151,-0.357 0.26,-0.729 0.324,-1.106 0.072,-0.415 0.092,-0.843 0.058,-1.272 -0.031,-0.388 -0.108,-0.773 -0.227,-1.144 -0.11,-0.341 -0.257,-0.673 -0.441,-0.988 -0.103,-0.177 -0.216,-0.347 -0.335,-0.503 -0.037,-0.051 -0.077,-0.1 -0.117,-0.151 l -0.033,-0.04 -0.005,0.002 -0.049,-0.07 c -0.022,-0.028 -0.043,-0.054 -0.066,-0.08 l -0.076,-0.08 c -0.028,-0.03 -0.055,-0.062 -0.081,-0.094 -0.05,-0.063 -0.095,-0.131 -0.134,-0.202 -0.035,-0.064 -0.066,-0.131 -0.09,-0.199 -0.019,-0.053 -0.035,-0.105 -0.049,-0.158 -0.085,-0.34 -0.062,-0.729 0.066,-1.123 0.021,-0.066 0.045,-0.13 0.071,-0.193 0.03,-0.073 0.063,-0.145 0.1,-0.215 0.045,-0.088 0.096,-0.176 0.156,-0.269 h 7.162 l 0.002,7.527 c 10e-4,0.022 0.002,0.043 0.006,0.065 0.008,0.061 0.027,0.124 0.057,0.187 0.03,0.06 0.067,0.114 0.111,0.161 0.034,0.037 0.065,0.062 0.102,0.09 l 0.062,0.048 c 0.087,0.065 0.173,0.126 0.262,0.183 0.487,0.316 1.027,0.526 1.563,0.608 0.154,0.024 0.312,0.037 0.482,0.04 0.037,0.001 0.076,0 0.113,-0.001 0.062,-0.002 0.124,-0.005 0.186,-0.01 0.123,-0.011 0.247,-0.029 0.367,-0.053 0.338,-0.071 0.654,-0.2 0.939,-0.383 0.11,-0.07 0.214,-0.149 0.31,-0.231 0.031,-0.026 0.061,-0.053 0.09,-0.081 l 0.029,-0.028 -0.07,-0.106 c 0.003,-0.003 0.007,-0.007 0.01,-0.01 l 0.071,0.101 0.215,-0.169 c 0.13,-0.101 0.243,-0.179 0.358,-0.25 0.24,-0.146 0.497,-0.266 0.763,-0.355 0.656,-0.219 1.363,-0.253 2.04,-0.097 0.374,0.086 0.731,0.229 1.059,0.424 C -0.581,-0.571 -0.268,-0.309 0,0" /></g><g
+           transform="translate(41.5882,22.8337)"
+           id="g24"><path
+             id="path26"
+             style="fill:white;fill-opacity:0.5;fill-rule:nonzero;stroke:none"
+             d="M 0,0 V 0 L 0.053,0.044 0.013,0.018 Z" /></g><g
+           transform="translate(28.5515,3.2736)"
+           id="g28"><path
+             id="path30"
+             style="fill:white;fill-opacity:0.5;fill-rule:nonzero;stroke:none"
+             d="m 0,0 c 0.934,-0.757 1.959,-1.398 3.045,-1.906 0.84,-0.39 1.72,-0.702 2.615,-0.927 0.883,-0.222 1.796,-0.362 2.714,-0.416 0.944,-0.054 1.897,-0.019 2.832,0.106 0.707,0.094 1.414,0.242 2.102,0.439 1.276,0.368 2.496,0.908 3.628,1.605 0.798,0.492 1.556,1.065 2.251,1.704 0.724,0.666 1.384,1.408 1.962,2.205 0.468,0.646 0.886,1.334 1.242,2.044 0.409,0.815 0.742,1.672 0.99,2.547 0.179,0.628 0.316,1.274 0.409,1.921 0.14,0.979 0.182,1.98 0.123,2.973 -0.048,0.819 -0.165,1.638 -0.348,2.433 -0.2,0.872 -0.484,1.732 -0.843,2.556 -0.538,1.234 -1.247,2.392 -2.106,3.442 -0.313,0.381 -0.648,0.752 -0.998,1.1 l -0.52,0.493 H 12.982 L 12.831,22.068 C 12.797,22.003 12.763,21.931 12.732,21.857 12.705,21.794 12.681,21.73 12.659,21.665 12.527,21.268 12.5,20.878 12.582,20.536 c 0.012,-0.053 0.027,-0.105 0.045,-0.156 0.024,-0.069 0.054,-0.137 0.088,-0.202 0.046,-0.086 0.103,-0.171 0.169,-0.251 0.027,-0.032 0.056,-0.062 0.086,-0.093 l 0.045,-0.048 c 0.023,-0.025 0.044,-0.051 0.065,-0.078 l -0.046,-0.082 0.061,0.057 0.003,-0.002 0.022,-0.028 -10e-4,-10e-4 0.01,-0.015 0.004,0.003 0.05,-0.062 c 0.171,-0.212 0.322,-0.433 0.451,-0.659 0.275,-0.478 0.469,-0.996 0.575,-1.538 0.141,-0.718 0.124,-1.472 -0.05,-2.181 -0.152,-0.623 -0.431,-1.228 -0.807,-1.751 -0.303,-0.42 -0.667,-0.79 -1.082,-1.1 C 11.868,12.048 11.425,11.81 10.954,11.64 10.456,11.461 9.935,11.362 9.406,11.345 8.955,11.331 8.505,11.376 8.07,11.48 c -0.494,0.118 -0.965,0.308 -1.399,0.565 -0.509,0.301 -0.958,0.685 -1.333,1.14 -0.393,0.477 -0.692,1.014 -0.89,1.596 -0.218,0.645 -0.305,1.348 -0.25,2.031 0.029,0.355 0.095,0.704 0.194,1.037 0.114,0.383 0.273,0.751 0.474,1.095 0.095,0.164 0.207,0.334 0.334,0.503 0.038,0.051 0.078,0.102 0.118,0.151 L 5.45,19.56 v 0 l -0.101,0.074 0.055,0.073 c 0.023,0.028 0.045,0.054 0.066,0.078 l 0.076,0.08 c 0.03,0.031 0.057,0.062 0.082,0.095 0.05,0.063 0.095,0.131 0.135,0.203 0.034,0.063 0.065,0.13 0.09,0.199 0.019,0.052 0.034,0.103 0.047,0.156 0.086,0.341 0.063,0.73 -0.065,1.124 -0.021,0.065 -0.044,0.129 -0.071,0.193 -0.033,0.082 -0.071,0.162 -0.113,0.24 -0.044,0.084 -0.092,0.165 -0.143,0.244 h -7.162 l -0.002,-7.532 c -0.001,-0.02 -0.003,-0.041 -0.006,-0.06 -0.015,-0.099 -0.052,-0.192 -0.107,-0.272 -0.018,-0.027 -0.039,-0.052 -0.061,-0.076 -0.035,-0.037 -0.064,-0.061 -0.102,-0.09 -0.02,-0.016 -0.042,-0.033 -0.065,-0.05 -0.122,-0.091 -0.254,-0.181 -0.393,-0.264 -0.619,-0.37 -1.29,-0.565 -1.942,-0.565 -0.093,-0.003 -0.179,0.003 -0.268,0.011 -0.11,0.01 -0.221,0.025 -0.33,0.046 -0.36,0.071 -0.709,0.213 -1.008,0.411 -0.109,0.072 -0.213,0.152 -0.309,0.237 -0.029,0.026 -0.058,0.052 -0.085,0.079 h -0.026 l -0.022,0.042 -0.036,0.027 c -0.035,0.03 -0.061,0.051 -0.087,0.072 l -0.073,0.059 c -0.212,0.162 -0.427,0.296 -0.648,0.404 -0.416,0.204 -0.861,0.328 -1.324,0.367 -0.345,0.028 -0.695,0.012 -1.037,-0.053 -0.259,-0.05 -0.511,-0.126 -0.75,-0.228 -0.498,-0.211 -0.954,-0.534 -1.32,-0.936 -0.345,-0.381 -0.614,-0.838 -0.779,-1.322 -0.154,-0.455 -0.218,-0.933 -0.191,-1.421 0.026,-0.462 0.136,-0.909 0.326,-1.327 0.175,-0.386 0.412,-0.738 0.706,-1.047 0.283,-0.296 0.609,-0.543 0.97,-0.734 0.319,-0.168 0.661,-0.289 1.015,-0.36 0.449,-0.089 0.906,-0.095 1.357,-0.021 0.246,0.041 0.49,0.108 0.725,0.198 0.254,0.098 0.499,0.225 0.727,0.377 0.113,0.075 0.229,0.161 0.343,0.257 l 0.068,0.054 0.016,0.046 0.041,0.002 c 0.012,0.013 0.02,0.02 0.029,0.028 l 0.055,0.052 c 0.051,0.043 0.1,0.084 0.151,0.124 0.104,0.08 0.212,0.151 0.322,0.213 0.28,0.159 0.589,0.268 0.917,0.324 0.152,0.026 0.31,0.04 0.48,0.043 h 0.109 C -4.1,9.429 -3.938,9.414 -3.782,9.389 -3.261,9.303 -2.734,9.095 -2.256,8.787 -2.167,8.73 -2.08,8.668 -1.996,8.604 l 0.053,-0.04 c 0.048,-0.037 0.077,-0.061 0.111,-0.097 0.047,-0.05 0.084,-0.104 0.113,-0.162 0.029,-0.062 0.048,-0.125 0.057,-0.187 0.003,-0.02 0.005,-0.04 0.006,-0.06 L -1.654,8.021 V 1.566 C -1.142,1.001 -0.585,0.474 0,0" /></g><g
+           transform="translate(22.2707,17.571)"
+           id="g32"><path
+             id="path34"
+             style="fill:white;fill-opacity:0.5;fill-rule:nonzero;stroke:none"
+             d="m 0,0 v -0.008 l 0.011,0.019 v 0 z" /></g><g
+           transform="translate(40.657,22.3049)"
+           id="g36"><path
+             id="path38"
+             style="fill:white;fill-opacity:0.5;fill-rule:nonzero;stroke:none"
+             d="M 0,0 Z" /></g><g
+           transform="translate(49.1867,28.5134)"
+           id="g40"><path
+             id="path42"
+             style="fill:white;fill-opacity:0.5;fill-rule:nonzero;stroke:none"
+             d="m 0,0 c 0.721,0.884 1.337,1.848 1.832,2.865 0.359,0.739 0.659,1.516 0.89,2.311 0.244,0.839 0.413,1.706 0.503,2.579 0.173,1.678 0.063,3.352 -0.329,4.976 -0.323,1.339 -0.834,2.625 -1.518,3.823 -0.416,0.728 -0.898,1.425 -1.432,2.072 -0.597,0.724 -1.268,1.394 -1.993,1.994 -0.636,0.525 -1.32,0.999 -2.034,1.409 -0.75,0.431 -1.539,0.796 -2.345,1.086 -1,0.359 -2.041,0.609 -3.095,0.743 -0.401,0.051 -0.808,0.086 -1.209,0.104 -0.205,0.009 -0.41,0.014 -0.615,0.015 h -0.098 c -0.479,-0.003 -0.943,-0.026 -1.379,-0.069 -0.825,-0.08 -1.65,-0.231 -2.451,-0.45 -0.861,-0.235 -1.705,-0.551 -2.509,-0.941 -0.765,-0.371 -1.502,-0.811 -2.191,-1.308 -0.571,-0.411 -1.115,-0.866 -1.618,-1.352 -0.236,-0.227 -0.471,-0.469 -0.698,-0.72 v -6.121 c 0.079,-0.052 0.161,-0.1 0.245,-0.144 0.07,-0.037 0.143,-0.071 0.216,-0.102 0.064,-0.027 0.13,-0.051 0.195,-0.074 0.394,-0.132 0.784,-0.158 1.127,-0.077 0.053,0.012 0.105,0.027 0.156,0.045 0.069,0.024 0.136,0.054 0.201,0.088 0.088,0.047 0.173,0.104 0.251,0.169 0.04,0.034 0.078,0.071 0.116,0.107 l 0.029,0.028 c 0.035,0.029 0.055,0.046 0.077,0.063 l 0.107,0.085 c 0.164,0.132 0.314,0.24 0.466,0.338 0.319,0.205 0.662,0.373 1.018,0.502 0.317,0.115 0.649,0.198 0.987,0.245 0.56,0.082 1.126,0.068 1.681,-0.038 0.51,-0.098 1,-0.273 1.455,-0.521 0.473,-0.257 0.901,-0.587 1.272,-0.981 0.394,-0.418 0.711,-0.894 0.942,-1.414 0.248,-0.558 0.392,-1.151 0.427,-1.762 0.037,-0.645 -0.052,-1.307 -0.258,-1.914 -0.22,-0.652 -0.583,-1.267 -1.049,-1.781 -0.491,-0.54 -1.105,-0.976 -1.776,-1.26 -0.352,-0.149 -0.724,-0.259 -1.106,-0.325 -0.421,-0.072 -0.849,-0.091 -1.272,-0.057 -0.371,0.03 -0.739,0.1 -1.09,0.21 -0.365,0.113 -0.716,0.267 -1.042,0.458 -0.167,0.096 -0.336,0.209 -0.503,0.334 -0.051,0.039 -0.102,0.078 -0.151,0.118 l -0.108,0.086 c -0.028,0.022 -0.053,0.043 -0.079,0.065 -0.03,0.028 -0.056,0.054 -0.082,0.079 -0.029,0.026 -0.061,0.054 -0.094,0.081 -0.064,0.05 -0.132,0.095 -0.201,0.133 -0.064,0.035 -0.132,0.065 -0.201,0.091 -0.051,0.018 -0.101,0.034 -0.154,0.046 C -20.829,6.024 -21.219,6 -21.613,5.873 -21.677,5.852 -21.742,5.828 -21.806,5.802 -21.887,5.768 -21.967,5.73 -22.045,5.689 -22.129,5.645 -22.21,5.598 -22.289,5.546 v -7.162 l 7.532,-0.003 c 0.02,-0.001 0.04,-0.002 0.06,-0.005 0.098,-0.015 0.192,-0.052 0.272,-0.107 0.026,-0.018 0.053,-0.039 0.076,-0.062 0.035,-0.033 0.058,-0.061 0.085,-0.096 l 0.053,-0.068 c 0.094,-0.125 0.184,-0.257 0.267,-0.395 0.362,-0.608 0.557,-1.269 0.563,-1.912 0.002,-0.099 -0.002,-0.198 -0.01,-0.298 -0.009,-0.11 -0.025,-0.221 -0.046,-0.33 -0.072,-0.362 -0.213,-0.711 -0.41,-1.008 -0.074,-0.11 -0.153,-0.214 -0.238,-0.309 -0.026,-0.029 -0.053,-0.057 -0.08,-0.086 l -0.025,-0.026 v 10e-4 l -0.18,-0.227 c -0.083,-0.106 -0.161,-0.219 -0.23,-0.334 -0.143,-0.233 -0.26,-0.483 -0.349,-0.744 -0.223,-0.654 -0.261,-1.359 -0.109,-2.037 0.084,-0.378 0.229,-0.742 0.429,-1.083 0.208,-0.351 0.47,-0.665 0.779,-0.932 0.323,-0.28 0.687,-0.5 1.081,-0.656 0.429,-0.17 0.884,-0.257 1.352,-0.26 h 0.026 c 0.482,0 0.951,0.09 1.397,0.269 0.473,0.19 0.913,0.483 1.274,0.848 0.383,0.388 0.682,0.862 0.865,1.371 0.203,0.566 0.267,1.184 0.184,1.786 -0.066,0.477 -0.221,0.931 -0.461,1.349 -0.117,0.204 -0.256,0.4 -0.413,0.584 l -0.073,0.089 0.005,0.01 -0.002,0.006 0.007,0.002 0.017,0.033 0.035,0.065 -0.091,-0.085 -0.053,0.057 c -0.051,0.06 -0.101,0.12 -0.148,0.182 -0.091,0.121 -0.173,0.253 -0.243,0.391 -0.067,0.13 -0.125,0.27 -0.172,0.417 -0.038,0.116 -0.068,0.239 -0.091,0.363 -0.018,0.097 -0.031,0.196 -0.039,0.294 -0.052,0.624 0.082,1.281 0.385,1.901 0.109,0.221 0.239,0.436 0.386,0.639 0.031,0.043 0.064,0.085 0.096,0.127 0.032,0.041 0.055,0.069 0.09,0.102 0.025,0.023 0.051,0.045 0.078,0.063 0.079,0.054 0.172,0.091 0.27,0.106 0.02,0.003 0.04,0.004 0.06,0.005 l 0.038,0.003 h 6.454 C -0.981,-1.113 -0.465,-0.57 0,0" /></g>
+          </g></g></g></svg>

apps/android/app/src/main/assets/index.html

@@ -0,0 +1,24 @@
+<html>
+	<head>
+		<meta name="apple-mobile-web-app-capable" content="yes" />
+		<meta name="viewport" content="width=device-width, initial-scale=1, maximum-scale=1.0, minimum-scale=1.0, user-scalable=no, minimal-ui">
+		<link rel="stylesheet" href="main.css">
+		<script src="roslib.js"></script>
+	</head>
+	<body>
+		<div class="telemetry"><span class="mode">DISCONNECTED</span></div>
+		<div class="battery"></div>
+		<div class="logo"></div>
+		<div class="container">
+			<div class="stick stick-left">
+				<div class="stick-pointer"></div>
+			</div>
+			<div class="stick stick-right">
+				<div class="stick-pointer"></div>
+			</div>
+		</div>
+		<div class="notifications"></div>
+		<script src="main.js" type="text/javascript"></script>
+		<script src="telemetry.js" type="text/javascript"></script>
+	</body>
+</html>

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

@@ -0,0 +1,125 @@
+html, body {
+	margin: 0;
+	padding: 0;
+	user-select: none;
+	font-family: sans-serif;
+	background: #212121;
+	color: rgba(255, 255, 255, 0.9);
+}
+
+* {
+	user-select: none;
+}
+
+.stick {
+	border-radius: 50%;
+	width: 5cm;
+	height: 5cm;
+	position: relative;
+	transform: translateZ(0);
+	border: 4px solid rgba(255,255,255,.4);
+	box-shadow: 0 0 0 1px rgba(0,0,0,.2), inset 0 0 0 1px rgba(0,0,0,.2);
+}
+
+.stick-pointer {
+	position: absolute;
+	border-radius: 50%;
+	background-color: rgba(255,255,255,.25);
+	box-shadow: 0 0 10px rgba(0,0,0,.3);
+	width: 3cm;
+	height: 3cm;
+	margin-left: -1.5cm;
+	margin-top: -1.5cm;
+	top: 2.5cm;
+	left: 2.5cm;
+	pointer-events: none;
+	transform: translateZ(0);
+}
+
+.container {
+	display: flex;
+	justify-content: space-around;
+	align-items: center;
+	width: 100%;
+	height: 100%;
+}
+
+.telemetry {
+	position: absolute;
+	text-align: center;
+	width: 100%;
+	top: 30px;
+	font-size: 20px;
+	user-select: none;
+	pointer-events: none;
+}
+
+body.armed .telemetry .mode {
+	font-weight: bold;
+}
+
+@keyframes scale {
+	0% { transform: scale(1.0); }
+	50% { transform: scale(1.2); }
+	100% { transform: scale(1.0); }
+}
+
+.battery {
+	position: absolute;
+	text-align: center;
+	width: 100%;
+	bottom: 30px;
+	font-size: 20px;
+	user-select: none;
+	pointer-events: none;
+}
+
+body.low-battery .battery {
+	color: #ff554b;
+	animation: scale 0.3s 1 ease-in-out
+}
+
+.logo {
+	position: absolute;
+	background: url(clever.svg);
+	-webkit-background-size: 50px;
+	background-size: 50px;
+	width: 50px;
+	height: 50px;
+	top: 50%;
+	left: 50%;
+	margin-top: -25px;
+	margin-left: -25px;
+	font-size: 20px;
+	user-select: none;
+	pointer-events: none;
+}
+
+.notifications {
+	pointer-events: none;
+	position: absolute;
+	top: 0;
+	left: 0;
+	right: 0;
+	color: white;
+}
+
+.notifications.hidden {
+	transform: translateY(-100%);
+}
+
+.notifications.anim {
+	transition: transform 0.2s ease;
+}
+
+.notifications .item {
+	font-size: 4mm;
+	-webkit-text-size-adjust: none;
+	background: #fca83a;
+	padding: 3mm;
+	padding-bottom: 1.5mm;
+}
+
+.notifications .item:last-child {
+	padding-bottom: 3mm;
+}

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

@@ -0,0 +1,138 @@
+function throttle(func, ms) {
+	var isThrottled = false,
+		savedArgs,
+		savedThis;
+
+	function wrapper() {
+		if (isThrottled) {
+			savedArgs = arguments;
+			savedThis = this;
+			return;
+		}
+		func.apply(this, arguments);
+		isThrottled = true;
+		setTimeout(function() {
+			isThrottled = false;
+			if (savedArgs) {
+				wrapper.apply(savedThis, savedArgs);
+				savedArgs = savedThis = null;
+			}
+		}, ms);
+	}
+	return wrapper;
+}
+
+function postAppMessage(msg) {
+	if (window.webkit != undefined) {
+		if (window.webkit.messageHandlers.appInterface != undefined) {
+			window.webkit.messageHandlers.appInterface.postMessage(JSON.stringify(msg));
+		}
+	} else if (window.appInterface != undefined) {
+		window.appInterface.postMessage(JSON.stringify(msg));
+	}
+}
+
+function callNativeApp(name, msg) {
+	try {
+		postAppMessage(msg);
+		return true;
+	} catch(err) {
+		console.warn('The native context does not exist yet');
+		return false;
+	}
+}
+var rcLastPublish = null;
+
+function rcPublish() {
+	callNativeApp('control', controlMessage);
+	rcLastPublish = new Date();
+}
+
+rcPublishThrottled = throttle(rcPublish, 30);
+
+setInterval(function() {
+	if (rcLastPublish !== null && new Date() - rcLastPublish > 800) {
+		rcPublishThrottled();
+	}
+}, 50);
+
+var body = document.querySelector('body');
+var stickLeft = document.querySelector('.stick-left');
+var stickRight = document.querySelector('.stick-right');
+
+var controlMessage = { x: 0, y: 0, z: 0, r: 0 };
+
+function onStickTouchMove(touch) {
+	var target = touch.target;
+	var targetRect = target.getBoundingClientRect();
+	var stickPointer = target.querySelector('.stick-pointer');
+
+	var offsetX = touch.clientX - targetRect.left;
+	var offsetY = touch.clientY - targetRect.top;
+
+	var x = 2 * offsetX / targetRect.width;
+	var y = 2 * offsetY / targetRect.height;
+
+	x = Math.max(0, x);
+	x = Math.min(2, x);
+	y = Math.max(0, y);
+	y = Math.min(2, y);
+
+	stickPointer.style.left = (x * 50) + '%';
+	stickPointer.style.top = (y * 50) + '%';
+
+	x -= 1;
+	y = 1 - y;
+
+	if (target.matches('.stick-left')) {
+		controlMessage.z = Math.round((y + 1) * 500);
+		controlMessage.r = Math.round(x * 1000);
+	} else if (target.matches('.stick-right')) {
+		controlMessage.x = Math.round(y * 1000);
+		controlMessage.y = Math.round(x * 1000);
+	}
+}
+
+body.addEventListener('touchmove', function (e) {
+	e.preventDefault();
+});
+
+function stickTouchStart(e) {
+	setControlMode();
+	callNativeApp('controlStart');
+	onStickTouchMove(e.changedTouches[0]);
+	rcPublishThrottled();
+	e.stopPropagation();
+	e.preventDefault();
+}
+
+function stickTouchMove(e) {
+	for (touch of e.changedTouches) {
+		onStickTouchMove(touch);
+	}
+	//onStickTouchMove(e.changedTouches[0]);
+	rcPublishThrottled();
+	e.stopPropagation();
+	e.preventDefault();
+}
+
+function stickTouchEnd(e) {
+	var pointer = e.target.querySelector('.stick-pointer');
+	if (e.target.matches('.stick-left')) {
+		controlMessage.r = 0;
+		pointer.style.left = '50%';
+	} else if (e.target.matches('.stick-right')) {
+		controlMessage.x = 0;
+		controlMessage.y = 0;
+		pointer.style.left = '50%';
+		pointer.style.top = '50%';
+	}
+	rcPublishThrottled();
+}
+
+stickLeft.addEventListener('touchmove', stickTouchMove);
+stickRight.addEventListener('touchmove', stickTouchMove);
+stickLeft.addEventListener('touchstart', stickTouchStart);
+stickRight.addEventListener('touchstart', stickTouchStart);
+stickLeft.addEventListener('touchend', stickTouchEnd);
+stickRight.addEventListener('touchend', stickTouchEnd);

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

@@ -0,0 +1,142 @@
+function throttle(func, ms) {
+	var isThrottled = false,
+		savedArgs,
+		savedThis;
+
+	function wrapper() {
+		if (isThrottled) {
+			savedArgs = arguments;
+			savedThis = this;
+			return;
+		}
+		func.apply(this, arguments);
+		isThrottled = true;
+		setTimeout(function() {
+			isThrottled = false;
+			if (savedArgs) {
+				wrapper.apply(savedThis, savedArgs);
+				savedArgs = savedThis = null;
+			}
+		}, ms);
+	}
+	return wrapper;
+}
+
+function postAppMessage(msg) {
+    if (window.webkit != undefined) {
+        if (window.webkit.messageHandlers.appInterface != undefined) {
+            window.webkit.messageHandlers.appInterface.postMessage(JSON.stringify(msg));
+        }
+    }
+    else if (window.appInterface != undefined) {
+        window.appInterface.postMessage(JSON.stringify(msg));
+    }
+}
+
+function callNativeApp(name, msg) {
+	try {
+        postAppMessage(msg);
+		return true;
+	} catch(err) {
+		console.warn('The native context does not exist yet');
+		return false;
+	}
+}
+
+
+
+var rcLastPublish = null;
+
+function rcPublish() {
+	callNativeApp('control', controlMessage);
+	rcLastPublish = new Date();
+}
+
+rcPublishThrottled = throttle(rcPublish, 30);
+
+setInterval(function() {
+	if (rcLastPublish !== null && new Date() - rcLastPublish > 800) {
+		rcPublishThrottled();
+	}
+}, 50);
+
+var body = document.querySelector('body');
+var stickLeft = document.querySelector('.stick-left');
+var stickRight = document.querySelector('.stick-right');
+
+var controlMessage = { x: 0, y: 0, z: 0, r: 0 };
+
+function onStickTouchMove(touch) {
+	var target = touch.target;
+	var targetRect = target.getBoundingClientRect();
+	var stickPointer = target.querySelector('.stick-pointer');
+
+	var offsetX = touch.clientX - targetRect.left;
+	var offsetY = touch.clientY - targetRect.top;
+
+	var x = 2 * offsetX / targetRect.width;
+	var y = 2 * offsetY / targetRect.height;
+
+	x = Math.max(0, x);
+	x = Math.min(2, x);
+	y = Math.max(0, y);
+	y = Math.min(2, y);
+
+	stickPointer.style.left = (x * 50) + '%';
+	stickPointer.style.top = (y * 50) + '%';
+
+	x -= 1;
+	y = 1 - y;
+
+	if (target.matches('.stick-left')) {
+		controlMessage.z = Math.round((y + 1) * 500);
+		controlMessage.r = Math.round(x * 1000);
+	} else if (target.matches('.stick-right')) {
+		controlMessage.x = Math.round(y * 1000);
+		controlMessage.y = Math.round(x * 1000);
+	}
+}
+
+body.addEventListener('touchmove', function (e) {
+	e.preventDefault();
+});
+
+function stickTouchStart(e) {
+	setControlMode();
+	callNativeApp('controlStart');
+	onStickTouchMove(e.changedTouches[0]);
+	rcPublishThrottled();
+	e.stopPropagation();
+	e.preventDefault();
+}
+
+function stickTouchMove(e) {
+    for (touch of e.changedTouches) {
+        onStickTouchMove(touch);
+    }
+	//onStickTouchMove(e.changedTouches[0]);
+	rcPublishThrottled();
+	e.stopPropagation();
+	e.preventDefault();
+}
+
+function stickTouchEnd(e) {
+	var pointer = e.target.querySelector('.stick-pointer');
+	if (e.target.matches('.stick-left')) {
+		controlMessage.r = 0;
+		pointer.style.left = '50%';
+	} else if (e.target.matches('.stick-right')) {
+		controlMessage.x = 0;
+		controlMessage.y = 0;
+		pointer.style.left = '50%';
+		pointer.style.top = '50%';
+	}
+	rcPublishThrottled();
+}
+
+stickLeft.addEventListener('touchmove', stickTouchMove);
+stickRight.addEventListener('touchmove', stickTouchMove);
+stickLeft.addEventListener('touchstart', stickTouchStart);
+stickRight.addEventListener('touchstart', stickTouchStart);
+stickLeft.addEventListener('touchend', stickTouchEnd);
+stickRight.addEventListener('touchend', stickTouchEnd);

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

@@ -0,0 +1,115 @@
+var url = 'ws://192.168.11.1:9090';
+var modeEl = document.querySelector('.telemetry .mode');
+var batteryEl = document.querySelector('.battery');
+var notificationsEl = document.querySelector('.notifications');
+
+var ros = new ROSLIB.Ros({ url: url });
+
+ros.on('connection', function () {
+	body.classList.add('connected');
+});
+
+ros.on('close', function () {
+	body.classList.remove('connected');
+	modeEl.classList.remove('armed');
+	modeEl.innerHTML = 'DISCONNECTED';
+	batteryEl.innerHTML = '';
+	setTimeout(function() {
+		modeEl.innerHTML = 'RECONNECTING';
+		ros.connect(url);
+	}, 2000);
+});
+
+var fcuState;
+
+new ROSLIB.Topic({
+	ros: ros,
+	name: '/state_latched',
+	messageType: 'mavros_msgs/State'
+}).subscribe(function(message) {
+	body.classList.toggle('fcu-disconnected', !message.connected);
+	body.classList.toggle('armed', message.armed);
+	fcuState = message;
+	modeEl.classList.toggle('armed', fcuState.armed);
+	modeEl.innerHTML = message.connected ? fcuState.mode : 'DISCONNECTED FROM FCU';
+	console.log('state', message);
+});
+
+function notifyLowBattery() {
+	console.log('low battery');
+	callNativeApp('lowBattery');
+	body.classList.remove('low-battery');
+	void body.offsetWidth; // trick for repeating animation
+	body.classList.add('low-battery');
+}
+
+notifyLowBatteryThrottled = throttle(notifyLowBattery, 15000);
+
+new ROSLIB.Topic({
+	ros: ros,
+	name: '/mavros/battery',
+	messageType: 'sensor_msgs/BatteryState',
+	throttle_rate: 5000
+}).subscribe(function(message) {
+	var LOW_BATTERY = 3.8;
+	batteryEl.innerHTML = (message.cell_voltage[0].toFixed(2) + ' V') || '';
+
+	if (message.cell_voltage[0] < LOW_BATTERY) {
+		notifyLowBatteryThrottled();
+	} else {
+		body.classList.remove('low-battery');
+	}
+});
+
+var notificationHideTimer;
+
+function notify(text, severity) {
+	var item = document.createElement('div');
+	item.innerHTML = text;
+	item.classList.add('item');
+	notificationsEl.prepend(item);
+	var itemHeight = item.offsetHeight;
+	notificationsEl.classList.remove('anim');
+	notificationsEl.style.transform = 'translateY(' + -itemHeight + 'px)';
+	setTimeout(function() {
+		notificationsEl.classList.add('anim');
+		notificationsEl.style.transform = 'translateY(0)';
+	}, 0);
+	clearTimeout(notificationHideTimer);
+	notificationHideTimer = setTimeout(function() {
+		notificationsEl.style.transform = '';
+		notificationsEl.classList.add('hidden');
+		setTimeout(function() {
+			notificationsEl.innerHTML = '';
+		}, 210);
+	}, 4000);
+}
+
+new ROSLIB.Topic({
+	ros: ros,
+	name: '/mavros/statustext/recv',
+	messageType: 'mavros_msgs/StatusText'
+}).subscribe(function(message) {
+	var BLACKLIST = ['CMD: ', 'PR: ', 'DROPPED', 'Clock skew detected', 'MANUAL CONTROL LOST'];
+	if (message.severity <= 4) {
+		if (BLACKLIST.some(function(e) {
+				return message.text.indexOf(e) != -1;
+			})) {
+			console.log('Filtered out message ' + message.text);
+			return;
+		}
+		notify(message.text, message.severity);
+		callNativeApp('notification', message);
+	}
+});
+
+var setMode = new ROSLIB.Service({
+	ros: ros,
+	name : '/mavros/set_mode',
+	serviceType : 'mavros_msgs/SetMode'
+});
+
+function setControlMode() {
+	var CONTROL_MODE = 'STABILIZED';
+	setMode.callService(new ROSLIB.ServiceRequest({ custom_mode: CONTROL_MODE }));
+}

apps/android/app/src/main/java/express/copter/cleverrc/MainActivity.kt

@@ -0,0 +1,86 @@
+package express.copter.cleverrc
+
+import android.content.Context
+import android.os.Build
+import android.support.v7.app.AppCompatActivity
+import android.os.Bundle
+import android.view.View
+import android.view.WindowManager
+import android.webkit.JavascriptInterface
+import kotlinx.android.synthetic.main.activity_main.*
+import org.json.JSONObject
+import java.net.DatagramPacket
+import java.net.DatagramSocket
+import java.net.InetAddress
+import java.nio.ByteBuffer
+
+fun pack(x: Short, y: Short, z: Short, r: Short): ByteArray {
+    val pump_on_buf: ByteBuffer = ByteBuffer.allocate(8)
+    pump_on_buf.putShort(r)
+    pump_on_buf.putShort(z)
+    pump_on_buf.putShort(y)
+    pump_on_buf.putShort(x)
+
+    return pump_on_buf.array().reversedArray()
+}
+
+fun send(host: String, port: Int, data: ByteArray, senderPort: Int = 0): Boolean {
+    var ret = false
+    var socket: DatagramSocket? = null
+    try {
+        socket = DatagramSocket(senderPort)
+        val address = InetAddress.getByName(host)
+        val packet = DatagramPacket(data, data.size, address, port)
+        socket.send(packet)
+        ret = true
+    } catch (e: Exception) {
+        e.printStackTrace()
+    } finally {
+        socket?.close()
+    }
+    return ret
+}
+
+class MainActivity : AppCompatActivity() {
+
+    override fun onCreate(savedInstanceState: Bundle?) {
+        super.onCreate(savedInstanceState)
+        setContentView(R.layout.activity_main)
+        fullScreenCall()
+        main_web.loadUrl("file:///android_asset/index.html")
+
+        main_web.settings.apply {
+            domStorageEnabled = true
+            javaScriptEnabled = true
+            loadWithOverviewMode = true
+            useWideViewPort = true
+            setSupportZoom(false)
+        }
+
+        main_web.addJavascriptInterface(WebAppInterface(this), "appInterface")
+    }
+
+    private fun fullScreenCall() {
+        window.setFlags(
+            WindowManager.LayoutParams.FLAG_FULLSCREEN,
+            WindowManager.LayoutParams.FLAG_FULLSCREEN
+        )
+        if (Build.VERSION.SDK_INT < 19) {
+            val v = this.window.decorView
+            v.systemUiVisibility = View.GONE
+        } else {
+            //for higher api versions.
+            val decorView = window.decorView
+            val uiOptions = View.SYSTEM_UI_FLAG_HIDE_NAVIGATION or View.SYSTEM_UI_FLAG_IMMERSIVE_STICKY
+            decorView.systemUiVisibility = uiOptions
+        }
+    }
+}
+
+class WebAppInterface(c: Context) {
+    @JavascriptInterface
+    public fun postMessage(message: String) {
+        val data = JSONObject(message)
+        send("255.255.255.255", 35602, pack(data.getInt("x").toShort(), data.getInt("y").toShort(), data.getInt("z").toShort(), data.getInt("r").toShort()))
+    }
+}

apps/android/app/src/main/res/drawable-v24/ic_launcher_foreground.xml

@@ -0,0 +1,34 @@
+<vector xmlns:android="http://schemas.android.com/apk/res/android"
+        xmlns:aapt="http://schemas.android.com/aapt"
+        android:width="108dp"
+        android:height="108dp"
+        android:viewportHeight="108"
+        android:viewportWidth="108">
+    <path
+            android:fillType="evenOdd"
+            android:pathData="M32,64C32,64 38.39,52.99 44.13,50.95C51.37,48.37 70.14,49.57 70.14,49.57L108.26,87.69L108,109.01L75.97,107.97L32,64Z"
+            android:strokeColor="#00000000"
+            android:strokeWidth="1">
+        <aapt:attr name="android:fillColor">
+            <gradient
+                    android:endX="78.5885"
+                    android:endY="90.9159"
+                    android:startX="48.7653"
+                    android:startY="61.0927"
+                    android:type="linear">
+                <item
+                        android:color="#44000000"
+                        android:offset="0.0"/>
+                <item
+                        android:color="#00000000"
+                        android:offset="1.0"/>
+            </gradient>
+        </aapt:attr>
+    </path>
+    <path
+            android:fillColor="#FFFFFF"
+            android:fillType="nonZero"
+            android:pathData="M66.94,46.02L66.94,46.02C72.44,50.07 76,56.61 76,64L32,64C32,56.61 35.56,50.11 40.98,46.06L36.18,41.19C35.45,40.45 35.45,39.3 36.18,38.56C36.91,37.81 38.05,37.81 38.78,38.56L44.25,44.05C47.18,42.57 50.48,41.71 54,41.71C57.48,41.71 60.78,42.57 63.68,44.05L69.11,38.56C69.84,37.81 70.98,37.81 71.71,38.56C72.44,39.3 72.44,40.45 71.71,41.19L66.94,46.02ZM62.94,56.92C64.08,56.92 65,56.01 65,54.88C65,53.76 64.08,52.85 62.94,52.85C61.8,52.85 60.88,53.76 60.88,54.88C60.88,56.01 61.8,56.92 62.94,56.92ZM45.06,56.92C46.2,56.92 47.13,56.01 47.13,54.88C47.13,53.76 46.2,52.85 45.06,52.85C43.92,52.85 43,53.76 43,54.88C43,56.01 43.92,56.92 45.06,56.92Z"
+            android:strokeColor="#00000000"
+            android:strokeWidth="1"/>
+</vector>

apps/android/app/src/main/res/drawable/ic_launcher_background.xml

@@ -0,0 +1,74 @@
+<?xml version="1.0" encoding="utf-8"?>
+<vector
+        xmlns:android="http://schemas.android.com/apk/res/android"
+        android:height="108dp"
+        android:width="108dp"
+        android:viewportHeight="108"
+        android:viewportWidth="108">
+    <path android:fillColor="#008577"
+          android:pathData="M0,0h108v108h-108z"/>
+    <path android:fillColor="#00000000" android:pathData="M9,0L9,108"
+          android:strokeColor="#33FFFFFF" android:strokeWidth="0.8"/>
+    <path android:fillColor="#00000000" android:pathData="M19,0L19,108"
+          android:strokeColor="#33FFFFFF" android:strokeWidth="0.8"/>
+    <path android:fillColor="#00000000" android:pathData="M29,0L29,108"
+          android:strokeColor="#33FFFFFF" android:strokeWidth="0.8"/>
+    <path android:fillColor="#00000000" android:pathData="M39,0L39,108"
+          android:strokeColor="#33FFFFFF" android:strokeWidth="0.8"/>
+    <path android:fillColor="#00000000" android:pathData="M49,0L49,108"
+          android:strokeColor="#33FFFFFF" android:strokeWidth="0.8"/>
+    <path android:fillColor="#00000000" android:pathData="M59,0L59,108"
+          android:strokeColor="#33FFFFFF" android:strokeWidth="0.8"/>
+    <path android:fillColor="#00000000" android:pathData="M69,0L69,108"
+          android:strokeColor="#33FFFFFF" android:strokeWidth="0.8"/>
+    <path android:fillColor="#00000000" android:pathData="M79,0L79,108"
+          android:strokeColor="#33FFFFFF" android:strokeWidth="0.8"/>
+    <path android:fillColor="#00000000" android:pathData="M89,0L89,108"
+          android:strokeColor="#33FFFFFF" android:strokeWidth="0.8"/>
+    <path android:fillColor="#00000000" android:pathData="M99,0L99,108"
+          android:strokeColor="#33FFFFFF" android:strokeWidth="0.8"/>
+    <path android:fillColor="#00000000" android:pathData="M0,9L108,9"
+          android:strokeColor="#33FFFFFF" android:strokeWidth="0.8"/>
+    <path android:fillColor="#00000000" android:pathData="M0,19L108,19"
+          android:strokeColor="#33FFFFFF" android:strokeWidth="0.8"/>
+    <path android:fillColor="#00000000" android:pathData="M0,29L108,29"
+          android:strokeColor="#33FFFFFF" android:strokeWidth="0.8"/>
+    <path android:fillColor="#00000000" android:pathData="M0,39L108,39"
+          android:strokeColor="#33FFFFFF" android:strokeWidth="0.8"/>
+    <path android:fillColor="#00000000" android:pathData="M0,49L108,49"
+          android:strokeColor="#33FFFFFF" android:strokeWidth="0.8"/>
+    <path android:fillColor="#00000000" android:pathData="M0,59L108,59"
+          android:strokeColor="#33FFFFFF" android:strokeWidth="0.8"/>
+    <path android:fillColor="#00000000" android:pathData="M0,69L108,69"
+          android:strokeColor="#33FFFFFF" android:strokeWidth="0.8"/>
+    <path android:fillColor="#00000000" android:pathData="M0,79L108,79"
+          android:strokeColor="#33FFFFFF" android:strokeWidth="0.8"/>
+    <path android:fillColor="#00000000" android:pathData="M0,89L108,89"
+          android:strokeColor="#33FFFFFF" android:strokeWidth="0.8"/>
+    <path android:fillColor="#00000000" android:pathData="M0,99L108,99"
+          android:strokeColor="#33FFFFFF" android:strokeWidth="0.8"/>
+    <path android:fillColor="#00000000" android:pathData="M19,29L89,29"
+          android:strokeColor="#33FFFFFF" android:strokeWidth="0.8"/>
+    <path android:fillColor="#00000000" android:pathData="M19,39L89,39"
+          android:strokeColor="#33FFFFFF" android:strokeWidth="0.8"/>
+    <path android:fillColor="#00000000" android:pathData="M19,49L89,49"
+          android:strokeColor="#33FFFFFF" android:strokeWidth="0.8"/>
+    <path android:fillColor="#00000000" android:pathData="M19,59L89,59"
+          android:strokeColor="#33FFFFFF" android:strokeWidth="0.8"/>
+    <path android:fillColor="#00000000" android:pathData="M19,69L89,69"
+          android:strokeColor="#33FFFFFF" android:strokeWidth="0.8"/>
+    <path android:fillColor="#00000000" android:pathData="M19,79L89,79"
+          android:strokeColor="#33FFFFFF" android:strokeWidth="0.8"/>
+    <path android:fillColor="#00000000" android:pathData="M29,19L29,89"
+          android:strokeColor="#33FFFFFF" android:strokeWidth="0.8"/>
+    <path android:fillColor="#00000000" android:pathData="M39,19L39,89"
+          android:strokeColor="#33FFFFFF" android:strokeWidth="0.8"/>
+    <path android:fillColor="#00000000" android:pathData="M49,19L49,89"
+          android:strokeColor="#33FFFFFF" android:strokeWidth="0.8"/>
+    <path android:fillColor="#00000000" android:pathData="M59,19L59,89"
+          android:strokeColor="#33FFFFFF" android:strokeWidth="0.8"/>
+    <path android:fillColor="#00000000" android:pathData="M69,19L69,89"
+          android:strokeColor="#33FFFFFF" android:strokeWidth="0.8"/>
+    <path android:fillColor="#00000000" android:pathData="M79,19L79,89"
+          android:strokeColor="#33FFFFFF" android:strokeWidth="0.8"/>
+</vector>

apps/android/app/src/main/res/layout/activity_main.xml

@@ -0,0 +1,14 @@
+<?xml version="1.0" encoding="utf-8"?>
+<FrameLayout
+        xmlns:android="http://schemas.android.com/apk/res/android"
+        xmlns:tools="http://schemas.android.com/tools"
+        xmlns:app="http://schemas.android.com/apk/res-auto"
+        android:layout_width="match_parent"
+        android:layout_height="match_parent"
+        tools:context=".MainActivity">
+
+    <WebView
+            android:layout_width="match_parent"
+            android:layout_height="match_parent"
+            android:id="@+id/main_web"/>
+</FrameLayout>

apps/android/app/src/main/res/mipmap-anydpi-v26/ic_launcher.xml

@@ -0,0 +1,5 @@
+<?xml version="1.0" encoding="utf-8"?>
+<adaptive-icon xmlns:android="http://schemas.android.com/apk/res/android">
+    <background android:drawable="@color/ic_launcher_background"/>
+    <foreground android:drawable="@mipmap/ic_launcher_foreground"/>
+</adaptive-icon>

apps/android/app/src/main/res/mipmap-anydpi-v26/ic_launcher_round.xml

@@ -0,0 +1,5 @@
+<?xml version="1.0" encoding="utf-8"?>
+<adaptive-icon xmlns:android="http://schemas.android.com/apk/res/android">
+    <background android:drawable="@color/ic_launcher_background"/>
+    <foreground android:drawable="@mipmap/ic_launcher_foreground"/>
+</adaptive-icon>

apps/android/app/src/main/res/values/colors.xml

@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="utf-8"?>
+<resources>
+    <color name="colorPrimary">#fafafa</color>
+    <color name="colorPrimaryDark">#d1d1d1</color>
+    <color name="colorAccent">#757575</color>
+</resources>

apps/android/app/src/main/res/values/ic_launcher_background.xml

@@ -0,0 +1,4 @@
+<?xml version="1.0" encoding="utf-8"?>
+<resources>
+    <color name="ic_launcher_background">#FFFFFF</color>
+</resources>

apps/android/app/src/main/res/values/strings.xml

@@ -0,0 +1,3 @@
+<resources>
+    <string name="app_name">CLEVER RC</string>
+</resources>

apps/android/app/src/main/res/values/styles.xml

@@ -0,0 +1,18 @@
+<resources>
+
+    <!-- Base application theme. -->
+    <style name="AppTheme" parent="Theme.AppCompat.Light.DarkActionBar">
+        <!-- Customize your theme here. -->
+        <item name="colorPrimary">@color/colorPrimary</item>
+        <item name="colorPrimaryDark">@color/colorPrimaryDark</item>
+        <item name="colorAccent">@color/colorAccent</item>
+    </style>
+
+    <style name="NoUiAppTheme"
+           parent="Theme.AppCompat.NoActionBar">
+        <item name="colorPrimary">@color/colorPrimary</item>
+        <item name="colorPrimaryDark">@color/colorPrimaryDark</item>
+        <item name="colorAccent">@color/colorAccent</item>
+    </style>
+
+</resources>

apps/android/app/src/test/java/express/copter/cleverrc/ExampleUnitTest.kt

@@ -0,0 +1,17 @@
+package express.copter.cleverrc
+
+import org.junit.Test
+
+import org.junit.Assert.*
+
+/**
+ * Example local unit test, which will execute on the development machine (host).
+ *
+ * See [testing documentation](http://d.android.com/tools/testing).
+ */
+class ExampleUnitTest {
+    @Test
+    fun addition_isCorrect() {
+        assertEquals(4, 2 + 2)
+    }
+}

apps/android/build.gradle

@@ -0,0 +1,27 @@
+// Top-level build file where you can add configuration options common to all sub-projects/modules.
+
+buildscript {
+    ext.kotlin_version = '1.2.71'
+    repositories {
+        google()
+        jcenter()
+    }
+    dependencies {
+        classpath 'com.android.tools.build:gradle:3.2.1'
+        classpath "org.jetbrains.kotlin:kotlin-gradle-plugin:$kotlin_version"
+
+        // NOTE: Do not place your application dependencies here; they belong
+        // in the individual module build.gradle files
+    }
+}
+
+allprojects {
+    repositories {
+        google()
+        jcenter()
+    }
+}
+
+task clean(type: Delete) {
+    delete rootProject.buildDir
+}

apps/android/gradle.properties

@@ -0,0 +1,15 @@
+# Project-wide Gradle settings.
+# IDE (e.g. Android Studio) users:
+# Gradle settings configured through the IDE *will override*
+# any settings specified in this file.
+# For more details on how to configure your build environment visit
+# http://www.gradle.org/docs/current/userguide/build_environment.html
+# Specifies the JVM arguments used for the daemon process.
+# The setting is particularly useful for tweaking memory settings.
+org.gradle.jvmargs=-Xmx1536m
+# When configured, Gradle will run in incubating parallel mode.
+# This option should only be used with decoupled projects. More details, visit
+# http://www.gradle.org/docs/current/userguide/multi_project_builds.html#sec:decoupled_projects
+# org.gradle.parallel=true
+# Kotlin code style for this project: "official" or "obsolete":
+kotlin.code.style=official

apps/android/gradle/wrapper/gradle-wrapper.properties

@@ -0,0 +1,5 @@
+distributionBase=GRADLE_USER_HOME
+distributionPath=wrapper/dists
+distributionUrl=https\://services.gradle.org/distributions/gradle-4.6-all.zip
+zipStoreBase=GRADLE_USER_HOME
+zipStorePath=wrapper/dists

apps/android/gradlew

@@ -0,0 +1,172 @@
+#!/usr/bin/env sh
+
+##############################################################################
+##
+##  Gradle start up script for UN*X
+##
+##############################################################################
+
+# Attempt to set APP_HOME
+# Resolve links: $0 may be a link
+PRG="$0"
+# Need this for relative symlinks.
+while [ -h "$PRG" ] ; do
+    ls=`ls -ld "$PRG"`
+    link=`expr "$ls" : '.*-> \(.*\)$'`
+    if expr "$link" : '/.*' > /dev/null; then
+        PRG="$link"
+    else
+        PRG=`dirname "$PRG"`"/$link"
+    fi
+done
+SAVED="`pwd`"
+cd "`dirname \"$PRG\"`/" >/dev/null
+APP_HOME="`pwd -P`"
+cd "$SAVED" >/dev/null
+
+APP_NAME="Gradle"
+APP_BASE_NAME=`basename "$0"`
+
+# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
+DEFAULT_JVM_OPTS=""
+
+# Use the maximum available, or set MAX_FD != -1 to use that value.
+MAX_FD="maximum"
+
+warn () {
+    echo "$*"
+}
+
+die () {
+    echo
+    echo "$*"
+    echo
+    exit 1
+}
+
+# OS specific support (must be 'true' or 'false').
+cygwin=false
+msys=false
+darwin=false
+nonstop=false
+case "`uname`" in
+  CYGWIN* )
+    cygwin=true
+    ;;
+  Darwin* )
+    darwin=true
+    ;;
+  MINGW* )
+    msys=true
+    ;;
+  NONSTOP* )
+    nonstop=true
+    ;;
+esac
+
+CLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar
+
+# Determine the Java command to use to start the JVM.
+if [ -n "$JAVA_HOME" ] ; then
+    if [ -x "$JAVA_HOME/jre/sh/java" ] ; then
+        # IBM's JDK on AIX uses strange locations for the executables
+        JAVACMD="$JAVA_HOME/jre/sh/java"
+    else
+        JAVACMD="$JAVA_HOME/bin/java"
+    fi
+    if [ ! -x "$JAVACMD" ] ; then
+        die "ERROR: JAVA_HOME is set to an invalid directory: $JAVA_HOME
+
+Please set the JAVA_HOME variable in your environment to match the
+location of your Java installation."
+    fi
+else
+    JAVACMD="java"
+    which java >/dev/null 2>&1 || die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH.
+
+Please set the JAVA_HOME variable in your environment to match the
+location of your Java installation."
+fi
+
+# Increase the maximum file descriptors if we can.
+if [ "$cygwin" = "false" -a "$darwin" = "false" -a "$nonstop" = "false" ] ; then
+    MAX_FD_LIMIT=`ulimit -H -n`
+    if [ $? -eq 0 ] ; then
+        if [ "$MAX_FD" = "maximum" -o "$MAX_FD" = "max" ] ; then
+            MAX_FD="$MAX_FD_LIMIT"
+        fi
+        ulimit -n $MAX_FD
+        if [ $? -ne 0 ] ; then
+            warn "Could not set maximum file descriptor limit: $MAX_FD"
+        fi
+    else
+        warn "Could not query maximum file descriptor limit: $MAX_FD_LIMIT"
+    fi
+fi
+
+# For Darwin, add options to specify how the application appears in the dock
+if $darwin; then
+    GRADLE_OPTS="$GRADLE_OPTS \"-Xdock:name=$APP_NAME\" \"-Xdock:icon=$APP_HOME/media/gradle.icns\""
+fi
+
+# For Cygwin, switch paths to Windows format before running java
+if $cygwin ; then
+    APP_HOME=`cygpath --path --mixed "$APP_HOME"`
+    CLASSPATH=`cygpath --path --mixed "$CLASSPATH"`
+    JAVACMD=`cygpath --unix "$JAVACMD"`
+
+    # We build the pattern for arguments to be converted via cygpath
+    ROOTDIRSRAW=`find -L / -maxdepth 1 -mindepth 1 -type d 2>/dev/null`
+    SEP=""
+    for dir in $ROOTDIRSRAW ; do
+        ROOTDIRS="$ROOTDIRS$SEP$dir"
+        SEP="|"
+    done
+    OURCYGPATTERN="(^($ROOTDIRS))"
+    # Add a user-defined pattern to the cygpath arguments
+    if [ "$GRADLE_CYGPATTERN" != "" ] ; then
+        OURCYGPATTERN="$OURCYGPATTERN|($GRADLE_CYGPATTERN)"
+    fi
+    # Now convert the arguments - kludge to limit ourselves to /bin/sh
+    i=0
+    for arg in "$@" ; do
+        CHECK=`echo "$arg"|egrep -c "$OURCYGPATTERN" -`
+        CHECK2=`echo "$arg"|egrep -c "^-"`                                 ### Determine if an option
+
+        if [ $CHECK -ne 0 ] && [ $CHECK2 -eq 0 ] ; then                    ### Added a condition
+            eval `echo args$i`=`cygpath --path --ignore --mixed "$arg"`
+        else
+            eval `echo args$i`="\"$arg\""
+        fi
+        i=$((i+1))
+    done
+    case $i in
+        (0) set -- ;;
+        (1) set -- "$args0" ;;
+        (2) set -- "$args0" "$args1" ;;
+        (3) set -- "$args0" "$args1" "$args2" ;;
+        (4) set -- "$args0" "$args1" "$args2" "$args3" ;;
+        (5) set -- "$args0" "$args1" "$args2" "$args3" "$args4" ;;
+        (6) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" ;;
+        (7) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" ;;
+        (8) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" "$args7" ;;
+        (9) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" "$args7" "$args8" ;;
+    esac
+fi
+
+# Escape application args
+save () {
+    for i do printf %s\\n "$i" | sed "s/'/'\\\\''/g;1s/^/'/;\$s/\$/' \\\\/" ; done
+    echo " "
+}
+APP_ARGS=$(save "$@")
+
+# Collect all arguments for the java command, following the shell quoting and substitution rules
+eval set -- $DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS "\"-Dorg.gradle.appname=$APP_BASE_NAME\"" -classpath "\"$CLASSPATH\"" org.gradle.wrapper.GradleWrapperMain "$APP_ARGS"
+
+# by default we should be in the correct project dir, but when run from Finder on Mac, the cwd is wrong
+if [ "$(uname)" = "Darwin" ] && [ "$HOME" = "$PWD" ]; then
+  cd "$(dirname "$0")"
+fi
+
+exec "$JAVACMD" "$@"

apps/android/gradlew.bat

@@ -0,0 +1,84 @@
+@if "%DEBUG%" == "" @echo off
+@rem ##########################################################################
+@rem
+@rem  Gradle startup script for Windows
+@rem
+@rem ##########################################################################
+
+@rem Set local scope for the variables with windows NT shell
+if "%OS%"=="Windows_NT" setlocal
+
+set DIRNAME=%~dp0
+if "%DIRNAME%" == "" set DIRNAME=.
+set APP_BASE_NAME=%~n0
+set APP_HOME=%DIRNAME%
+
+@rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
+set DEFAULT_JVM_OPTS=
+
+@rem Find java.exe
+if defined JAVA_HOME goto findJavaFromJavaHome
+
+set JAVA_EXE=java.exe
+%JAVA_EXE% -version >NUL 2>&1
+if "%ERRORLEVEL%" == "0" goto init
+
+echo.
+echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH.
+echo.
+echo Please set the JAVA_HOME variable in your environment to match the
+echo location of your Java installation.
+
+goto fail
+
+:findJavaFromJavaHome
+set JAVA_HOME=%JAVA_HOME:"=%
+set JAVA_EXE=%JAVA_HOME%/bin/java.exe
+
+if exist "%JAVA_EXE%" goto init
+
+echo.
+echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME%
+echo.
+echo Please set the JAVA_HOME variable in your environment to match the
+echo location of your Java installation.
+
+goto fail
+
+:init
+@rem Get command-line arguments, handling Windows variants
+
+if not "%OS%" == "Windows_NT" goto win9xME_args
+
+:win9xME_args
+@rem Slurp the command line arguments.
+set CMD_LINE_ARGS=
+set _SKIP=2
+
+:win9xME_args_slurp
+if "x%~1" == "x" goto execute
+
+set CMD_LINE_ARGS=%*
+
+:execute
+@rem Setup the command line
+
+set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar
+
+@rem Execute Gradle
+"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %CMD_LINE_ARGS%
+
+:end
+@rem End local scope for the variables with windows NT shell
+if "%ERRORLEVEL%"=="0" goto mainEnd
+
+:fail
+rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of
+rem the _cmd.exe /c_ return code!
+if  not "" == "%GRADLE_EXIT_CONSOLE%" exit 1
+exit /b 1
+
+:mainEnd
+if "%OS%"=="Windows_NT" endlocal
+
+:omega

apps/android/settings.gradle

@@ -0,0 +1 @@
+include ':app'

apps/ios/README.md

@@ -1,5 +1,4 @@
-iOS-приложение для управления Клевером
---------------------------------------
+# iOS-приложение для управления Клевером
 
 Для установки зависимостей необходим [CocoaPods](https://cocoapods.org):
 
@@ -8,3 +7,11 @@ pod install
 ```
 
 Для разработки и сборки откройте в XCode файл `cleverrc.xcworkspace`.
+
+## Политика конфиденциальности
+
+App Store приложение CLEVER RC не собирает и не хранит каких-либо личных данных пользователя.
+
+## Privacy policy
+
+The App Store app CLEVER RC does not collect and store any personal user data.

apps/ios/cleverrc/Assets.xcassets/AppIcon.appiconset/Contents.json

@@ -145,8 +145,15 @@
       "size" : "44x44",
       "idiom" : "watch",
       "scale" : "2x",
-      "role" : "longLook",
-      "subtype" : "42mm"
+      "role" : "appLauncher",
+      "subtype" : "40mm"
+    },
+    {
+      "size" : "50x50",
+      "idiom" : "watch",
+      "scale" : "2x",
+      "role" : "appLauncher",
+      "subtype" : "44mm"
     },
     {
       "size" : "86x86",
@@ -162,10 +169,24 @@
       "role" : "quickLook",
       "subtype" : "42mm"
     },
+    {
+      "size" : "108x108",
+      "idiom" : "watch",
+      "scale" : "2x",
+      "role" : "quickLook",
+      "subtype" : "44mm"
+    },
     {
       "idiom" : "watch-marketing",
       "size" : "1024x1024",
       "scale" : "1x"
+    },
+    {
+      "size" : "44x44",
+      "idiom" : "watch",
+      "scale" : "2x",
+      "role" : "longLook",
+      "subtype" : "42mm"
     }
   ],
   "info" : {

apps/ios/cleverrc/Info.plist

@@ -17,9 +17,9 @@
 	<key>CFBundlePackageType</key>
 	<string>APPL</string>
 	<key>CFBundleShortVersionString</key>
-	<string>1.1</string>
+	<string>1.2</string>
 	<key>CFBundleVersion</key>
-	<string>6</string>
+	<string>7</string>
 	<key>LSRequiresIPhoneOS</key>
 	<true/>
 	<key>UILaunchStoryboardName</key>

apps/ios/cleverrc/telemetry.js

@@ -64,9 +64,19 @@ new ROSLIB.Topic({
 var notificationHideTimer;
 
 function notify(text, severity) {
+	var repeated = notificationsEl.querySelector('.item:first-of-type[data-text=' + text + ']');
+	if (repeated) {
+		// don't repeat notifications
+		var count = repeated.getAttribute('data-count') || 1;
+		repeated.setAttribute('data-count', ++count);
+		repeated.innerHTML = text + ' (' + count + ')';
+		return;
+	}
+
 	var item = document.createElement('div');
 	item.innerHTML = text;
 	item.classList.add('item');
+	item.setAttribute('data-text', text);
 	notificationsEl.prepend(item);
 	var itemHeight = item.offsetHeight;
 	notificationsEl.classList.remove('anim');

aruco_pose/CMakeLists.txt

@@ -16,11 +16,15 @@ find_package(catkin REQUIRED COMPONENTS
   image_transport
   cv_bridge
   tf
-  #tf2
-  #tf2_ros
-  #aruco_msgs
+  tf2
+  tf2_ros
+  tf2_geometry_msgs
+  sensor_msgs
+  message_generation
 )
 
+find_package(OpenCV 3 REQUIRED)
+
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
 
@@ -55,11 +59,12 @@ find_package(catkin REQUIRED COMPONENTS
 ##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
 
 ## Generate messages in the 'msg' folder
-#add_message_files(
-#  FILES
-#  Marker.msg
-#  MarkerArray.msg
-#)
+add_message_files(
+  FILES
+  Point2D.msg
+  Marker.msg
+  MarkerArray.msg
+)
 
 ## Generate services in the 'srv' folder
 # add_service_files(
@@ -76,10 +81,11 @@ find_package(catkin REQUIRED COMPONENTS
 # )
 
 ## Generate added messages and services with any dependencies listed here
-#generate_messages(
-#  DEPENDENCIES
-#  std_msgs  # Or other packages containing msgs
-#)
+generate_messages(
+  DEPENDENCIES
+  std_msgs
+  geometry_msgs
+)
 
 ################################################
 ## Declare ROS dynamic reconfigure parameters ##
@@ -111,9 +117,9 @@ find_package(catkin REQUIRED COMPONENTS
 ## CATKIN_DEPENDS: catkin_packages dependent projects also need
 ## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
-#  INCLUDE_DIRS include
+  INCLUDE_DIRS DEPENDS OpenCV
   LIBRARIES aruco_pose
-#  CATKIN_DEPENDS other_catkin_pkg
+  CATKIN_DEPENDS message_runtime
 #  DEPENDS system_lib
 )
 
@@ -126,17 +132,17 @@ catkin_package(
 include_directories(
 # include
   ${catkin_INCLUDE_DIRS}
+  ${OpenCV_INCLUDE_DIRS}
 )
 
 ## Declare a C++ library
-add_library(${PROJECT_NAME}
-  src/aruco_pose.cpp
+add_library(aruco_pose
+  src/aruco_detect.cpp
+  src/aruco_map.cpp
+  src/draw.cpp
 )
 
-## Add cmake target dependencies of the library
-## as an example, code may need to be generated before libraries
-## either from message generation or dynamic reconfigure
-# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(${PROJECT_NAME} aruco_pose_generate_messages_cpp)
 
 ## Declare a C++ executable
 ## With catkin_make all packages are built within a single CMake context
@@ -154,11 +160,9 @@ add_library(${PROJECT_NAME}
 # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
 ## Specify libraries to link a library or executable target against
-link_directories(/opt/ros/kinetic/lib)
-
-target_link_libraries(${PROJECT_NAME}
+target_link_libraries(aruco_pose
   ${catkin_LIBRARIES}
-  "/opt/ros/kinetic/lib/libopencv_aruco3.so" # TODO: fix launch fails with .so loading
+  ${OpenCV_LIBRARIES}
 )
 
 #############
@@ -169,7 +173,7 @@ target_link_libraries(${PROJECT_NAME}
 # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
 
 ## Mark executable scripts (Python etc.) for installation
- ## in contrast to setup.py, you can choose the destination
+## in contrast to setup.py, you can choose the destination
 # install(PROGRAMS
 #   scripts/my_python_script
 #   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
@@ -208,3 +212,12 @@ target_link_libraries(${PROJECT_NAME}
 
 ## Add folders to be run by python nosetests
 # catkin_add_nosetests(test)
+
+if (CATKIN_ENABLE_TESTING)
+  find_package(rostest REQUIRED)
+  add_rostest(test/basic.test)
+  add_rostest(test/test_parser_pass.test)
+  add_rostest(test/test_parser_empty_map.test)
+  add_rostest(test/test_node_failure.test)
+  add_rostest(test/largemap.test)
+endif()

aruco_pose/README.md

@@ -0,0 +1,120 @@
+# Positioning with ArUco markers
+
+`aruco_pose` package consists of two nodelets: `aruco_detect` detects individual ArUco-markers and estimates their poses, `aruco_map` detects maps of markers using `aruco_detect` output.
+
+## Quick start
+
+To run a camera nodelet, markers and maps detector:
+
+```bash
+roslaunch aruco_pose sample.launch
+```
+
+You're going to need [`cv_camera`](http://wiki.ros.org/cv_camera) package installed.
+
+## aruco_detect nodelet
+
+`aruco_detect` detects ArUco markers on the image, publishes list of them (with poses), TF transformations, visualization markers and processed image for debugging.
+
+It's recommended to run it within the same nodelet manager with the camera nodelet (e. g. [`cv_camera`](http://wiki.ros.org/cv_camera)).
+
+### Parameters
+
+* `~dictionary` (*int*) – ArUco dictionary (default: 2)
+  * 0 = DICT_4X4_50
+  * 1 = DICT_4X4_100,
+  * 2 = DICT_4X4_250,
+  * 3 = DICT_4X4_1000,
+  * 4 = DICT_5X5_50,
+  * 5 = DICT_5X5_100,
+  * 6 = DICT_5X5_250,
+  * 7 = DICT_5X5_1000,
+  * 8 = DICT_6X6_50,
+  * 9 = DICT_6X6_100,
+  * 10 = DICT_6X6_250,
+  * 11 = DICT_6X6_1000,
+  * 12 = DICT_7X7_50,
+  * 13 = DICT_7X7_100,
+  * 14 = DICT_7X7_250,
+  * 15 = DICT_7X7_1000,
+  * 16 = DICT_ARUCO_ORIGINAL
+* `~estimate_poses` (*bool*) – estimate single markers' poses (default: true)
+* `~send_tf` (*bool*) – send TF transforms (default: true)
+* `~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
+
+### Topics
+
+#### Subscribed
+
+* `image_raw` (*sensor_msgs/Image*) – camera image
+* `camera_info` (*sensor_msgs/CameraInfo*) – camera calibration info
+
+#### Published
+
+* `~markers` (*aruco_pose/MarkerArray*) – list of detected markers with their corners and poses
+* `~visualization` (*visualization_msgs/MarkerArray*) – visualization markers for rviz
+* `~debug` (*sensor_msgs/Image*) – debug image with detected markers
+
+### Published transforms
+
+* `<camera_frame>` => `<frame_id_prefix><id>` – markers' poses
+
+## aruco_map nodelet
+
+`aruco_map` nodelet estimates position of markers map.
+
+### Parameters
+
+* `~map` – path to text file with markers list
+* `~frame_id` – published frame id (default: `aruco_map`)
+* `~known_tilt` – debug image width
+* `~image_width` – debug image width (default: 2000)
+* `~image_height` – debug image height (default: 2000)
+* `~image_margin` – debug image margin (default: 200)
+* `~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:
+
+```
+marker_id marker_length x y z yaw pitch roll
+```
+
+Where yaw, pitch and roll are extrinsic rotation around Z, Y, X axis, respectively.
+
+See examples in [`map`](map/) directory.
+
+### Topics
+
+#### Subscribed
+
+* `image_raw` (*sensor_msgs/Image*) – camera image (used for debug image)
+* `camera_info` (*sensor_msgs/CameraInfo*) – camera calibration info (used for debug image)
+* `markers` (*aruco_pose/MarkerArray*) – list of markers detected by `aruco_pose` nodelet
+
+#### Published
+
+* `~pose` (*geometry_msgs/PoseWithCovarianceStamped*) – estimated map pose
+* `~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
+
+### Published transforms
+
+* `<camera_frame>` => `<map_name>` – markers map pose
+
+## Running tests
+
+Command for running tests:
+
+```bash
+catkin_make run_tests && catkin_test_results
+```
+
+## Copyright
+
+Copyright © 2018 Copter Express Technologies. Author: Oleg Kalachev.
+
+Distributed under MIT License (https://opensource.org/licenses/MIT).

aruco_pose/launch/sample.launch

@@ -0,0 +1,26 @@
+<launch>
+    <node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager"/>
+
+    <!-- camera node -->
+    <node pkg="nodelet" type="nodelet" name="main_camera" args="load cv_camera/CvCameraNodelet nodelet_manager">
+        <param name="frame_id" value="main_camera_optical"/>
+        <param name="camera_info_url" value="file://$(find aruco_pose)/test/camera_info.yaml" />
+        <param name="image_width" value="640"/>
+        <param name="image_height" value="480"/>
+    </node>
+
+    <!-- detect aruco markers -->
+    <node pkg="nodelet" clear_params="true" type="nodelet" name="aruco_detect" args="load aruco_pose/aruco_detect nodelet_manager">
+        <remap from="image_raw" to="main_camera/image_raw"/>
+        <remap from="camera_info" to="main_camera/camera_info"/>
+        <param name="length" value="0.33"/>
+    </node>
+
+    <!-- aruco map -->
+    <node pkg="nodelet" clear_params="true" type="nodelet" name="aruco_map" args="load aruco_pose/aruco_map nodelet_manager">
+        <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"/>
+    </node>
+</launch>

aruco_pose/map/cmit.txt

@@ -0,0 +1,100 @@
+0	0.33	0.0	9.0	0	0	0	0	
+1	0.33	1.0	9.0	0	0	0	0	
+2	0.33	2.0	9.0	0	0	0	0	
+3	0.33	3.0	9.0	0	0	0	0	
+4	0.33	4.0	9.0	0	0	0	0	
+5	0.33	5.0	9.0	0	0	0	0	
+6	0.33	6.0	9.0	0	0	0	0	
+7	0.33	7.0	9.0	0	0	0	0	
+8	0.33	8.0	9.0	0	0	0	0	
+9	0.33	9.0	9.0	0	0	0	0	
+10	0.33	0.0	8.0	0	0	0	0	
+11	0.33	1.0	8.0	0	0	0	0	
+12	0.33	2.0	8.0	0	0	0	0	
+13	0.33	3.0	8.0	0	0	0	0	
+14	0.33	4.0	8.0	0	0	0	0	
+15	0.33	5.0	8.0	0	0	0	0	
+16	0.33	6.0	8.0	0	0	0	0	
+#17	0.33	7.0	8.0	0	0	0	0	
+18	0.33	8.0	8.0	0	0	0	0	
+19	0.33	9.0	8.0	0	0	0	0	
+20	0.33	0.0	7.0	0	0	0	0	
+21	0.33	1.0	7.0	0	0	0	0	
+22	0.33	2.0	7.0	0	0	0	0	
+23	0.33	3.0	7.0	0	0	0	0	
+24	0.33	4.0	7.0	0	0	0	0	
+25	0.33	5.0	7.0	0	0	0	0	
+26	0.33	6.0	7.0	0	0	0	0	
+27	0.33	7.0	7.0	0	0	0	0	
+28	0.33	8.0	7.0	0	0	0	0	
+29	0.33	9.0	7.0	0	0	0	0	
+30	0.33	0.0	6.0	0	0	0	0	
+31	0.33	1.0	6.0	0	0	0	0	
+32	0.33	2.0	6.0	0	0	0	0	
+33	0.33	3.0	6.0	0	0	0	0	
+34	0.33	4.0	6.0	0	0	0	0	
+35	0.33	5.0	6.0	0	0	0	0	
+36	0.33	6.0	6.0	0	0	0	0	
+37	0.33	7.0	6.0	0	0	0	0	
+38	0.33	8.0	6.0	0	0	0	0	
+39	0.33	9.0	6.0	0	0	0	0	
+40	0.33	0.0	5.0	0	0	0	0	
+41	0.33	1.0	5.0	0	0	0	0	
+42	0.33	2.0	5.0	0	0	0	0	
+43	0.33	3.0	5.0	0	0	0	0	
+44	0.33	4.0	5.0	0	0	0	0	
+45	0.33	5.0	5.0	0	0	0	0	
+46	0.33	6.0	5.0	0	0	0	0	
+47	0.33	7.0	5.0	0	0	0	0	
+48	0.33	8.0	5.0	0	0	0	0	
+49	0.33	9.0	5.0	0	0	0	0	
+50	0.33	0.0	4.0	0	0	0	0	
+51	0.33	1.0	4.0	0	0	0	0	
+52	0.33	2.0	4.0	0	0	0	0	
+53	0.33	3.0	4.0	0	0	0	0	
+54	0.33	4.0	4.0	0	0	0	0	
+55	0.33	5.0	4.0	0	0	0	0	
+56	0.33	6.0	4.0	0	0	0	0	
+57	0.33	7.0	4.0	0	0	0	0	
+58	0.33	8.0	4.0	0	0	0	0	
+59	0.33	9.0	4.0	0	0	0	0	
+60	0.33	0.0	3.0	0	0	0	0	
+61	0.33	1.0	3.0	0	0	0	0	
+62	0.33	2.0	3.0	0	0	0	0	
+63	0.33	3.0	3.0	0	0	0	0	
+64	0.33	4.0	3.0	0	0	0	0	
+65	0.33	5.0	3.0	0	0	0	0	
+66	0.33	6.0	3.0	0	0	0	0	
+67	0.33	7.0	3.0	0	0	0	0	
+68	0.33	8.0	3.0	0	0	0	0	
+69	0.33	9.0	3.0	0	0	0	0	
+70	0.33	0.0	2.0	0	0	0	0	
+71	0.33	1.0	2.0	0	0	0	0	
+72	0.33	2.0	2.0	0	0	0	0	
+73	0.33	3.0	2.0	0	0	0	0	
+74	0.33	4.0	2.0	0	0	0	0	
+75	0.33	5.0	2.0	0	0	0	0	
+76	0.33	6.0	2.0	0	0	0	0	
+77	0.33	7.0	2.0	0	0	0	0	
+78	0.33	8.0	2.0	0	0	0	0	
+79	0.33	9.0	2.0	0	0	0	0	
+80	0.33	0.0	1.0	0	0	0	0	
+81	0.33	1.0	1.0	0	0	0	0	
+82	0.33	2.0	1.0	0	0	0	0	
+83	0.33	3.0	1.0	0	0	0	0	
+84	0.33	4.0	1.0	0	0	0	0	
+85	0.33	5.0	1.0	0	0	0	0	
+86	0.33	6.0	1.0	0	0	0	0	
+87	0.33	7.0	1.0	0	0	0	0	
+88	0.33	8.0	1.0	0	0	0	0	
+89	0.33	9.0	1.0	0	0	0	0	
+90	0.33	0.0	0.0	0	0	0	0	
+91	0.33	1.0	0.0	0	0	0	0	
+92	0.33	2.0	0.0	0	0	0	0	
+93	0.33	3.0	0.0	0	0	0	0	
+94	0.33	4.0	0.0	0	0	0	0	
+95	0.33	5.0	0.0	0	0	0	0	
+96	0.33	6.0	0.0	0	0	0	0	
+97	0.33	7.0	0.0	0	0	0	0	
+98	0.33	8.0	0.0	0	0	0	0	
+99	0.33	9.0	0.0	0	0	0	0	

aruco_pose/map/map.txt

@@ -0,0 +1,4 @@
+1	0.33	0	0	0	0	0	0
+2	0.33	1	0	0	0	0	0
+3	0.33	0	1	0	0	0	0
+4	0.33	1	1	0	0	0	0

aruco_pose/map/office.txt

@@ -0,0 +1,8 @@
+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
+104	0.33	0.77	0.77	0	0	0	0
+103	0.33	0	1.54	0	0	0	0
+102	0.33	0.77	1.54	0	0	0	0
+101	0.33	0	2.31	0	0	0	0
+100	0.33	0.77	2.31	0	0	0	0

aruco_pose/map/office_ceiling.txt

@@ -0,0 +1,31 @@
+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
+31	0.365	4.200	0.0	0	0	0	0
+12	0.365	0.000	1.8	0	0	0	0
+13	0.365	1.335	1.8	0	0	0	0
+28	0.365	2.865	1.8	0	0	0	0
+29	0.365	4.200	1.8	0	0	0	0
+10	0.365	0.000	3.6	0	0	0	0
+11	0.365	1.335	3.6	0	0	0	0
+26	0.365	2.865	3.6	0	0	0	0
+27	0.365	4.200	3.6	0	0	0	0
+8	0.365	0.000	5.4	0	0	0	0
+9	0.365	1.335	5.4	0	0	0	0
+24	0.365	2.865	5.4	0	0	0	0
+25	0.365	4.200	5.4	0	0	0	0
+6	0.365	0.000	7.2	0	0	0	0
+7	0.365	1.335	7.2	0	0	0	0
+22	0.365	2.865	7.2	0	0	0	0
+23	0.365	4.200	7.2	0	0	0	0
+4	0.365	0.000	9.0	0	0	0	0
+5	0.365	1.335	9.0	0	0	0	0
+20	0.365	2.865	9.0	0	0	0	0
+21	0.365	4.200	9.0	0	0	0	0
+2	0.365	0.000	10.8	0	0	0	0
+3	0.365	1.335	10.8	0	0	0	0
+18	0.365	2.865	10.8	0	0	0	0
+19	0.365	4.200	10.8	0	0	0	0
+1	0.365	0.000	12.6	0	0	0	0
+0	0.365	1.335	12.6	0	0	0	0
+16	0.365	2.865	12.6	0	0	0	0

aruco_pose/msg/Marker.msg

@@ -0,0 +1,7 @@
+uint32 id
+float32 length
+geometry_msgs/Pose pose
+Point2D c1
+Point2D c2
+Point2D c3
+Point2D c4

aruco_pose/msg/MarkerArray.msg

@@ -0,0 +1,2 @@
+Header header
+Marker[] markers

aruco_pose/msg/Point2D.msg

@@ -0,0 +1,2 @@
+float32 x
+float32 y

aruco_pose/nodelet_plugins.xml

@@ -1,5 +1,8 @@
 <library path="lib/libaruco_pose">
-   <class name="aruco_pose/aruco_pose" type="ArucoPose" base_class_type="nodelet::Nodelet">
+   <class name="aruco_pose/aruco_detect" type="ArucoDetect" base_class_type="nodelet::Nodelet">
+      <description/>
+   </class>
+   <class name="aruco_pose/aruco_map" type="ArucoMap" base_class_type="nodelet::Nodelet">
       <description/>
    </class>
 </library>

aruco_pose/package.xml

@@ -1,62 +1,40 @@
 <?xml version="1.0"?>
-<package>
+<package format="2">
   <name>aruco_pose</name>
-  <version>0.0.0</version>
-  <description>ArUco maps precise pose estimation nodelet</description>
+  <version>0.0.1</version>
+  <description>Positioning with ArUco markers</description>
 
-  <!-- One maintainer tag required, multiple allowed, one person per tag -->
-  <!-- Example:  -->
-  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
   <maintainer email="okalachev@gmail.com">Oleg Kalachev</maintainer>
+  <license>MIT</license>
 
+  <!--url type="website">http://wiki.ros.org/aruco_pose</url-->
+  <author email="okalachev@gmail.com">Oleg Kalachev</author>
 
-  <!-- One license tag required, multiple allowed, one license per tag -->
-  <!-- Commonly used license strings: -->
-  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
-  <license>TODO</license>
-
-
-  <!-- Url tags are optional, but multiple are allowed, one per tag -->
-  <!-- Optional attribute type can be: website, bugtracker, or repository -->
-  <!-- Example: -->
-  <!-- <url type="website">http://wiki.ros.org/aruco_pose</url> -->
-
-
-  <!-- Author tags are optional, multiple are allowed, one per tag -->
-  <!-- Authors do not have to be maintainers, but could be -->
-  <!-- Example: -->
-  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
-
-
-  <!-- The *_depend tags are used to specify dependencies -->
-  <!-- Dependencies can be catkin packages or system dependencies -->
-  <!-- Examples: -->
-  <!-- Use build_depend for packages you need at compile time: -->
-  <!--   <build_depend>message_generation</build_depend> -->
-  <!-- Use buildtool_depend for build tool packages: -->
-  <!--   <buildtool_depend>catkin</buildtool_depend> -->
-  <!-- Use run_depend for packages you need at runtime: -->
-  <!--   <run_depend>message_runtime</run_depend> -->
-  <!-- Use test_depend for packages you need only for testing: -->
-  <!--   <test_depend>gtest</test_depend> -->
   <buildtool_depend>catkin</buildtool_depend>
 
-  <build_depend>nodelet</build_depend>
-  <build_depend>roscpp</build_depend>
-  <build_depend>image_transport</build_depend>
-  <build_depend>cv_bridge</build_depend>
-  <build_depend>tf</build_depend>
+  <depend>roscpp</depend>
+  <depend>nodelet</depend>
+  <depend>tf</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>opencv3</depend>
+  <depend>cv_bridge</depend>
+  <depend>image_transport</depend>
+  <depend>message_generation</depend>
+  <depend>message_runtime</depend>
+  <depend>std_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>visualization_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>rostest</depend>
 
-  <run_depend>nodelet</run_depend>
-  <run_depend>roscpp</run_depend>
-  <run_depend>image_transport</run_depend>
-  <run_depend>cv_bridge</run_depend>
-  <build_depend>tf</build_depend>
+  <test_depend>image_publisher</test_depend>
+  <test_depend>ros_pytest</test_depend>
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>
-        <nodelet plugin="${prefix}/nodelet_plugins.xml" />
+    <nodelet plugin="${prefix}/nodelet_plugins.xml" />
     <!-- Other tools can request additional information be placed here -->
-
   </export>
 </package>

aruco_pose/src/aruco_detect.cpp

@@ -0,0 +1,346 @@
+/*
+ * Detecting and pose estimation of ArUco markers
+ * Copyright (C) 2018 Copter Express Technologies
+ *
+ * Author: Oleg Kalachev <okalachev@gmail.com>
+ *
+ * Distributed under MIT License (available at https://opensource.org/licenses/MIT).
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ */
+
+/*
+ * Code is based on https://github.com/UbiquityRobotics/fiducials, which is distributed
+ * under the BSD license.
+ */
+
+#include <math.h>
+#include <vector>
+#include <string>
+#include <map>
+#include <unordered_map>
+#include <unordered_set>
+#include <ros/ros.h>
+#include <nodelet/nodelet.h>
+#include <pluginlib/class_list_macros.h>
+#include <tf/transform_datatypes.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <image_transport/image_transport.h>
+#include <cv_bridge/cv_bridge.h>
+#include <geometry_msgs/Vector3.h>
+#include <geometry_msgs/Pose.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
+#include <geometry_msgs/TransformStamped.h>
+#include <visualization_msgs/Marker.h>
+#include <visualization_msgs/MarkerArray.h>
+
+#include <opencv2/opencv.hpp>
+#include <opencv2/highgui.hpp>
+#include <opencv2/aruco.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/calib3d/calib3d.hpp>
+
+#include <aruco_pose/Marker.h>
+#include <aruco_pose/MarkerArray.h>
+
+#include "utils.h"
+
+using std::vector;
+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_};
+	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_;
+	double length_;
+	std::unordered_map<int, double> length_override_;
+	std::string frame_id_prefix_, known_tilt_;
+	Mat camera_matrix_, dist_coeffs_;
+	aruco_pose::MarkerArray array_;
+	std::unordered_set<int> map_markers_ids_;
+	visualization_msgs::MarkerArray vis_array_;
+
+public:
+	virtual void onInit()
+	{
+		nh_ = getNodeHandle();
+		nh_priv_ = getPrivateNodeHandle();
+
+		int dictionary;
+		nh_priv_.param("dictionary", dictionary, 2);
+		nh_priv_.param("estimate_poses", estimate_poses_, true);
+		nh_priv_.param("send_tf", send_tf_, true);
+		if (estimate_poses_ && !nh_priv_.getParam("length", length_)) {
+			NODELET_FATAL("can't estimate marker's poses as ~length parameter is not defined");
+			ros::shutdown();
+		}
+		readLengthOverride();
+
+		nh_priv_.param<std::string>("known_tilt", known_tilt_, "");
+		nh_priv_.param("auto_flip", auto_flip_, false);
+
+		nh_priv_.param<std::string>("frame_id_prefix", 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);
+		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);
+
+		NODELET_INFO("ready");
+	}
+
+private:
+	void imageCallback(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr &cinfo)
+	{
+		Mat image = cv_bridge::toCvShare(msg, "bgr8")->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;
+
+		// Detect markers
+		cv::aruco::detectMarkers(image, dictionary_, corners, ids, parameters_, rejected);
+
+		array_.header.stamp = msg->header.stamp;
+		array_.header.frame_id = msg->header.frame_id;
+		array_.markers.clear();
+
+		if (ids.size() != 0) {
+			parseCameraInfo(cinfo, camera_matrix_, dist_coeffs_);
+
+			// Estimate individual markers' poses
+			if (estimate_poses_) {
+				cv::aruco::estimatePoseSingleMarkers(corners, length_, camera_matrix_, dist_coeffs_,
+				                                     rvecs, tvecs);
+
+				// process length override, TODO: efficiency
+				if (!length_override_.empty()) {
+					for (unsigned int i = 0; i < ids.size(); i++) {
+						int id = ids[i];
+						auto item = length_override_.find(id);
+						if (item != length_override_.end()) { // found override
+							vector<cv::Vec3d> rvecs_current, tvecs_current;
+							vector<vector<cv::Point2f>> corners_current;
+							corners_current.push_back(corners[i]);
+							cv::aruco::estimatePoseSingleMarkers(corners_current, item->second,
+							                                     camera_matrix_, dist_coeffs_,
+										                         rvecs_current, tvecs_current);
+							rvecs[i] = rvecs_current[0];
+							tvecs[i] = tvecs_current[0];
+						}
+					}
+				}
+
+				if (!known_tilt_.empty()) {
+					try {
+						snap_to = tf_buffer_.lookupTransform(msg->header.frame_id, known_tilt_,
+						                                     msg->header.stamp, ros::Duration(0.02));
+					} catch (const tf2::TransformException& e) {
+						NODELET_WARN_THROTTLE(5, "can't snap: %s", e.what());
+					}
+				}
+			}
+
+			array_.markers.reserve(ids.size());
+			aruco_pose::Marker marker;
+			geometry_msgs::TransformStamped transform;
+			transform.header.stamp = msg->header.stamp;
+			transform.header.frame_id = msg->header.frame_id;
+
+			for (unsigned int i = 0; i < ids.size(); i++) {
+				marker.id = ids[i];
+				marker.length = getMarkerLength(marker.id);
+				fillCorners(marker, corners[i]);
+
+				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_);
+					}
+
+					// 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);
+						}
+					}
+				}
+				array_.markers.push_back(marker);
+			}
+		}
+
+		markers_pub_.publish(array_);
+
+		// Publish visualization markers
+		if (estimate_poses_ && vis_markers_pub_.getNumSubscribers() != 0) {
+			// Delete all markers
+			visualization_msgs::Marker vis_marker;
+			vis_marker.action = visualization_msgs::Marker::DELETEALL;
+			vis_array_.markers.clear();
+			vis_array_.markers.reserve(ids.size() + 1);
+			vis_array_.markers.push_back(vis_marker);
+
+			for (unsigned int i = 0; i < ids.size(); i++)
+				pushVisMarkers(msg->header.frame_id, msg->header.stamp, array_.markers[i].pose,
+				               getMarkerLength(ids[i]), ids[i], i);
+
+			vis_markers_pub_.publish(vis_array_);
+		}
+
+		// Publish debug image
+		if (debug_pub_.getNumSubscribers() != 0) {
+			Mat debug = image.clone();
+			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]));
+
+			cv_bridge::CvImage out_msg;
+			out_msg.header.frame_id = msg->header.frame_id;
+			out_msg.header.stamp = msg->header.stamp;
+			out_msg.encoding = sensor_msgs::image_encodings::BGR8;
+			out_msg.image = debug;
+			debug_pub_.publish(out_msg.toImageMsg());
+		}
+	}
+
+	inline void fillCorners(aruco_pose::Marker& marker, const vector<cv::Point2f>& corners) const
+	{
+		marker.c1.x = corners[0].x;
+		marker.c2.x = corners[1].x;
+		marker.c3.x = corners[2].x;
+		marker.c4.x = corners[3].x;
+		marker.c1.y = corners[0].y;
+		marker.c2.y = corners[1].y;
+		marker.c3.y = corners[2].y;
+		marker.c4.y = corners[3].y;
+	}
+
+	inline void fillPose(geometry_msgs::Pose& pose, const cv::Vec3d& rvec, const cv::Vec3d& tvec) const
+	{
+		pose.position.x = tvec[0];
+		pose.position.y = tvec[1];
+		pose.position.z = tvec[2];
+
+		double angle = norm(rvec);
+		cv::Vec3d axis = rvec / angle;
+
+		tf2::Quaternion q;
+		q.setRotation(tf2::Vector3(axis[0], axis[1], axis[2]), angle);
+
+		pose.orientation.w = q.w();
+		pose.orientation.x = q.x();
+		pose.orientation.y = q.y();
+		pose.orientation.z = q.z();
+	}
+
+	inline void fillTranslation(geometry_msgs::Vector3& translation, const cv::Vec3d& tvec) const
+	{
+		translation.x = tvec[0];
+		translation.y = tvec[1];
+		translation.z = tvec[2];
+	}
+
+	void pushVisMarkers(const std::string& frame_id, const ros::Time& stamp,
+	                    const geometry_msgs::Pose &pose, double length, int id, int index)
+	{
+		visualization_msgs::Marker marker;
+		marker.header.frame_id = frame_id;
+		marker.header.stamp = stamp;
+		marker.action = visualization_msgs::Marker::ADD;
+		marker.id = index;
+
+		// Marker
+		marker.ns = "aruco_marker";
+		marker.type = visualization_msgs::Marker::CUBE;
+		marker.scale.x = length;
+		marker.scale.y = length;
+		marker.scale.z = 0.001;
+		marker.color.r = 1;
+		marker.color.g = 1;
+		marker.color.b = 1;
+		marker.color.a = 0.9;
+		marker.pose = pose;
+		vis_array_.markers.push_back(marker);
+
+		// Label
+		marker.ns = "aruco_marker_label";
+		marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
+		marker.scale.z = length * 0.6;
+		marker.color.r = 0;
+		marker.color.g = 0;
+		marker.color.b = 0;
+		marker.color.a = 1;
+		marker.text = std::to_string(id);
+		marker.pose = pose;
+		vis_array_.markers.push_back(marker);
+	}
+
+	inline std::string getChildFrameId(int id) const
+	{
+		return frame_id_prefix_ + std::to_string(id);
+	}
+
+	void readLengthOverride()
+	{
+		std::map<std::string, double> length_override;
+		nh_priv_.getParam("length_override", length_override);
+		for (auto const& item : length_override) {
+			length_override_[std::stoi(item.first)] = item.second;
+		}
+	}
+
+	inline double getMarkerLength(int id)
+	{
+		auto item = length_override_.find(id);
+		if (item != length_override_.end()) {
+			return item->second;
+		} else {
+			return length_;
+		}
+	}
+
+	void mapMarkersCallback(const aruco_pose::MarkerArray& msg)
+	{
+		map_markers_ids_.clear();
+		for (auto const& marker : msg.markers) {
+			map_markers_ids_.insert(marker.id);
+		}
+	}
+};
+
+PLUGINLIB_EXPORT_CLASS(ArucoDetect, nodelet::Nodelet)

aruco_pose/src/aruco_map.cpp

@@ -0,0 +1,516 @@
+/*
+ * Detecting and pose estimation of ArUco markers maps
+ * Copyright (C) 2018 Copter Express Technologies
+ *
+ * Author: Oleg Kalachev <okalachev@gmail.com>
+ *
+ * Distributed under MIT License (available at https://opensource.org/licenses/MIT).
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ */
+
+/*
+ * Code is based on https://github.com/UbiquityRobotics/fiducials, which is distributed
+ * under the BSD license.
+ */
+
+#include <math.h>
+#include <string>
+#include <vector>
+#include <fstream>
+#include <algorithm>
+#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 <tf/transform_datatypes.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include <tf2_ros/static_transform_broadcaster.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <message_filters/subscriber.h>
+#include <message_filters/synchronizer.h>
+#include <message_filters/sync_policies/exact_time.h>
+#include <geometry_msgs/TransformStamped.h>
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
+#include <sensor_msgs/Image.h>
+#include <visualization_msgs/Marker.h>
+#include <visualization_msgs/MarkerArray.h>
+
+#include <aruco_pose/MarkerArray.h>
+#include <aruco_pose/Marker.h>
+
+#include <opencv2/opencv.hpp>
+#include <opencv2/aruco.hpp>
+
+#include "draw.h"
+#include "utils.h"
+
+using std::vector;
+using cv::Mat;
+using sensor_msgs::Image;
+using sensor_msgs::CameraInfo;
+using aruco_pose::MarkerArray;
+
+typedef message_filters::sync_policies::ExactTime<Image, CameraInfo, MarkerArray> SyncPolicy;
+
+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_;
+	message_filters::Subscriber<CameraInfo> info_sub_;
+	message_filters::Subscriber<MarkerArray> markers_sub_;
+	boost::shared_ptr<message_filters::Synchronizer<SyncPolicy> > sync_;
+	cv::Ptr<cv::aruco::Board> board_;
+	Mat camera_matrix_, dist_coeffs_;
+	geometry_msgs::TransformStamped transform_;
+	geometry_msgs::PoseWithCovarianceStamped pose_;
+	vector<geometry_msgs::TransformStamped> markers_transforms_;
+	aruco_pose::MarkerArray markers_;
+	tf2_ros::TransformBroadcaster br_;
+	tf2_ros::StaticTransformBroadcaster static_br_;
+	tf2_ros::Buffer tf_buffer_;
+	tf2_ros::TransformListener tf_listener_{tf_buffer_};
+	visualization_msgs::MarkerArray vis_array_;
+	std::string known_tilt_, map_, markers_frame_, markers_parent_frame_;
+	int image_width_, image_height_, image_margin_;
+	bool auto_flip_, image_axis_;
+
+public:
+	virtual void onInit()
+	{
+		nh_ = getNodeHandle();
+		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);
+
+		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_, "");
+
+		// createStripLine();
+
+		if (type == "map") {
+			param(nh_priv_, "map", map);
+			loadMap(map);
+		} else if (type == "gridboard") {
+			createGridBoard();
+		} else {
+			NODELET_FATAL("unknown type: %s", type.c_str());
+			ros::shutdown();
+		}
+
+		pose_pub_ = nh_priv_.advertise<geometry_msgs::PoseWithCovarianceStamped>("pose", 1);
+		vis_markers_pub_ = nh_priv_.advertise<visualization_msgs::MarkerArray>("visualization", 1, true);
+		debug_pub_ = it_priv.advertise("debug", 1);
+
+		image_sub_.subscribe(nh_, "image_raw", 1);
+		info_sub_.subscribe(nh_, "camera_info", 1);
+		markers_sub_.subscribe(nh_, "markers", 1);
+
+		sync_.reset(new message_filters::Synchronizer<SyncPolicy>(SyncPolicy(10), image_sub_, info_sub_, markers_sub_));
+		sync_->registerCallback(boost::bind(&ArucoMap::callback, this, _1, _2, _3));
+
+		publishMarkersFrames();
+		publishMarkers();
+		publishMapImage();
+		vis_markers_pub_.publish(vis_array_);
+
+		NODELET_INFO("ready");
+	}
+
+	void callback(const sensor_msgs::ImageConstPtr& image,
+	              const sensor_msgs::CameraInfoConstPtr& cinfo,
+	              const aruco_pose::MarkerArrayConstPtr& markers)
+	{
+		int valid = 0;
+		int count = markers->markers.size();
+		std::vector<int> ids;
+		std::vector<std::vector<cv::Point2f>> corners;
+		cv::Vec3d rvec, tvec;
+
+		parseCameraInfo(cinfo, camera_matrix_, dist_coeffs_);
+		if (markers->markers.empty()) goto publish_debug;
+
+		ids.reserve(count);
+		corners.reserve(count);
+
+		for(auto const &marker : markers->markers) {
+			ids.push_back(marker.id);
+			std::vector<cv::Point2f> marker_corners = {
+				cv::Point2f(marker.c1.x, marker.c1.y),
+				cv::Point2f(marker.c2.x, marker.c2.y),
+				cv::Point2f(marker.c3.x, marker.c3.y),
+				cv::Point2f(marker.c4.x, marker.c4.y)
+			};
+			corners.push_back(marker_corners);
+		}
+
+		if (known_tilt_.empty()) {
+			// simple estimation
+			valid = cv::aruco::estimatePoseBoard(corners, ids, board_, camera_matrix_, dist_coeffs_,
+			                                     rvec, tvec, false);
+			if (!valid) goto publish_debug;
+
+			transform_.header.stamp = markers->header.stamp;
+			transform_.header.frame_id = markers->header.frame_id;
+			pose_.header = transform_.header;
+			fillPose(pose_.pose.pose, rvec, tvec);
+			fillTransform(transform_.transform, rvec, tvec);
+
+		} else {
+			Mat obj_points, img_points;
+			// estimation with "snapping"
+			cv::aruco::getBoardObjectAndImagePoints(board_, corners, ids, obj_points, img_points);
+			if (obj_points.empty()) goto publish_debug;
+
+			double center_x = 0, center_y = 0, center_z = 0;
+			alignObjPointsToCenter(obj_points, center_x, center_y, center_z);
+
+			valid = solvePnP(obj_points, img_points, camera_matrix_, dist_coeffs_, rvec, tvec, false);
+			if (!valid) goto publish_debug;
+
+			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_);
+			} catch (const tf2::TransformException& e) {
+				NODELET_WARN_THROTTLE(1, "can't snap: %s", e.what());
+			}
+
+			geometry_msgs::TransformStamped shift;
+			shift.transform.translation.x = -center_x;
+			shift.transform.translation.y = -center_y;
+			shift.transform.translation.z = -center_z;
+			shift.transform.rotation.w = 1;
+			tf2::doTransform(shift, transform_, transform_);
+
+			// for debug topic
+			tvec[0] = transform_.transform.translation.x;
+			tvec[1] = transform_.transform.translation.y;
+			tvec[2] = transform_.transform.translation.z;
+
+			transform_.header.stamp = markers->header.stamp;
+			transform_.header.frame_id = markers->header.frame_id;
+			pose_.header = transform_.header;
+			transformToPose(transform_.transform, pose_.pose.pose);
+		}
+
+		if (!transform_.child_frame_id.empty()) {
+			br_.sendTransform(transform_);
+		}
+		pose_pub_.publish(pose_);
+
+publish_debug:
+		// publish debug image (even if no map detected)
+		if (debug_pub_.getNumSubscribers() > 0) {
+			Mat mat = cv_bridge::toCvCopy(image, "bgr8")->image; // copy image as we're planning to modify it
+			cv::aruco::drawDetectedMarkers(mat, corners, ids); // draw detected markers
+			if (valid) {
+				_drawAxis(mat, camera_matrix_, dist_coeffs_, rvec, tvec, 1.0); // draw board axis
+			}
+			cv_bridge::CvImage out_msg;
+			out_msg.header.frame_id = image->header.frame_id;
+			out_msg.header.stamp = image->header.stamp;
+			out_msg.encoding = sensor_msgs::image_encodings::BGR8;
+			out_msg.image = mat;
+			debug_pub_.publish(out_msg.toImageMsg());
+		}
+	}
+
+	void alignObjPointsToCenter(Mat &obj_points, double &center_x, double &center_y, double &center_z) const
+	{
+		// Align object points to the center of mass
+		double sum_x = 0;
+		double sum_y = 0;
+		double sum_z = 0;
+
+		for (int i = 0; i < obj_points.rows; i++) {
+			sum_x += obj_points.at<float>(i, 0);
+			sum_y += obj_points.at<float>(i, 1);
+			sum_z += obj_points.at<float>(i, 2);
+		}
+
+		center_x = sum_x / obj_points.rows;
+		center_y = sum_y / obj_points.rows;
+		center_z = sum_z / obj_points.rows;
+
+		for (int i = 0; i < obj_points.rows; i++) {
+			obj_points.at<float>(i, 0) -= center_x;
+			obj_points.at<float>(i, 1) -= center_y;
+			obj_points.at<float>(i, 2) -= center_z;
+		}
+	}
+
+	void loadMap(std::string filename)
+	{
+		std::ifstream f(filename);
+		std::string line;
+
+		if (!f.good()) {
+			NODELET_FATAL("%s - %s", strerror(errno), filename.c_str());
+			ros::shutdown();
+		}
+
+		while (std::getline(f, line)) {
+			int id;
+			double length, x, y, z, yaw, pitch, roll;
+
+			std::istringstream s(line);
+
+			// Read first character to see whether it's a comment
+			char first = 0;
+			if (!(s >> first)) {
+				// No non-whitespace characters, must be a blank line
+				continue;
+			}
+
+			if (first == '#') {
+				NODELET_DEBUG("Skipping line as a comment: %s", line.c_str());
+				continue;
+			} else if (isdigit(first)) {
+				// Put the digit back into the stream
+				// Note that this is a non-modifying putback, so this should work with istreams
+				// (see https://en.cppreference.com/w/cpp/io/basic_istream/putback)
+				s.putback(first);
+			} else {
+				// Probably garbage data; inform user and throw an exception, possibly killing nodelet
+				NODELET_FATAL("Malformed input: %s", line.c_str());
+				ros::shutdown();
+				throw std::runtime_error("Malformed input");
+			}
+
+			if (!(s >> id >> length >> x >> y)) {
+				NODELET_ERROR("Not enough data in line: %s; "
+				          "Each marker must have at least id, length, x, y fields", line.c_str());
+				continue;
+			}
+			// Be less strict about z, yaw, pitch roll
+			if (!(s >> z)) {
+				NODELET_DEBUG("No z coordinate provided for marker %d, assuming 0", id);
+				z = 0;
+			}
+			if (!(s >> yaw)) {
+				NODELET_DEBUG("No yaw provided for marker %d, assuming 0", id);
+				yaw = 0;
+			}
+			if (!(s >> pitch)) {
+				NODELET_DEBUG("No pitch provided for marker %d, assuming 0", id);
+				pitch = 0;
+			}
+			if (!(s >> roll)) {
+				NODELET_DEBUG("No roll provided for marker %d, assuming 0", id);
+				roll = 0;
+			}
+			addMarker(id, length, x, y, z, yaw, pitch, roll);
+		}
+
+		NODELET_INFO("loading %s complete (%d markers)", filename.c_str(), static_cast<int>(board_->ids.size()));
+	}
+
+	void createGridBoard()
+	{
+		NODELET_INFO("generate gridboard");
+		NODELET_WARN("gridboard maps are deprecated");
+
+		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);
+
+		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);
+
+		if (nh_priv_.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();
+			}
+		} else {
+			// Fill marker_ids automatically
+			marker_ids.resize(markers_x * markers_y);
+			for (int i = 0; i < markers_x * markers_y; i++)
+			{
+				marker_ids.at(i) = first_marker++;
+			}
+		}
+
+		double max_y = markers_y * markers_side + (markers_y - 1) * markers_sep_y;
+		for(int y = 0; y < markers_y; y++) {
+			for(int x = 0; x < markers_x; x++) {
+				double x_pos = x * (markers_side + markers_sep_x);
+				double y_pos = max_y - y * (markers_side + markers_sep_y) - markers_side;
+				NODELET_INFO("add marker %d %g %g", marker_ids[y * markers_y + x], x_pos, y_pos);
+				addMarker(marker_ids[y * markers_y + x], markers_side, x_pos, y_pos, 0, 0, 0, 0);
+			}
+		}
+	}
+
+	// void createStripLine()
+	// {
+	// 	visualization_msgs::Marker marker;
+	// 	marker.header.frame_id = transform_.child_frame_id;
+	// 	marker.action = visualization_msgs::Marker::ADD;
+	// 	marker.ns = "aruco_map_link";
+	// 	marker.type = visualization_msgs::Marker::LINE_STRIP;
+	// 	marker.scale.x = 0.02;
+	// 	marker.color.g = 1;
+	// 	marker.color.a = 0.8;
+	// 	marker.frame_locked = true;
+	// 	marker.pose.orientation.w = 1;
+	// 	vis_array_.markers.push_back(marker);
+	// }
+
+	void addMarker(int id, double length, double x, double y, double z,
+				   double yaw, double pitch, double roll)
+	{
+		// Check whether the id is in range for current dictionary
+		int num_markers = board_->dictionary->bytesList.rows;
+		if (num_markers <= id) {
+			NODELET_ERROR("Marker id %d is not in dictionary; current dictionary contains %d markers. "
+			              "Please see https://github.com/CopterExpress/clever/blob/master/aruco_pose/README.md#parameters for details",
+					  id, num_markers);
+			return;
+		}
+		// Check if marker is already in the board
+		if (std::count(board_->ids.begin(), board_->ids.end(), id) > 0) {
+			NODELET_ERROR("Marker id %d is already in the map", id);
+			return;
+		}
+		// Create transform
+		tf::Quaternion q;
+		q.setRPY(roll, pitch, yaw);
+		tf::Transform transform(q, tf::Vector3(x, y, z));
+
+		/* marker's corners:
+			0    1
+			3    2
+		*/
+		double halflen = length / 2;
+		tf::Point p0(-halflen, halflen, 0);
+		tf::Point p1(halflen, halflen, 0);
+		tf::Point p2(halflen, -halflen, 0);
+		tf::Point p3(-halflen, -halflen, 0);
+		p0 = transform * p0;
+		p1 = transform * p1;
+		p2 = transform * p2;
+		p3 = transform * p3;
+
+		vector<cv::Point3f> obj_points = {
+			cv::Point3f(p0.x(), p0.y(), p0.z()),
+			cv::Point3f(p1.x(), p1.y(), p1.z()),
+			cv::Point3f(p2.x(), p2.y(), p2.z()),
+			cv::Point3f(p3.x(), p3.y(), p3.z())
+		};
+
+		board_->ids.push_back(id);
+		board_->objPoints.push_back(obj_points);
+
+		// Add marker's static transform
+		if (!markers_frame_.empty()) {
+			geometry_msgs::TransformStamped marker_transform;
+			marker_transform.header.frame_id = markers_parent_frame_;
+			marker_transform.child_frame_id = markers_frame_ + std::to_string(id);
+			tf::transformTFToMsg(transform, marker_transform.transform);
+			markers_transforms_.push_back(marker_transform);
+		}
+
+		// Add marker to array
+		aruco_pose::Marker marker;
+		marker.id = id;
+		marker.length = length;
+		marker.pose.position.x = x;
+		marker.pose.position.y = y;
+		marker.pose.position.z = z;
+		tf::quaternionTFToMsg(q, marker.pose.orientation);
+		markers_.markers.push_back(marker);
+
+		// Add visualization marker
+		visualization_msgs::Marker vis_marker;
+		vis_marker.header.frame_id = transform_.child_frame_id;
+		vis_marker.action = visualization_msgs::Marker::ADD;
+		vis_marker.id = vis_array_.markers.size();
+		vis_marker.ns = "aruco_map_marker";
+		vis_marker.type = visualization_msgs::Marker::CUBE;
+		vis_marker.scale.x = length;
+		vis_marker.scale.y = length;
+		vis_marker.scale.z = 0.001;
+		vis_marker.color.r = 1;
+		vis_marker.color.g = 0.5;
+		vis_marker.color.b = 0.5;
+		vis_marker.color.a = 0.8;
+		vis_marker.pose.position.x = x;
+		vis_marker.pose.position.y = y;
+		vis_marker.pose.position.z = z;
+		tf::quaternionTFToMsg(q, marker.pose.orientation);
+		vis_marker.frame_locked = true;
+		vis_array_.markers.push_back(vis_marker);
+
+		// Add linking line
+		// geometry_msgs::Point p;
+		// p.x = x;
+		// p.y = y;
+		// p.z = z;
+		// vis_array_.markers.at(0).points.push_back(p);
+	}
+
+	void publishMarkersFrames()
+	{
+		if (!markers_transforms_.empty()) {
+			static_br_.sendTransform(markers_transforms_);
+		}
+	}
+
+	void publishMarkers()
+	{
+		markers_pub_.publish(markers_);
+	}
+
+	void publishMapImage()
+	{
+		cv::Size size(image_width_, image_height_);
+		cv::Mat image;
+		cv_bridge::CvImage msg;
+
+		if (!board_->ids.empty()) {
+			_drawPlanarBoard(board_, size, image, image_margin_, 1, image_axis_);
+			msg.encoding = image_axis_ ? sensor_msgs::image_encodings::RGB8 : sensor_msgs::image_encodings::MONO8;
+		} else {
+			// empty map
+			image.create(size, CV_8UC1);
+			image.setTo(cv::Scalar::all(255));
+			msg.encoding = sensor_msgs::image_encodings::MONO8;
+		}
+
+		msg.image = image;
+		img_pub_.publish(msg.toImageMsg());
+	}
+};
+
+PLUGINLIB_EXPORT_CLASS(ArucoMap, nodelet::Nodelet)

aruco_pose/src/aruco_pose.cpp

@@ -1,350 +0,0 @@
-#include <algorithm>
-#include <nodelet/nodelet.h>
-#include <image_transport/image_transport.h>
-#include <cv_bridge/cv_bridge.h>
-#include <opencv2/imgproc/imgproc.hpp>
-#include <pluginlib/class_list_macros.h>
-#include <geometry_msgs/TransformStamped.h>
-#include <geometry_msgs/PoseStamped.h>
-#include <geometry_msgs/PoseWithCovarianceStamped.h>
-#include <visualization_msgs/MarkerArray.h>
-#include <tf/transform_datatypes.h>
-#include <opencv2/calib3d/calib3d.hpp>
-#include <opencv2/opencv.hpp>
-#include <opencv2/aruco.hpp>
-#include <opencv2/aruco/dictionary.hpp>
-#include <stdio.h>
-#include <tf/transform_broadcaster.h>
-
-#include "util.h"
-
-using std::vector;
-using std::string;
-
-namespace aruco_pose {
-
-class ArucoPose : public nodelet::Nodelet {
-    tf::TransformBroadcaster br;
-    cv::Ptr<cv::aruco::Dictionary> dictionary;
-    cv::Ptr<cv::aruco::DetectorParameters> parameters;
-    cv::Ptr<cv::aruco::Board> board;
-    std::string frame_id_;
-    image_transport::CameraSubscriber img_sub;
-    image_transport::Publisher img_pub;
-    ros::Publisher marker_pub;
-    ros::Publisher pose_pub;
-    ros::NodeHandle nh_, nh_priv_;
-
-    virtual void onInit();
-    void createBoard();
-    cv::Point3f getObjPointsCenter(cv::Mat objPoints);
-    void detect(const sensor_msgs::ImageConstPtr&, const sensor_msgs::CameraInfoConstPtr&);
-    void parseCameraInfo(const sensor_msgs::CameraInfoConstPtr&, cv::Mat&, cv::Mat&);
-    tf::Transform aruco2tf(cv::Mat rvec, cv::Mat tvec);
-};
-
-void ArucoPose::onInit() {
-    ROS_INFO("Initializing aruco_pose");
-    nh_ = getNodeHandle();
-    nh_priv_ = getPrivateNodeHandle();
-
-    nh_priv_.param("frame_id", frame_id_, std::string("aruco_map"));
-
-    dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_1000);
-    parameters = cv::aruco::DetectorParameters::create();
-
-    try
-    {
-        createBoard();
-    }
-    catch (const std::exception &exc)
-    {
-        std::cerr << exc.what();
-        exit(0);
-    }
-
-    image_transport::ImageTransport it(nh_);
-    img_sub = it.subscribeCamera("image", 1, &ArucoPose::detect, this);
-
-    image_transport::ImageTransport it_priv(nh_priv_);
-    img_pub = it_priv.advertise("debug", 1);
-
-    pose_pub = nh_priv_.advertise<geometry_msgs::PoseStamped>("pose", 1);
-
-    ROS_INFO("aruco_pose nodelet inited");
-}
-
-cv::Ptr<cv::aruco::Board> createCustomGridBoard(int markersX, int markersY, float markerLength, float markerSeparationX, float markerSeparationY,
-                            const cv::Ptr<cv::aruco::Dictionary> &dictionary, std::vector<int> ids) {
-
-    CV_Assert(markersX > 0 && markersY > 0 && markerLength > 0 && markerSeparationX > 0 && markerSeparationY > 0);
-
-    cv::Ptr<cv::aruco::Board> res = cv::makePtr<cv::aruco::Board>();
-
-    res->dictionary = dictionary;
-
-    size_t totalMarkers = (size_t) markersX * markersY;
-    res->ids = ids;
-    res->objPoints.reserve(totalMarkers);
-
-    // calculate Board objPoints
-    float maxY = (float)markersY * markerLength + (markersY - 1) * markerSeparationY;
-    for(int y = 0; y < markersY; y++) {
-        for(int x = 0; x < markersX; x++) {
-            std::vector< cv::Point3f > corners;
-            corners.resize(4);
-            corners[0] = cv::Point3f(x * (markerLength + markerSeparationX),
-                                 maxY - y * (markerLength + markerSeparationY), 0);
-            corners[1] = corners[0] + cv::Point3f(markerLength, 0, 0);
-            corners[2] = corners[0] + cv::Point3f(markerLength, -markerLength, 0);
-            corners[3] = corners[0] + cv::Point3f(0, -markerLength, 0);
-            res->objPoints.push_back(corners);
-        }
-    }
-
-    return res;
-}
-
-cv::Ptr<cv::aruco::Board> createCustomBoard(std::map<string, string> markers, const cv::Ptr<cv::aruco::Dictionary> &dictionary) {
-    cv::Ptr<cv::aruco::Board> res = cv::makePtr<cv::aruco::Board>();
-
-    res->dictionary = dictionary;
-
-    size_t total_markers = markers.size();
-    res->ids.reserve(total_markers);
-    res->objPoints.reserve(total_markers);
-
-    // Generate ids and objPoints
-    for(auto const &marker : markers) {
-        res->ids.push_back(std::stoi(marker.first));
-
-        vector<string> parts;
-        parts = strSplit(marker.second, " ");
-
-        float size = std::stof(parts.at(0));
-        float x = std::stof(parts.at(1));
-        float y = std::stof(parts.at(2));
-        float z = std::stof(parts.at(3));
-        float yaw = std::stof(parts.at(4));
-        float pitch = std::stof(parts.at(5));
-        float roll = std::stof(parts.at(6));
-
-        vector<cv::Point3f> corners;
-        corners.resize(4);
-        corners[0] = cv::Point3f(x - size / 2, y + size / 2, 0);
-        corners[1] = corners[0] + cv::Point3f(size, 0, 0);
-        corners[2] = corners[0] + cv::Point3f(size, -size, 0);
-        corners[3] = corners[0] + cv::Point3f(0, -size, 0);
-
-        // TODO: process yaw, pitch, roll
-
-        res->objPoints.push_back(corners);
-    }
-
-    return res;
-}
-
-#include "fix.cpp"
-
-void ArucoPose::createBoard()
-{
-    static auto map_image_pub = nh_priv_.advertise<sensor_msgs::Image>("map_image", 1, true);
-    cv_bridge::CvImage map_image_msg;
-    cv::Mat map_image;
-
-    std::string type;
-
-    nh_priv_.param<std::string>("type", type, "gridboard");
-    if (type == "gridboard")
-    {
-        ROS_INFO("Initialize gridboard");
-
-        int markers_x, markers_y, first_marker;
-        float 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);
-
-        if (!nh_priv_.getParam("markers_side", markers_side))
-        {
-            ROS_ERROR("gridboard: required parameter ~markers_side is not set.");
-            exit(1);
-        }
-
-        if (!nh_priv_.getParam("markers_sep_x", markers_sep_x))
-        {
-            if (!nh_priv_.getParam("markers_sep", markers_sep_x))
-            {
-               ROS_ERROR("gridboard: ~markers_sep_x or ~markers_sep parameters are required");
-               exit(1);
-            }
-        }
-
-        if (!nh_priv_.getParam("markers_sep_y", markers_sep_y))
-        {
-            if (!nh_priv_.getParam("markers_sep", markers_sep_y))
-            {
-               ROS_ERROR("gridboard: ~markers_sep_y or ~markers_sep parameters are required");
-               exit(1);
-            }
-        }
-
-        if (nh_priv_.getParam("marker_ids", marker_ids))
-        {
-            if (markers_x * markers_y != marker_ids.size())
-            {
-                ROS_FATAL("~marker_ids length should be equal to ~markers_x * ~markers_y");
-                exit(1);
-            }
-        }
-        else
-        {
-            // Fill marker_ids automatically
-            marker_ids.resize(markers_x * markers_y);
-            for(int i = 0; i < markers_x * markers_y; i++)
-            {
-                marker_ids.at(i) = first_marker++;
-            }
-        }
-
-        // Create grid board
-        board = createCustomGridBoard(markers_x, markers_y, markers_side, markers_sep_x, markers_sep_y, dictionary, marker_ids);
-
-        // Publish map image for debugging
-        _drawPlanarBoard(board,  cv::Size(2000, 2000), map_image, 50, 1);
-
-        cv::cvtColor(map_image, map_image, CV_GRAY2BGR);
-
-        map_image_msg.encoding = sensor_msgs::image_encodings::BGR8;
-        map_image_msg.image = map_image;
-        map_image_pub.publish(map_image_msg.toImageMsg());
-    }
-    else if (type == "custom")
-    {
-        ROS_INFO("Initialize a custom board");
-
-        std::map<string, string> markers;
-        nh_priv_.getParam("markers", markers);
-
-        board = createCustomBoard(markers, dictionary);
-
-        ROS_INFO("Draw a custom board");
-        // Publish map image for debugging
-        _drawPlanarBoard(board,  cv::Size(2000, 2000), map_image, 50, 1);
-
-        cv::cvtColor(map_image, map_image, CV_GRAY2BGR);
-
-        map_image_msg.encoding = sensor_msgs::image_encodings::BGR8;
-        map_image_msg.image = map_image;
-        map_image_pub.publish(map_image_msg.toImageMsg());
-    }
-    else
-    {
-        ROS_ERROR("Incorrect map type '%s'", type.c_str());
-    }
-}
-
-cv::Point3f ArucoPose::getObjPointsCenter(cv::Mat objPoints) {
-    float min_x = std::numeric_limits<float>::max();
-    float max_x = std::numeric_limits<float>::min();
-    float min_y = min_x, max_y = max_x;
-    for (int i = 0; i < objPoints.rows; i++) {
-        max_x = std::max(max_x, objPoints.at<float>(i, 0));
-        max_y = std::max(max_y, objPoints.at<float>(i, 1));
-        min_x = std::min(min_x, objPoints.at<float>(i, 0));
-        min_y = std::min(min_y, objPoints.at<float>(i, 1));
-    }
-    cv::Point3f res((min_x + max_x) / 2, (min_y + max_y) / 2, 0);
-    return res;
-}
-
-void ArucoPose::detect(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr &cinfo) {
-    cv::Mat image = cv_bridge::toCvShare(msg, "bgr8")->image;
-
-    std::vector<int> markerIds;
-    std::vector<std::vector<cv::Point2f>> markerCorners;
-    std::vector<std::vector<cv::Point2f>> rejectedCandidates;
-
-    cv::aruco::detectMarkers(image, dictionary, markerCorners, markerIds, parameters, rejectedCandidates);
-
-    cv::Mat cameraMatrix(3, 3, CV_64F);
-    cv::Mat distCoeffs(8, 1, CV_64F);
-    parseCameraInfo(cinfo, cameraMatrix, distCoeffs);
-
-    int valid = 0;
-    cv::Mat rvec, tvec, objPoints;
-
-    if (markerIds.size() > 0) {
-
-        valid = _estimatePoseBoard(markerCorners, markerIds, board, cameraMatrix, distCoeffs,
-                                       rvec, tvec, false, objPoints);
-
-        if (valid) {
-            // Send map transform
-            tf::StampedTransform transform(aruco2tf(rvec, tvec), msg->header.stamp, cinfo->header.frame_id, frame_id_);
-            br.sendTransform(transform);
-
-            // Publish map pose
-            static geometry_msgs::PoseStamped ps;
-            ps.header.frame_id = frame_id_;
-            ps.header.stamp = msg->header.stamp;
-            ps.pose.orientation.w = 1;
-            pose_pub.publish(ps);
-
-            // Send reference point
-            cv::Point3f ref = getObjPointsCenter(objPoints);
-            tf::Vector3 ref_vector3 = tf::Vector3(ref.x, ref.y, ref.z);
-            tf::Quaternion q(0, 0, 0);
-            static tf::StampedTransform ref_transform;
-            ref_transform.stamp_ = msg->header.stamp;
-            ref_transform.frame_id_ = frame_id_;
-            ref_transform.child_frame_id_ = "aruco_map_reference";
-            ref_transform.setOrigin(ref_vector3);
-            ref_transform.setRotation(q);
-            br.sendTransform(ref_transform);
-        }
-    }
-
-    if (img_pub.getNumSubscribers() > 0)
-    {
-        cv::aruco::drawDetectedMarkers(image, markerCorners, markerIds); // draw markers
-        if (valid)
-        {
-            cv::aruco::drawAxis(image, cameraMatrix, distCoeffs, rvec, tvec, 0.3); // draw board axis
-        }
-        cv_bridge::CvImage out_msg;
-        out_msg.header.frame_id = msg->header.frame_id;
-        out_msg.header.stamp = msg->header.stamp;
-        out_msg.encoding = sensor_msgs::image_encodings::BGR8;
-        out_msg.image = image;
-        img_pub.publish(out_msg.toImageMsg());
-    }
-}
-
-void ArucoPose::parseCameraInfo(const sensor_msgs::CameraInfoConstPtr &cinfo, cv::Mat &cameraMat, cv::Mat &distCoeffs) {
-    for (int i = 0; i < 3; ++i) {
-        for (int j = 0; j < 3; ++j) {
-            cameraMat.at<double>(i, j) = cinfo->K[3 * i + j];
-        }
-    }
-    for (int k = 0; k < cinfo->D.size(); k++) {
-        distCoeffs.at<double>(k) = cinfo->D[k];
-    }
-}
-
-tf::Transform ArucoPose::aruco2tf(cv::Mat rvec, cv::Mat tvec) {
-
-    cv::Mat rot;
-    cv::Rodrigues(rvec, rot);
-
-    tf::Matrix3x3 tf_rot(rot.at<double>(0,0), rot.at<double>(0,1), rot.at<double>(0,2),
-                         rot.at<double>(1,0), rot.at<double>(1,1), rot.at<double>(1,2),
-                         rot.at<double>(2,0), rot.at<double>(2,1), rot.at<double>(2,2));
-    tf::Vector3 tf_orig(tvec.at<double>(0,0), tvec.at<double>(1,0), tvec.at<double>(2,0));
-    return tf::Transform(tf_rot, tf_orig);
-}
-
-PLUGINLIB_EXPORT_CLASS(ArucoPose, nodelet::Nodelet)
-
-}

aruco_pose/src/draw.cpp

@@ -0,0 +1,832 @@
+// This code is basically taken from https://github.com/opencv/opencv_contrib/blob/master/modules/aruco/src/aruco.cpp
+// with some improvements and fixes
+
+#include "draw.h"
+#include <math.h>
+
+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) {
+
+	CV_Assert(outSize.area() > 0);
+	CV_Assert(marginSize >= 0);
+
+	_img.create(outSize, drawAxis ? CV_8UC3 : CV_8UC1);
+	Mat out = _img.getMat();
+	out.setTo(Scalar::all(255));
+	out.adjustROI(-marginSize, -marginSize, -marginSize, -marginSize);
+
+	// calculate max and min values in XY plane
+	CV_Assert(_board->objPoints.size() > 0);
+	float minX, maxX, minY, maxY;
+	minX = maxX = _board->objPoints[0][0].x;
+	minY = maxY = _board->objPoints[0][0].y;
+
+	for(unsigned int i = 0; i < _board->objPoints.size(); i++) {
+		for(int j = 0; j < 4; j++) {
+			minX = min(minX, _board->objPoints[i][j].x);
+			maxX = max(maxX, _board->objPoints[i][j].x);
+			minY = min(minY, _board->objPoints[i][j].y);
+			maxY = max(maxY, _board->objPoints[i][j].y);
+		}
+	}
+
+	float sizeX = maxX - minX;
+	float sizeY = maxY - minY;
+
+	// proportion transformations
+	float xReduction = sizeX / float(out.cols);
+	float yReduction = sizeY / float(out.rows);
+
+	// determine the zone where the markers are placed
+	if(xReduction > yReduction) {
+		int nRows = int(sizeY / xReduction);
+		int rowsMargins = (out.rows - nRows) / 2;
+		out.adjustROI(-rowsMargins, -rowsMargins, 0, 0);
+	} else {
+		int nCols = int(sizeX / yReduction);
+		int colsMargins = (out.cols - nCols) / 2;
+		out.adjustROI(0, 0, -colsMargins, -colsMargins);
+	}
+
+	// now paint each marker
+	Dictionary &dictionary = *(_board->dictionary);
+	Mat marker;
+	Point2f outCorners[3];
+	Point2f inCorners[3];
+	for(unsigned int m = 0; m < _board->objPoints.size(); m++) {
+		// transform corners to markerZone coordinates
+		for(int j = 0; j < 3; j++) {
+			Point2f pf = Point2f(_board->objPoints[m][j].x, _board->objPoints[m][j].y);
+			// move top left to 0, 0
+			pf -= Point2f(minX, minY);
+			pf.x = pf.x / sizeX * float(out.cols);
+			pf.y = (1.0f - pf.y / sizeY) * float(out.rows);
+			outCorners[j] = pf;
+		}
+
+		// get marker
+		Size dst_sz(outCorners[2] - outCorners[0]); // assuming CCW order
+		// dst_sz.width = dst_sz.height = std::min(dst_sz.width, dst_sz.height); //marker should be square
+		double diag = std::round(std::hypot(dst_sz.width, dst_sz.height));
+		int side = std::round(diag / std::sqrt(2));
+		side = std::max(side, 10);
+
+		dictionary.drawMarker(_board->ids[m], side, marker, borderBits);
+		if (drawAxis) {
+			cvtColor(marker, marker, COLOR_GRAY2RGB);
+		}
+
+		// interpolate tiny marker to marker position in markerZone
+		inCorners[0] = Point2f(-0.5f, -0.5f);
+		inCorners[1] = Point2f(marker.cols - 0.5f, -0.5f);
+		inCorners[2] = Point2f(marker.cols - 0.5f, marker.rows - 0.5f);
+
+		// remove perspective
+		Mat transformation = getAffineTransform(inCorners, outCorners);
+		warpAffine(marker, out, transformation, out.size(), INTER_LINEAR,
+						BORDER_TRANSPARENT);
+	}
+
+	// draw axis
+	if (drawAxis) {
+		Size wholeSize; Point ofs;
+		out.locateROI(wholeSize, ofs);
+		auto out_copy = _img.getMat();
+
+		cv::Point center(ofs.x - minX / sizeX * float(out.cols), ofs.y + out.rows + minY / sizeY * float(out.rows));
+
+		int axis_points[3][2] = {{300, 0}, {0, -300}, {-150, 150}};
+		Point axis_names[3] = {Point(270, 50), Point(25, -270), Point(-160, 115)};
+		Scalar colors[] = {Scalar(255, 0, 0), Scalar(0, 255, 0), Scalar(0, 0, 255)};
+		String names[] = {"X", "Y", "Z"};
+
+		int r_half = 14;
+		int height = 55;
+
+		for(int poly = 2; poly >= 0; poly--){
+			double alpha = atan2(0 - axis_points[poly][0], 0 - axis_points[poly][1]);
+			float x_delta = r_half * cos(alpha);
+			float y_delta = r_half * sin(alpha);
+
+			Point polygon_vertices[1][3];
+			polygon_vertices[0][0] = center + Point(axis_points[poly][0] + x_delta, axis_points[poly][1] - y_delta);
+			polygon_vertices[0][1] = center + Point(axis_points[poly][0] - x_delta, axis_points[poly][1] + y_delta);
+			polygon_vertices[0][2] = center + Point(axis_points[poly][0] - sin(alpha) * height, axis_points[poly][1] - cos(alpha) * height);
+
+			const Point* ppt[1] = {polygon_vertices[0]};
+			int npt[] = {3};
+
+			fillPoly(out_copy, ppt, npt, 1, colors[poly]);
+			putText(out_copy, names[poly], center + axis_names[poly], FONT_HERSHEY_SIMPLEX, 1.2, colors[poly], 7);
+			line(out_copy, center, center + Point(axis_points[poly][0], axis_points[poly][1]), colors[poly], 10);
+		}
+	}
+}
+
+/* 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)
+{
+	// If both points are behind the screen, don't draw anything
+	if (p1.z <= 0 && p2.z <= 0) {
+		return;
+	}
+	Point2f p1p{p1.x, p1.y};
+	Point2f p2p{p2.x, p2.y};
+	// If points are on the different sides of the plane, compute intersection point
+	if (p1.z * p2.z < 0) {
+		// Compute intersection point with the screen
+		// We denote alpha as such:
+		// xi = (1 - alpha) * x1 + alpha * x2
+		// yi = (1 - alpha) * y1 + alpha * y2
+		// zi = (1 - alpha) * z1 + alpha * z2 = 0
+		// Thus, alpha can be expressed as
+		// alpha = z1 / (z1 - z2)
+		float alpha = p1.z / (p1.z - p2.z);
+		Point2f pi{(1 - alpha) * p1.x + alpha * p2.x, (1 - alpha) * p1.y + alpha * p2.y};
+		// Now, if z1 is negative, we draw the line from (xi, yi) to (x2, y2), else we draw from (x1, y1) to (xi, yi)
+		if (p1.z < 0) {
+			p1p = pi;
+		} else {
+			p2p = pi;
+		}
+	}
+	line(image, p1p, p2p, color, thickness, lineType, shift);
+}
+
+void _drawAxis(InputOutputArray _image, InputArray _cameraMatrix, InputArray _distCoeffs,
+              InputArray _rvec, InputArray _tvec, float length) {
+
+	CV_Assert(_image.getMat().total() != 0 &&
+			  (_image.getMat().channels() == 1 || _image.getMat().channels() == 3));
+	CV_Assert(length > 0);
+
+	// project axis points
+	std::vector<Point3f> axisPoints;
+	axisPoints.push_back(Point3f(0, 0, 0));
+	axisPoints.push_back(Point3f(length, 0, 0));
+	axisPoints.push_back(Point3f(0, length, 0));
+	axisPoints.push_back(Point3f(0, 0, length));
+	std::vector<Point3f> imagePointsZ;
+	_projectPoints(axisPoints, _rvec, _tvec, _cameraMatrix, _distCoeffs, imagePointsZ);
+
+	// draw axis lines
+	linePartial(_image, imagePointsZ[0], imagePointsZ[1], Scalar(0, 0, 255), 3);
+	linePartial(_image, imagePointsZ[0], imagePointsZ[2], Scalar(0, 255, 0), 3);
+	linePartial(_image, imagePointsZ[0], imagePointsZ[3], Scalar(255, 0, 0), 3);
+}
+
+static CvMat _cvMat(const cv::Mat& m)
+{
+	CvMat self;
+	CV_DbgAssert(m.dims <= 2);
+	self = cvMat(m.rows, m.dims == 1 ? 1 : m.cols, m.type(), m.data);
+	self.step = (int)m.step[0];
+	self.type = (self.type & ~cv::Mat::CONTINUOUS_FLAG) | (m.flags & cv::Mat::CONTINUOUS_FLAG);
+	return self;
+}
+
+static void _projectPoints( InputArray _opoints,
+                        InputArray _rvec,
+                        InputArray _tvec,
+                        InputArray _cameraMatrix,
+                        InputArray _distCoeffs,
+                        OutputArray _ipoints,
+                        OutputArray _jacobian,
+                        double aspectRatio )
+{
+	Mat opoints = _opoints.getMat();
+	int npoints = opoints.checkVector(3), depth = opoints.depth();
+	CV_Assert(npoints >= 0 && (depth == CV_32F || depth == CV_64F));
+
+	CvMat dpdrot, dpdt, dpdf, dpdc, dpddist;
+	CvMat *pdpdrot = 0, *pdpdt = 0, *pdpdf = 0, *pdpdc = 0, *pdpddist = 0;
+
+	CV_Assert(_ipoints.needed());
+
+	_ipoints.create(npoints, 1, CV_MAKETYPE(depth, 3), -1, true);
+	Mat imagePoints = _ipoints.getMat();
+	CvMat c_imagePoints = _cvMat(imagePoints);
+	CvMat c_objectPoints = _cvMat(opoints);
+	Mat cameraMatrix = _cameraMatrix.getMat();
+
+	Mat rvec = _rvec.getMat(), tvec = _tvec.getMat();
+	CvMat c_cameraMatrix = _cvMat(cameraMatrix);
+	CvMat c_rvec = _cvMat(rvec), c_tvec = _cvMat(tvec);
+
+	double dc0buf[5] = {0};
+	Mat dc0(5, 1, CV_64F, dc0buf);
+	Mat distCoeffs = _distCoeffs.getMat();
+	if (distCoeffs.empty())
+		distCoeffs = dc0;
+	CvMat c_distCoeffs = _cvMat(distCoeffs);
+	int ndistCoeffs = distCoeffs.rows + distCoeffs.cols - 1;
+
+	Mat jacobian;
+	if (_jacobian.needed())
+	{
+		_jacobian.create(npoints * 2, 3 + 3 + 2 + 2 + ndistCoeffs, CV_64F);
+		jacobian = _jacobian.getMat();
+		pdpdrot = &(dpdrot = _cvMat(jacobian.colRange(0, 3)));
+		pdpdt = &(dpdt = _cvMat(jacobian.colRange(3, 6)));
+		pdpdf = &(dpdf = _cvMat(jacobian.colRange(6, 8)));
+		pdpdc = &(dpdc = _cvMat(jacobian.colRange(8, 10)));
+		pdpddist = &(dpddist = _cvMat(jacobian.colRange(10, 10 + ndistCoeffs)));
+	}
+
+	_cvProjectPoints2(&c_objectPoints, &c_rvec, &c_tvec, &c_cameraMatrix, &c_distCoeffs,
+					  &c_imagePoints, pdpdrot, pdpdt, pdpdf, pdpdc, pdpddist, aspectRatio);
+}
+
+namespace _detail
+{
+    template <typename FLOAT>
+    void computeTiltProjectionMatrix(FLOAT tauX,
+                                     FLOAT tauY,
+                                     Matx<FLOAT, 3, 3>* matTilt = 0,
+                                     Matx<FLOAT, 3, 3>* dMatTiltdTauX = 0,
+                                     Matx<FLOAT, 3, 3>* dMatTiltdTauY = 0,
+                                     Matx<FLOAT, 3, 3>* invMatTilt = 0)
+    {
+        FLOAT cTauX = cos(tauX);
+        FLOAT sTauX = sin(tauX);
+        FLOAT cTauY = cos(tauY);
+        FLOAT sTauY = sin(tauY);
+        Matx<FLOAT, 3, 3> matRotX = Matx<FLOAT, 3, 3>(1,0,0,0,cTauX,sTauX,0,-sTauX,cTauX);
+        Matx<FLOAT, 3, 3> matRotY = Matx<FLOAT, 3, 3>(cTauY,0,-sTauY,0,1,0,sTauY,0,cTauY);
+        Matx<FLOAT, 3, 3> matRotXY = matRotY * matRotX;
+        Matx<FLOAT, 3, 3> matProjZ = Matx<FLOAT, 3, 3>(matRotXY(2,2),0,-matRotXY(0,2),0,matRotXY(2,2),-matRotXY(1,2),0,0,1);
+        if (matTilt)
+        {
+            // Matrix for trapezoidal distortion of tilted image sensor
+            *matTilt = matProjZ * matRotXY;
+        }
+        if (dMatTiltdTauX)
+        {
+            // Derivative with respect to tauX
+            Matx<FLOAT, 3, 3> dMatRotXYdTauX = matRotY * Matx<FLOAT, 3, 3>(0,0,0,0,-sTauX,cTauX,0,-cTauX,-sTauX);
+            Matx<FLOAT, 3, 3> dMatProjZdTauX = Matx<FLOAT, 3, 3>(dMatRotXYdTauX(2,2),0,-dMatRotXYdTauX(0,2),
+                                                                 0,dMatRotXYdTauX(2,2),-dMatRotXYdTauX(1,2),0,0,0);
+            *dMatTiltdTauX = (matProjZ * dMatRotXYdTauX) + (dMatProjZdTauX * matRotXY);
+        }
+        if (dMatTiltdTauY)
+        {
+            // Derivative with respect to tauY
+            Matx<FLOAT, 3, 3> dMatRotXYdTauY = Matx<FLOAT, 3, 3>(-sTauY,0,-cTauY,0,0,0,cTauY,0,-sTauY) * matRotX;
+            Matx<FLOAT, 3, 3> dMatProjZdTauY = Matx<FLOAT, 3, 3>(dMatRotXYdTauY(2,2),0,-dMatRotXYdTauY(0,2),
+                                                                 0,dMatRotXYdTauY(2,2),-dMatRotXYdTauY(1,2),0,0,0);
+            *dMatTiltdTauY = (matProjZ * dMatRotXYdTauY) + (dMatProjZdTauY * matRotXY);
+        }
+        if (invMatTilt)
+        {
+            FLOAT inv = 1./matRotXY(2,2);
+            Matx<FLOAT, 3, 3> invMatProjZ = Matx<FLOAT, 3, 3>(inv,0,inv*matRotXY(0,2),0,inv,inv*matRotXY(1,2),0,0,1);
+            *invMatTilt = matRotXY.t()*invMatProjZ;
+        }
+    }
+}
+
+static const char* cvDistCoeffErr = "Distortion coefficients must be 1x4, 4x1, 1x5, 5x1, 1x8, 8x1, 1x12, 12x1, 1x14 or 14x1 floating-point vector";
+
+static void _cvProjectPoints2Internal( const CvMat* objectPoints,
+                                      const CvMat* r_vec,
+                                      const CvMat* t_vec,
+                                      const CvMat* A,
+                                      const CvMat* distCoeffs,
+                                      CvMat* imagePoints, CvMat* dpdr CV_DEFAULT(NULL),
+                                      CvMat* dpdt CV_DEFAULT(NULL), CvMat* dpdf CV_DEFAULT(NULL),
+                                      CvMat* dpdc CV_DEFAULT(NULL), CvMat* dpdk CV_DEFAULT(NULL),
+                                      CvMat* dpdo CV_DEFAULT(NULL),
+                                      double aspectRatio CV_DEFAULT(0) )
+{
+    Ptr<CvMat> matM, _m;
+    Ptr<CvMat> _dpdr, _dpdt, _dpdc, _dpdf, _dpdk;
+    Ptr<CvMat> _dpdo;
+
+    int i, j, count;
+    int calc_derivatives;
+    const CvPoint3D64f* M;
+    CvPoint3D64f* m;
+    double r[3], R[9], dRdr[27], t[3], a[9], k[14] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, fx, fy, cx, cy;
+    Matx33d matTilt = Matx33d::eye();
+    Matx33d dMatTiltdTauX(0,0,0,0,0,0,0,-1,0);
+    Matx33d dMatTiltdTauY(0,0,0,0,0,0,1,0,0);
+    CvMat _r, _t, _a = cvMat( 3, 3, CV_64F, a ), _k;
+    CvMat matR = cvMat( 3, 3, CV_64F, R ), _dRdr = cvMat( 3, 9, CV_64F, dRdr );
+    double *dpdr_p = 0, *dpdt_p = 0, *dpdk_p = 0, *dpdf_p = 0, *dpdc_p = 0;
+    double* dpdo_p = 0;
+    int dpdr_step = 0, dpdt_step = 0, dpdk_step = 0, dpdf_step = 0, dpdc_step = 0;
+    int dpdo_step = 0;
+    bool fixedAspectRatio = aspectRatio > FLT_EPSILON;
+
+    if( !CV_IS_MAT(objectPoints) || !CV_IS_MAT(r_vec) ||
+        !CV_IS_MAT(t_vec) || !CV_IS_MAT(A) ||
+        /*!CV_IS_MAT(distCoeffs) ||*/ !CV_IS_MAT(imagePoints) )
+        CV_Error( CV_StsBadArg, "One of required arguments is not a valid matrix" );
+
+    int total = objectPoints->rows * objectPoints->cols * CV_MAT_CN(objectPoints->type);
+    if(total % 3 != 0)
+    {
+        //we have stopped support of homogeneous coordinates because it cause ambiguity in interpretation of the input data
+        CV_Error( CV_StsBadArg, "Homogeneous coordinates are not supported" );
+    }
+    count = total / 3;
+
+    if( CV_IS_CONT_MAT(objectPoints->type) &&
+        (CV_MAT_DEPTH(objectPoints->type) == CV_32F || CV_MAT_DEPTH(objectPoints->type) == CV_64F)&&
+        ((objectPoints->rows == 1 && CV_MAT_CN(objectPoints->type) == 3) ||
+         (objectPoints->rows == count && CV_MAT_CN(objectPoints->type)*objectPoints->cols == 3) ||
+         (objectPoints->rows == 3 && CV_MAT_CN(objectPoints->type) == 1 && objectPoints->cols == count)))
+    {
+        matM.reset(cvCreateMat( objectPoints->rows, objectPoints->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(objectPoints->type)) ));
+        cvConvert(objectPoints, matM);
+    }
+    else
+    {
+//        matM = cvCreateMat( 1, count, CV_64FC3 );
+//        cvConvertPointsHomogeneous( objectPoints, matM );
+        CV_Error( CV_StsBadArg, "Homogeneous coordinates are not supported" );
+    }
+
+    if( CV_IS_CONT_MAT(imagePoints->type) &&
+        (CV_MAT_DEPTH(imagePoints->type) == CV_32F || CV_MAT_DEPTH(imagePoints->type) == CV_64F) &&
+        ((imagePoints->rows == 1 && CV_MAT_CN(imagePoints->type) == 3) ||
+         (imagePoints->rows == count && CV_MAT_CN(imagePoints->type)*imagePoints->cols == 3) ||
+         (imagePoints->rows == 3 && CV_MAT_CN(imagePoints->type) == 1 && imagePoints->cols == count)))
+    {
+        _m.reset(cvCreateMat( imagePoints->rows, imagePoints->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(imagePoints->type)) ));
+        cvConvert(imagePoints, _m);
+    }
+    else
+    {
+//        _m = cvCreateMat( 1, count, CV_64FC2 );
+        CV_Error( CV_StsBadArg, "Homogeneous coordinates are not supported" );
+    }
+
+    M = (CvPoint3D64f*)matM->data.db;
+    m = (CvPoint3D64f*)_m->data.db;
+
+    if( (CV_MAT_DEPTH(r_vec->type) != CV_64F && CV_MAT_DEPTH(r_vec->type) != CV_32F) ||
+        (((r_vec->rows != 1 && r_vec->cols != 1) ||
+          r_vec->rows*r_vec->cols*CV_MAT_CN(r_vec->type) != 3) &&
+         ((r_vec->rows != 3 && r_vec->cols != 3) || CV_MAT_CN(r_vec->type) != 1)))
+        CV_Error( CV_StsBadArg, "Rotation must be represented by 1x3 or 3x1 "
+                                "floating-point rotation vector, or 3x3 rotation matrix" );
+
+    if( r_vec->rows == 3 && r_vec->cols == 3 )
+    {
+        _r = cvMat( 3, 1, CV_64FC1, r );
+        cvRodrigues2( r_vec, &_r );
+        cvRodrigues2( &_r, &matR, &_dRdr );
+        cvCopy( r_vec, &matR );
+    }
+    else
+    {
+        _r = cvMat( r_vec->rows, r_vec->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(r_vec->type)), r );
+        cvConvert( r_vec, &_r );
+        cvRodrigues2( &_r, &matR, &_dRdr );
+    }
+
+    if( (CV_MAT_DEPTH(t_vec->type) != CV_64F && CV_MAT_DEPTH(t_vec->type) != CV_32F) ||
+        (t_vec->rows != 1 && t_vec->cols != 1) ||
+        t_vec->rows*t_vec->cols*CV_MAT_CN(t_vec->type) != 3 )
+        CV_Error( CV_StsBadArg,
+                  "Translation vector must be 1x3 or 3x1 floating-point vector" );
+
+    _t = cvMat( t_vec->rows, t_vec->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(t_vec->type)), t );
+    cvConvert( t_vec, &_t );
+
+    if( (CV_MAT_TYPE(A->type) != CV_64FC1 && CV_MAT_TYPE(A->type) != CV_32FC1) ||
+        A->rows != 3 || A->cols != 3 )
+        CV_Error( CV_StsBadArg, "Instrinsic parameters must be 3x3 floating-point matrix" );
+
+    cvConvert( A, &_a );
+    fx = a[0]; fy = a[4];
+    cx = a[2]; cy = a[5];
+
+    if( fixedAspectRatio )
+        fx = fy*aspectRatio;
+
+    if( distCoeffs )
+    {
+        if( !CV_IS_MAT(distCoeffs) ||
+            (CV_MAT_DEPTH(distCoeffs->type) != CV_64F &&
+             CV_MAT_DEPTH(distCoeffs->type) != CV_32F) ||
+            (distCoeffs->rows != 1 && distCoeffs->cols != 1) ||
+            (distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 4 &&
+             distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 5 &&
+             distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 8 &&
+             distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 12 &&
+             distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 14) )
+            CV_Error( CV_StsBadArg, cvDistCoeffErr );
+
+        _k = cvMat( distCoeffs->rows, distCoeffs->cols,
+                    CV_MAKETYPE(CV_64F,CV_MAT_CN(distCoeffs->type)), k );
+        cvConvert( distCoeffs, &_k );
+        if(k[12] != 0 || k[13] != 0)
+        {
+            _detail::computeTiltProjectionMatrix(k[12], k[13],
+                                                &matTilt, &dMatTiltdTauX, &dMatTiltdTauY);
+        }
+    }
+
+    if( dpdr )
+    {
+        if( !CV_IS_MAT(dpdr) ||
+            (CV_MAT_TYPE(dpdr->type) != CV_32FC1 &&
+             CV_MAT_TYPE(dpdr->type) != CV_64FC1) ||
+            dpdr->rows != count*2 || dpdr->cols != 3 )
+            CV_Error( CV_StsBadArg, "dp/drot must be 2Nx3 floating-point matrix" );
+
+        if( CV_MAT_TYPE(dpdr->type) == CV_64FC1 )
+        {
+            _dpdr.reset(cvCloneMat(dpdr));
+        }
+        else
+            _dpdr.reset(cvCreateMat( 2*count, 3, CV_64FC1 ));
+        dpdr_p = _dpdr->data.db;
+        dpdr_step = _dpdr->step/sizeof(dpdr_p[0]);
+    }
+
+    if( dpdt )
+    {
+        if( !CV_IS_MAT(dpdt) ||
+            (CV_MAT_TYPE(dpdt->type) != CV_32FC1 &&
+             CV_MAT_TYPE(dpdt->type) != CV_64FC1) ||
+            dpdt->rows != count*2 || dpdt->cols != 3 )
+            CV_Error( CV_StsBadArg, "dp/dT must be 2Nx3 floating-point matrix" );
+
+        if( CV_MAT_TYPE(dpdt->type) == CV_64FC1 )
+        {
+            _dpdt.reset(cvCloneMat(dpdt));
+        }
+        else
+            _dpdt.reset(cvCreateMat( 2*count, 3, CV_64FC1 ));
+        dpdt_p = _dpdt->data.db;
+        dpdt_step = _dpdt->step/sizeof(dpdt_p[0]);
+    }
+
+    if( dpdf )
+    {
+        if( !CV_IS_MAT(dpdf) ||
+            (CV_MAT_TYPE(dpdf->type) != CV_32FC1 && CV_MAT_TYPE(dpdf->type) != CV_64FC1) ||
+            dpdf->rows != count*2 || dpdf->cols != 2 )
+            CV_Error( CV_StsBadArg, "dp/df must be 2Nx2 floating-point matrix" );
+
+        if( CV_MAT_TYPE(dpdf->type) == CV_64FC1 )
+        {
+            _dpdf.reset(cvCloneMat(dpdf));
+        }
+        else
+            _dpdf.reset(cvCreateMat( 2*count, 2, CV_64FC1 ));
+        dpdf_p = _dpdf->data.db;
+        dpdf_step = _dpdf->step/sizeof(dpdf_p[0]);
+    }
+
+    if( dpdc )
+    {
+        if( !CV_IS_MAT(dpdc) ||
+            (CV_MAT_TYPE(dpdc->type) != CV_32FC1 && CV_MAT_TYPE(dpdc->type) != CV_64FC1) ||
+            dpdc->rows != count*2 || dpdc->cols != 2 )
+            CV_Error( CV_StsBadArg, "dp/dc must be 2Nx2 floating-point matrix" );
+
+        if( CV_MAT_TYPE(dpdc->type) == CV_64FC1 )
+        {
+            _dpdc.reset(cvCloneMat(dpdc));
+        }
+        else
+            _dpdc.reset(cvCreateMat( 2*count, 2, CV_64FC1 ));
+        dpdc_p = _dpdc->data.db;
+        dpdc_step = _dpdc->step/sizeof(dpdc_p[0]);
+    }
+
+    if( dpdk )
+    {
+        if( !CV_IS_MAT(dpdk) ||
+            (CV_MAT_TYPE(dpdk->type) != CV_32FC1 && CV_MAT_TYPE(dpdk->type) != CV_64FC1) ||
+            dpdk->rows != count*2 || (dpdk->cols != 14 && dpdk->cols != 12 && dpdk->cols != 8 && dpdk->cols != 5 && dpdk->cols != 4 && dpdk->cols != 2) )
+            CV_Error( CV_StsBadArg, "dp/df must be 2Nx14, 2Nx12, 2Nx8, 2Nx5, 2Nx4 or 2Nx2 floating-point matrix" );
+
+        if( !distCoeffs )
+            CV_Error( CV_StsNullPtr, "distCoeffs is NULL while dpdk is not" );
+
+        if( CV_MAT_TYPE(dpdk->type) == CV_64FC1 )
+        {
+            _dpdk.reset(cvCloneMat(dpdk));
+        }
+        else
+            _dpdk.reset(cvCreateMat( dpdk->rows, dpdk->cols, CV_64FC1 ));
+        dpdk_p = _dpdk->data.db;
+        dpdk_step = _dpdk->step/sizeof(dpdk_p[0]);
+    }
+
+    if( dpdo )
+    {
+        if( !CV_IS_MAT( dpdo ) || ( CV_MAT_TYPE( dpdo->type ) != CV_32FC1
+                                    && CV_MAT_TYPE( dpdo->type ) != CV_64FC1 )
+            || dpdo->rows != count * 2 || dpdo->cols != count * 3 )
+            CV_Error( CV_StsBadArg, "dp/do must be 2Nx3N floating-point matrix" );
+
+        if( CV_MAT_TYPE( dpdo->type ) == CV_64FC1 )
+        {
+            _dpdo.reset( cvCloneMat( dpdo ) );
+        }
+        else
+            _dpdo.reset( cvCreateMat( 2 * count, 3 * count, CV_64FC1 ) );
+        cvZero(_dpdo);
+        dpdo_p = _dpdo->data.db;
+        dpdo_step = _dpdo->step / sizeof( dpdo_p[0] );
+    }
+
+    calc_derivatives = dpdr || dpdt || dpdf || dpdc || dpdk || dpdo;
+
+    for( i = 0; i < count; i++ )
+    {
+        double X = M[i].x, Y = M[i].y, Z = M[i].z;
+        double x = R[0]*X + R[1]*Y + R[2]*Z + t[0];
+        double y = R[3]*X + R[4]*Y + R[5]*Z + t[1];
+        double z = R[6]*X + R[7]*Y + R[8]*Z + t[2];
+        double r2, r4, r6, a1, a2, a3, cdist, icdist2;
+        double xd, yd, xd0, yd0, invProj;
+        Vec3d vecTilt;
+        Vec3d dVecTilt;
+        Matx22d dMatTilt;
+        Vec2d dXdYd;
+
+        double z0 = z;
+        z = z ? 1./z : 1;
+        x *= z; y *= z;
+
+        r2 = x*x + y*y;
+        r4 = r2*r2;
+        r6 = r4*r2;
+        a1 = 2*x*y;
+        a2 = r2 + 2*x*x;
+        a3 = r2 + 2*y*y;
+        cdist = 1 + k[0]*r2 + k[1]*r4 + k[4]*r6;
+        icdist2 = 1./(1 + k[5]*r2 + k[6]*r4 + k[7]*r6);
+        xd0 = x*cdist*icdist2 + k[2]*a1 + k[3]*a2 + k[8]*r2+k[9]*r4;
+        yd0 = y*cdist*icdist2 + k[2]*a3 + k[3]*a1 + k[10]*r2+k[11]*r4;
+
+        // additional distortion by projecting onto a tilt plane
+        vecTilt = matTilt*Vec3d(xd0, yd0, 1);
+        invProj = vecTilt(2) ? 1./vecTilt(2) : 1;
+        xd = invProj * vecTilt(0);
+        yd = invProj * vecTilt(1);
+
+        m[i].x = xd*fx + cx;
+        m[i].y = yd*fy + cy;
+        m[i].z = z; // Just put the projected Z coordinate here, we mainly care about the sign
+
+        if( calc_derivatives )
+        {
+            if( dpdc_p )
+            {
+                dpdc_p[0] = 1; dpdc_p[1] = 0; // dp_xdc_x; dp_xdc_y
+                dpdc_p[dpdc_step] = 0;
+                dpdc_p[dpdc_step+1] = 1;
+                dpdc_p += dpdc_step*2;
+            }
+
+            if( dpdf_p )
+            {
+                if( fixedAspectRatio )
+                {
+                    dpdf_p[0] = 0; dpdf_p[1] = xd*aspectRatio; // dp_xdf_x; dp_xdf_y
+                    dpdf_p[dpdf_step] = 0;
+                    dpdf_p[dpdf_step+1] = yd;
+                }
+                else
+                {
+                    dpdf_p[0] = xd; dpdf_p[1] = 0;
+                    dpdf_p[dpdf_step] = 0;
+                    dpdf_p[dpdf_step+1] = yd;
+                }
+                dpdf_p += dpdf_step*2;
+            }
+            for (int row = 0; row < 2; ++row)
+                for (int col = 0; col < 2; ++col)
+                    dMatTilt(row,col) = matTilt(row,col)*vecTilt(2)
+                                        - matTilt(2,col)*vecTilt(row);
+            double invProjSquare = (invProj*invProj);
+            dMatTilt *= invProjSquare;
+            if( dpdk_p )
+            {
+                dXdYd = dMatTilt*Vec2d(x*icdist2*r2, y*icdist2*r2);
+                dpdk_p[0] = fx*dXdYd(0);
+                dpdk_p[dpdk_step] = fy*dXdYd(1);
+                dXdYd = dMatTilt*Vec2d(x*icdist2*r4, y*icdist2*r4);
+                dpdk_p[1] = fx*dXdYd(0);
+                dpdk_p[dpdk_step+1] = fy*dXdYd(1);
+                if( _dpdk->cols > 2 )
+                {
+                    dXdYd = dMatTilt*Vec2d(a1, a3);
+                    dpdk_p[2] = fx*dXdYd(0);
+                    dpdk_p[dpdk_step+2] = fy*dXdYd(1);
+                    dXdYd = dMatTilt*Vec2d(a2, a1);
+                    dpdk_p[3] = fx*dXdYd(0);
+                    dpdk_p[dpdk_step+3] = fy*dXdYd(1);
+                    if( _dpdk->cols > 4 )
+                    {
+                        dXdYd = dMatTilt*Vec2d(x*icdist2*r6, y*icdist2*r6);
+                        dpdk_p[4] = fx*dXdYd(0);
+                        dpdk_p[dpdk_step+4] = fy*dXdYd(1);
+
+                        if( _dpdk->cols > 5 )
+                        {
+                            dXdYd = dMatTilt*Vec2d(
+                                    x*cdist*(-icdist2)*icdist2*r2, y*cdist*(-icdist2)*icdist2*r2);
+                            dpdk_p[5] = fx*dXdYd(0);
+                            dpdk_p[dpdk_step+5] = fy*dXdYd(1);
+                            dXdYd = dMatTilt*Vec2d(
+                                    x*cdist*(-icdist2)*icdist2*r4, y*cdist*(-icdist2)*icdist2*r4);
+                            dpdk_p[6] = fx*dXdYd(0);
+                            dpdk_p[dpdk_step+6] = fy*dXdYd(1);
+                            dXdYd = dMatTilt*Vec2d(
+                                    x*cdist*(-icdist2)*icdist2*r6, y*cdist*(-icdist2)*icdist2*r6);
+                            dpdk_p[7] = fx*dXdYd(0);
+                            dpdk_p[dpdk_step+7] = fy*dXdYd(1);
+                            if( _dpdk->cols > 8 )
+                            {
+                                dXdYd = dMatTilt*Vec2d(r2, 0);
+                                dpdk_p[8] = fx*dXdYd(0); //s1
+                                dpdk_p[dpdk_step+8] = fy*dXdYd(1); //s1
+                                dXdYd = dMatTilt*Vec2d(r4, 0);
+                                dpdk_p[9] = fx*dXdYd(0); //s2
+                                dpdk_p[dpdk_step+9] = fy*dXdYd(1); //s2
+                                dXdYd = dMatTilt*Vec2d(0, r2);
+                                dpdk_p[10] = fx*dXdYd(0);//s3
+                                dpdk_p[dpdk_step+10] = fy*dXdYd(1); //s3
+                                dXdYd = dMatTilt*Vec2d(0, r4);
+                                dpdk_p[11] = fx*dXdYd(0);//s4
+                                dpdk_p[dpdk_step+11] = fy*dXdYd(1); //s4
+                                if( _dpdk->cols > 12 )
+                                {
+                                    dVecTilt = dMatTiltdTauX * Vec3d(xd0, yd0, 1);
+                                    dpdk_p[12] = fx * invProjSquare * (
+                                            dVecTilt(0) * vecTilt(2) - dVecTilt(2) * vecTilt(0));
+                                    dpdk_p[dpdk_step+12] = fy*invProjSquare * (
+                                            dVecTilt(1) * vecTilt(2) - dVecTilt(2) * vecTilt(1));
+                                    dVecTilt = dMatTiltdTauY * Vec3d(xd0, yd0, 1);
+                                    dpdk_p[13] = fx * invProjSquare * (
+                                            dVecTilt(0) * vecTilt(2) - dVecTilt(2) * vecTilt(0));
+                                    dpdk_p[dpdk_step+13] = fy * invProjSquare * (
+                                            dVecTilt(1) * vecTilt(2) - dVecTilt(2) * vecTilt(1));
+                                }
+                            }
+                        }
+                    }
+                }
+                dpdk_p += dpdk_step*2;
+            }
+
+            if( dpdt_p )
+            {
+                double dxdt[] = { z, 0, -x*z }, dydt[] = { 0, z, -y*z };
+                for( j = 0; j < 3; j++ )
+                {
+                    double dr2dt = 2*x*dxdt[j] + 2*y*dydt[j];
+                    double dcdist_dt = k[0]*dr2dt + 2*k[1]*r2*dr2dt + 3*k[4]*r4*dr2dt;
+                    double dicdist2_dt = -icdist2*icdist2*(k[5]*dr2dt + 2*k[6]*r2*dr2dt + 3*k[7]*r4*dr2dt);
+                    double da1dt = 2*(x*dydt[j] + y*dxdt[j]);
+                    double dmxdt = (dxdt[j]*cdist*icdist2 + x*dcdist_dt*icdist2 + x*cdist*dicdist2_dt +
+                                    k[2]*da1dt + k[3]*(dr2dt + 4*x*dxdt[j]) + k[8]*dr2dt + 2*r2*k[9]*dr2dt);
+                    double dmydt = (dydt[j]*cdist*icdist2 + y*dcdist_dt*icdist2 + y*cdist*dicdist2_dt +
+                                    k[2]*(dr2dt + 4*y*dydt[j]) + k[3]*da1dt + k[10]*dr2dt + 2*r2*k[11]*dr2dt);
+                    dXdYd = dMatTilt*Vec2d(dmxdt, dmydt);
+                    dpdt_p[j] = fx*dXdYd(0);
+                    dpdt_p[dpdt_step+j] = fy*dXdYd(1);
+                }
+                dpdt_p += dpdt_step*2;
+            }
+
+            if( dpdr_p )
+            {
+                double dx0dr[] =
+                        {
+                                X*dRdr[0] + Y*dRdr[1] + Z*dRdr[2],
+                                X*dRdr[9] + Y*dRdr[10] + Z*dRdr[11],
+                                X*dRdr[18] + Y*dRdr[19] + Z*dRdr[20]
+                        };
+                double dy0dr[] =
+                        {
+                                X*dRdr[3] + Y*dRdr[4] + Z*dRdr[5],
+                                X*dRdr[12] + Y*dRdr[13] + Z*dRdr[14],
+                                X*dRdr[21] + Y*dRdr[22] + Z*dRdr[23]
+                        };
+                double dz0dr[] =
+                        {
+                                X*dRdr[6] + Y*dRdr[7] + Z*dRdr[8],
+                                X*dRdr[15] + Y*dRdr[16] + Z*dRdr[17],
+                                X*dRdr[24] + Y*dRdr[25] + Z*dRdr[26]
+                        };
+                for( j = 0; j < 3; j++ )
+                {
+                    double dxdr = z*(dx0dr[j] - x*dz0dr[j]);
+                    double dydr = z*(dy0dr[j] - y*dz0dr[j]);
+                    double dr2dr = 2*x*dxdr + 2*y*dydr;
+                    double dcdist_dr = (k[0] + 2*k[1]*r2 + 3*k[4]*r4)*dr2dr;
+                    double dicdist2_dr = -icdist2*icdist2*(k[5] + 2*k[6]*r2 + 3*k[7]*r4)*dr2dr;
+                    double da1dr = 2*(x*dydr + y*dxdr);
+                    double dmxdr = (dxdr*cdist*icdist2 + x*dcdist_dr*icdist2 + x*cdist*dicdist2_dr +
+                                    k[2]*da1dr + k[3]*(dr2dr + 4*x*dxdr) + (k[8] + 2*r2*k[9])*dr2dr);
+                    double dmydr = (dydr*cdist*icdist2 + y*dcdist_dr*icdist2 + y*cdist*dicdist2_dr +
+                                    k[2]*(dr2dr + 4*y*dydr) + k[3]*da1dr + (k[10] + 2*r2*k[11])*dr2dr);
+                    dXdYd = dMatTilt*Vec2d(dmxdr, dmydr);
+                    dpdr_p[j] = fx*dXdYd(0);
+                    dpdr_p[dpdr_step+j] = fy*dXdYd(1);
+                }
+                dpdr_p += dpdr_step*2;
+            }
+
+            if( dpdo_p )
+            {
+                double dxdo[] = { z * ( R[0] - x * z * z0 * R[6] ),
+                                  z * ( R[1] - x * z * z0 * R[7] ),
+                                  z * ( R[2] - x * z * z0 * R[8] ) };
+                double dydo[] = { z * ( R[3] - y * z * z0 * R[6] ),
+                                  z * ( R[4] - y * z * z0 * R[7] ),
+                                  z * ( R[5] - y * z * z0 * R[8] ) };
+                for( j = 0; j < 3; j++ )
+                {
+                    double dr2do = 2 * x * dxdo[j] + 2 * y * dydo[j];
+                    double dr4do = 2 * r2 * dr2do;
+                    double dr6do = 3 * r4 * dr2do;
+                    double da1do = 2 * y * dxdo[j] + 2 * x * dydo[j];
+                    double da2do = dr2do + 4 * x * dxdo[j];
+                    double da3do = dr2do + 4 * y * dydo[j];
+                    double dcdist_do
+                            = k[0] * dr2do + k[1] * dr4do + k[4] * dr6do;
+                    double dicdist2_do = -icdist2 * icdist2
+                                         * ( k[5] * dr2do + k[6] * dr4do + k[7] * dr6do );
+                    double dxd0_do = cdist * icdist2 * dxdo[j]
+                                     + x * icdist2 * dcdist_do + x * cdist * dicdist2_do
+                                     + k[2] * da1do + k[3] * da2do + k[8] * dr2do
+                                     + k[9] * dr4do;
+                    double dyd0_do = cdist * icdist2 * dydo[j]
+                                     + y * icdist2 * dcdist_do + y * cdist * dicdist2_do
+                                     + k[2] * da3do + k[3] * da1do + k[10] * dr2do
+                                     + k[11] * dr4do;
+                    dXdYd = dMatTilt * Vec2d( dxd0_do, dyd0_do );
+                    dpdo_p[i * 3 + j] = fx * dXdYd( 0 );
+                    dpdo_p[dpdo_step + i * 3 + j] = fy * dXdYd( 1 );
+                }
+                dpdo_p += dpdo_step * 2;
+            }
+        }
+    }
+
+    if( _m != imagePoints )
+        cvConvert( _m, imagePoints );
+
+    if( _dpdr != dpdr )
+        cvConvert( _dpdr, dpdr );
+
+    if( _dpdt != dpdt )
+        cvConvert( _dpdt, dpdt );
+
+    if( _dpdf != dpdf )
+        cvConvert( _dpdf, dpdf );
+
+    if( _dpdc != dpdc )
+        cvConvert( _dpdc, dpdc );
+
+    if( _dpdk != dpdk )
+        cvConvert( _dpdk, dpdk );
+
+    if( _dpdo != dpdo )
+        cvConvert( _dpdo, dpdo );
+}
+
+static void _cvProjectPoints2( const CvMat* objectPoints,
+                        const CvMat* r_vec,
+                        const CvMat* t_vec,
+                        const CvMat* A,
+                        const CvMat* distCoeffs,
+                        CvMat* imagePoints, CvMat* dpdr,
+                        CvMat* dpdt, CvMat* dpdf,
+                        CvMat* dpdc, CvMat* dpdk,
+                        double aspectRatio )
+{
+    _cvProjectPoints2Internal( objectPoints, r_vec, t_vec, A, distCoeffs, imagePoints, dpdr, dpdt,
+                               dpdf, dpdc, dpdk, NULL, aspectRatio );
+}

aruco_pose/src/draw.h

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

aruco_pose/src/fix.cpp

@@ -1,145 +0,0 @@
-using namespace cv;
-using namespace cv::aruco;
-
-// Temporal fix!
-// TODO: remove
-// fix strange bug in our OpenCV version
-
-void _getBoardObjectAndImagePoints(const Ptr<aruco::Board> &board, InputArrayOfArrays detectedCorners,
-    InputArray detectedIds, OutputArray objPoints, OutputArray imgPoints) {
-
-    CV_Assert(board->ids.size() == board->objPoints.size());
-    CV_Assert(detectedIds.total() == detectedCorners.total());
-
-    size_t nDetectedMarkers = detectedIds.total();
-
-    std::vector< Point3f > objPnts;
-    objPnts.reserve(nDetectedMarkers);
-
-    std::vector< Point2f > imgPnts;
-    imgPnts.reserve(nDetectedMarkers);
-
-    // look for detected markers that belong to the board and get their information
-    for(unsigned int i = 0; i < nDetectedMarkers; i++) {
-        int currentId = detectedIds.getMat().ptr< int >(0)[i];
-        for(unsigned int j = 0; j < board->ids.size(); j++) {
-            if(currentId == board->ids[j]) {
-                for(int p = 0; p < 4; p++) {
-                    objPnts.push_back(board->objPoints[j][p]);
-                    imgPnts.push_back(detectedCorners.getMat(i).ptr< Point2f >(0)[p]);
-                }
-            }
-        }
-    }
-
-    // create output
-    Mat(objPnts).copyTo(objPoints);
-    Mat(imgPnts).copyTo(imgPoints);
-}
-
-int _estimatePoseBoard(InputArrayOfArrays _corners, InputArray _ids, const Ptr<aruco::Board> &board,
-                      InputArray _cameraMatrix, InputArray _distCoeffs, OutputArray _rvec,
-                      OutputArray _tvec, bool useExtrinsicGuess, Mat &objPoints) {
-
-    CV_Assert(_corners.total() == _ids.total());
-
-    // get object and image points for the solvePnP function
-    Mat /*objPoints, */imgPoints;
-    _getBoardObjectAndImagePoints(board, _corners, _ids, objPoints, imgPoints);
-
-    CV_Assert(imgPoints.total() == objPoints.total());
-
-    if(objPoints.total() == 0) // 0 of the detected markers in board
-        return 0;
-
-//    std::cout << "objPoints: " << objPoints << std::endl;
-//    std::cout << "imgPoints: " << imgPoints << std::endl;
-
-    solvePnP(objPoints, imgPoints, _cameraMatrix, _distCoeffs, _rvec, _tvec, useExtrinsicGuess);
-
-    // divide by four since all the four corners are concatenated in the array for each marker
-    return (int)objPoints.total() / 4;
-}
-
-void _drawPlanarBoard(Board *_board, Size outSize, OutputArray _img, int marginSize,
-                     int borderBits) {
-
-    CV_Assert(outSize.area() > 0);
-    CV_Assert(marginSize >= 0);
-
-    _img.create(outSize, CV_8UC1);
-    Mat out = _img.getMat();
-    out.setTo(Scalar::all(255));
-    out.adjustROI(-marginSize, -marginSize, -marginSize, -marginSize);
-
-    // calculate max and min values in XY plane
-    CV_Assert(_board->objPoints.size() > 0);
-    float minX, maxX, minY, maxY;
-    minX = maxX = _board->objPoints[0][0].x;
-    minY = maxY = _board->objPoints[0][0].y;
-
-    for(unsigned int i = 0; i < _board->objPoints.size(); i++) {
-        for(int j = 0; j < 4; j++) {
-            minX = min(minX, _board->objPoints[i][j].x);
-            maxX = max(maxX, _board->objPoints[i][j].x);
-            minY = min(minY, _board->objPoints[i][j].y);
-            maxY = max(maxY, _board->objPoints[i][j].y);
-        }
-    }
-
-    float sizeX = maxX - minX;
-    float sizeY = maxY - minY;
-
-    // proportion transformations
-    float xReduction = sizeX / float(out.cols);
-    float yReduction = sizeY / float(out.rows);
-
-    // determine the zone where the markers are placed
-    if(xReduction > yReduction) {
-        int nRows = int(sizeY / xReduction);
-        int rowsMargins = (out.rows - nRows) / 2;
-        out.adjustROI(-rowsMargins, -rowsMargins, 0, 0);
-    } else {
-        int nCols = int(sizeX / yReduction);
-        int colsMargins = (out.cols - nCols) / 2;
-        out.adjustROI(0, 0, -colsMargins, -colsMargins);
-    }
-
-    // now paint each marker
-    Dictionary &dictionary = *(_board->dictionary);
-    Mat marker;
-    Point2f outCorners[3];
-    Point2f inCorners[3];
-    for(unsigned int m = 0; m < _board->objPoints.size(); m++) {
-        // transform corners to markerZone coordinates
-        for(int j = 0; j < 3; j++) {
-            Point2f pf = Point2f(_board->objPoints[m][j].x, _board->objPoints[m][j].y);
-            // move top left to 0, 0
-            pf -= Point2f(minX, minY);
-            pf.x = pf.x / sizeX * float(out.cols);
-            pf.y = (1.0f - pf.y / sizeY) * float(out.rows);
-            outCorners[j] = pf;
-        }
-
-        // get marker
-        Size dst_sz(outCorners[2] - outCorners[0]); // assuming CCW order
-        dst_sz.width = dst_sz.height = std::min(dst_sz.width, dst_sz.height); //marker should be square
-        dictionary.drawMarker(_board->ids[m], dst_sz.width, marker, borderBits);
-
-        if((outCorners[0].y == outCorners[1].y) && (outCorners[1].x == outCorners[2].x)) {
-            // marker is aligned to image axes
-            marker.copyTo(out(Rect(outCorners[0], dst_sz)));
-            continue;
-        }
-
-        // interpolate tiny marker to marker position in markerZone
-        inCorners[0] = Point2f(-0.5f, -0.5f);
-        inCorners[1] = Point2f(marker.cols - 0.5f, -0.5f);
-        inCorners[2] = Point2f(marker.cols - 0.5f, marker.rows - 0.5f);
-
-        // remove perspective
-        Mat transformation = getAffineTransform(inCorners, outCorners);
-        warpAffine(marker, out, transformation, out.size(), INTER_LINEAR,
-                        BORDER_TRANSPARENT);
-    }
-}

aruco_pose/src/genmap.py

@@ -0,0 +1,53 @@
+#!/usr/bin/env python
+
+# Copyright (C) 2018 Copter Express Technologies
+#
+# Author: Oleg Kalachev <okalachev@gmail.com>
+#
+# Distributed under MIT License (available at https://opensource.org/licenses/MIT).
+# The above copyright notice and this permission notice shall be included in all
+# copies or substantial portions of the Software.
+
+"""Markers map generator
+
+Generate map file for aruco_map nodelet.
+
+Usage:
+  genmap.py <length> <x> <y> <dist_x> <dist_y> <first> [--top-left]
+  genmap.py (-h | --help)
+
+Options:
+  <length>    Marker side length
+  <x>         Marker count along X axis
+  <y>         Marker count along Y axis
+  <dist_x>    Distance between markers along X axis
+  <dist_y>    Distance between markers along Y axis
+  <first>     First marker ID
+  --top-left  First marker is on top-left (not bottom-left)
+"""
+
+from __future__ import print_function
+
+from docopt import docopt
+
+
+arguments = docopt(__doc__)
+
+length = float(arguments['<length>'])
+first = int(arguments['<first>'])
+markers_x = int(arguments['<x>'])
+markers_y = int(arguments['<y>'])
+dist_x = float(arguments['<dist_x>'])
+dist_y = float(arguments['<dist_y>'])
+top_left = arguments['--top-left']
+
+max_y = (markers_y - 1) * dist_y
+
+for y in range(markers_y):
+    for x in range(markers_x):
+        pos_x = x * dist_x
+        pos_y = y * dist_y
+        if top_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))
+        first += 1

aruco_pose/src/util.h

@@ -1,20 +0,0 @@
-#pragma once
-
-#include <vector>
-#include <string>
-
-std::vector<std::string> strSplit(const std::string& str, const std::string& delim)
-{
-    std::vector<std::string> tokens;
-    size_t prev = 0, pos = 0;
-    do
-    {
-        pos = str.find(delim, prev);
-        if (pos == std::string::npos) pos = str.length();
-        std::string token = str.substr(prev, pos-prev);
-        if (!token.empty()) tokens.push_back(token);
-        prev = pos + delim.length();
-    }
-    while (pos < str.length() && prev < str.length());
-    return tokens;
-}

aruco_pose/src/utils.h

@@ -0,0 +1,139 @@
+/*
+ * Utility functions
+ * Copyright (C) 2018 Copter Express Technologies
+ *
+ * Author: Oleg Kalachev <okalachev@gmail.com>
+ *
+ * Distributed under MIT License (available at https://opensource.org/licenses/MIT).
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ */
+
+#pragma once
+
+#include <cmath>
+#include <ros/ros.h>
+#include <tf/transform_datatypes.h>
+#include <geometry_msgs/Quaternion.h>
+#include <geometry_msgs/Pose.h>
+#include <geometry_msgs/Vector3.h>
+#include <sensor_msgs/CameraInfo.h>
+#include <opencv2/opencv.hpp>
+
+// Read required param or shutdown the node
+template<typename T>
+static void param(ros::NodeHandle nh, const std::string& param_name, T& param_val)
+{
+	if (!nh.getParam(param_name, param_val)) {
+		ROS_FATAL("Required param %s is not defined", param_name.c_str());
+		ros::shutdown();
+	}
+}
+
+static void parseCameraInfo(const sensor_msgs::CameraInfoConstPtr& cinfo, cv::Mat& matrix, cv::Mat& dist)
+{
+	for (unsigned int i = 0; i < 3; ++i)
+		for (unsigned int j = 0; j < 3; ++j)
+			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];
+}
+
+inline void rotatePoint(cv::Point3f& p, cv::Point3f origin, float angle)
+{
+	float s = sin(angle);
+	float c = cos(angle);
+
+	// translate point back to origin:
+	p.x -= origin.x;
+	p.y -= origin.y;
+
+	// rotate point
+	float xnew = p.x * c - p.y * s;
+	float ynew = p.x * s + p.y * c;
+
+	// translate point back:
+	p.x = xnew + origin.x;
+	p.y = ynew + origin.y;
+}
+
+inline void fillPose(geometry_msgs::Pose& pose, const cv::Vec3d& rvec, const cv::Vec3d& tvec)
+{
+	pose.position.x = tvec[0];
+	pose.position.y = tvec[1];
+	pose.position.z = tvec[2];
+
+	double angle = norm(rvec);
+	cv::Vec3d axis = rvec / angle;
+
+	tf2::Quaternion q;
+	q.setRotation(tf2::Vector3(axis[0], axis[1], axis[2]), angle);
+
+	pose.orientation.w = q.w();
+	pose.orientation.x = q.x();
+	pose.orientation.y = q.y();
+	pose.orientation.z = q.z();
+}
+
+inline void fillTransform(geometry_msgs::Transform& transform, const cv::Vec3d& rvec, const cv::Vec3d& tvec)
+{
+	transform.translation.x = tvec[0];
+	transform.translation.y = tvec[1];
+	transform.translation.z = tvec[2];
+
+	double angle = norm(rvec);
+	cv::Vec3d axis = rvec / angle;
+
+	tf2::Quaternion q;
+	q.setRotation(tf2::Vector3(axis[0], axis[1], axis[2]), angle);
+
+	transform.rotation.w = q.w();
+	transform.rotation.x = q.x();
+	transform.rotation.y = q.y();
+	transform.rotation.z = q.z();
+}
+
+inline void fillTranslation(geometry_msgs::Vector3& translation, const cv::Vec3d& tvec)
+{
+	translation.x = tvec[0];
+	translation.y = tvec[1];
+	translation.z = tvec[2];
+}
+
+inline bool isFlipped(tf::Quaternion& q)
+{
+	double yaw, pitch, roll;
+	tf::Matrix3x3(q).getEulerYPR(yaw, pitch, roll);
+	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)
+{
+	tf::Quaternion _from, _to;
+	tf::quaternionMsgToTF(from, _from);
+	tf::quaternionMsgToTF(to, _to);
+
+	if (auto_flip) {
+		if (!isFlipped(_from)) {
+			static const tf::Quaternion flip = tf::createQuaternionFromRPY(M_PI, 0, 0);
+			_from *= flip; // flip "from"
+		}
+	}
+
+	auto diff = tf::Matrix3x3(_to).transposeTimes(tf::Matrix3x3(_from));
+	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"
+}
+
+inline void transformToPose(const geometry_msgs::Transform& transform, geometry_msgs::Pose& pose)
+{
+	pose.position.x = transform.translation.x;
+	pose.position.y = transform.translation.y;
+	pose.position.z = transform.translation.z;
+	pose.orientation = transform.rotation;
+}

aruco_pose/test/basic.py

@@ -0,0 +1,204 @@
+import rospy
+import pytest
+
+import tf2_ros
+import tf2_geometry_msgs
+from geometry_msgs.msg import PoseWithCovarianceStamped
+from sensor_msgs.msg import Image
+from aruco_pose.msg import MarkerArray
+from visualization_msgs.msg import MarkerArray as VisMarkerArray
+
+
+@pytest.fixture
+def node():
+    return rospy.init_node('aruco_pose_test', anonymous=True)
+
+@pytest.fixture
+def tf_buffer():
+    buf = tf2_ros.Buffer()
+    tf2_ros.TransformListener(buf)
+    return buf
+
+def approx(expected):
+    return pytest.approx(expected, abs=1e-4) # compare floats more roughly
+
+def test_markers(node):
+    markers = rospy.wait_for_message('aruco_detect/markers', MarkerArray, timeout=5)
+    assert len(markers.markers) == 5
+    assert markers.header.frame_id == 'main_camera_optical'
+
+    assert markers.markers[0].id == 2
+    assert markers.markers[0].length == approx(0.33)
+    assert markers.markers[0].pose.position.x == approx(0.36706567854)
+    assert markers.markers[0].pose.position.y == approx(0.290484516644)
+    assert markers.markers[0].pose.position.z == approx(2.18787602301)
+    assert markers.markers[0].pose.orientation.x == approx(0.993997406299)
+    assert markers.markers[0].pose.orientation.y == approx(-0.00532003481626)
+    assert markers.markers[0].pose.orientation.z == approx(-0.107390951553)
+    assert markers.markers[0].pose.orientation.w == approx(0.0201999263402)
+    assert markers.markers[0].c1.x == approx(415.557739258)
+    assert markers.markers[0].c1.y == approx(335.557739258)
+    assert markers.markers[0].c2.x == approx(509.442260742)
+    assert markers.markers[0].c2.y == approx(335.557739258)
+    assert markers.markers[0].c3.x == approx(509.442260742)
+    assert markers.markers[0].c3.y == approx(429.442260742)
+    assert markers.markers[0].c4.x == approx(415.557739258)
+    assert markers.markers[0].c4.y == approx(429.442260742)
+
+    assert markers.markers[4].id == 3
+    assert markers.markers[4].length == approx(0.1)
+    assert markers.markers[4].pose.position.x == approx(-0.1805169666)
+    assert markers.markers[4].pose.position.y == approx(-0.200697302327)
+    assert markers.markers[4].pose.position.z == approx(0.585767514823)
+    assert markers.markers[4].pose.orientation.x == approx(-0.961738074009)
+    assert markers.markers[4].pose.orientation.y == approx(-0.0375180244707)
+    assert markers.markers[4].pose.orientation.z == approx(-0.0115387773672)
+    assert markers.markers[4].pose.orientation.w == approx(0.271144115664)
+    assert markers.markers[4].c1.x == approx(129.557723999)
+    assert markers.markers[4].c1.y == approx(49.557723999)
+    assert markers.markers[4].c2.x == approx(223.442276001)
+    assert markers.markers[4].c2.y == approx(49.557723999)
+    assert markers.markers[4].c3.x == approx(223.442276001)
+    assert markers.markers[4].c3.y == approx(143.442276001)
+    assert markers.markers[4].c4.x == approx(129.557723999)
+    assert markers.markers[4].c4.y == approx(143.442276001)
+
+    assert markers.markers[1].id == 1
+    assert markers.markers[1].length == approx(0.33)
+    assert markers.markers[3].id == 4
+    assert markers.markers[3].length == approx(0.33)
+
+    assert markers.markers[2].id == 100
+    assert markers.markers[2].length == approx(0.33)
+    assert markers.markers[2].pose.position.x == approx(-1.37600105389)
+    assert markers.markers[2].pose.position.y == approx(-0.323028530991)
+    assert markers.markers[2].pose.position.z == approx(2.94611272668)
+    assert markers.markers[2].pose.orientation.x == approx(-0.955543925678)
+    assert markers.markers[2].pose.orientation.y == approx(0.0458801909197)
+    assert markers.markers[2].pose.orientation.z == approx(-0.249604946264)
+    assert markers.markers[2].pose.orientation.w == approx(-0.150093920537)
+    assert markers.markers[2].c1.x == approx(52.557723999)
+    assert markers.markers[2].c1.y == approx(205.557723999)
+    assert markers.markers[2].c2.x == approx(113.442276001)
+    assert markers.markers[2].c2.y == approx(205.557723999)
+    assert markers.markers[2].c3.x == approx(113.442276001)
+    assert markers.markers[2].c3.y == approx(265.442260742)
+    assert markers.markers[2].c4.x == approx(52.557723999)
+    assert markers.markers[2].c4.y == approx(265.442260742)
+
+def test_markers_frames(node, tf_buffer):
+    marker_2 = tf_buffer.lookup_transform('main_camera_optical', 'aruco_2', rospy.Time(), rospy.Duration(5))
+    assert marker_2.transform.translation.x == approx(0.36706567854)
+    assert marker_2.transform.translation.y == approx(0.290484516644)
+    assert marker_2.transform.translation.z == approx(2.18787602301)
+    assert marker_2.transform.rotation.x == approx(0.993997406299)
+    assert marker_2.transform.rotation.y == approx(-0.00532003481626)
+    assert marker_2.transform.rotation.z == approx(-0.107390951553)
+    assert marker_2.transform.rotation.w == approx(0.0201999263402)
+
+def test_map_markers_frames(node, tf_buffer):
+    stamp = rospy.get_rostime()
+    timeout = rospy.Duration(5)
+
+    marker_1 = tf_buffer.lookup_transform('aruco_map', 'aruco_in_map_1', stamp, timeout)
+    assert marker_1.transform.translation.x == approx(0)
+    assert marker_1.transform.translation.y == approx(0)
+    assert marker_1.transform.translation.z == approx(0)
+
+    marker_4 = tf_buffer.lookup_transform('aruco_map', 'aruco_in_map_4', stamp, timeout)
+    assert marker_4.transform.translation.x == approx(1)
+    assert marker_4.transform.translation.y == approx(1)
+    assert marker_4.transform.translation.z == approx(0)
+
+    marker_12 = tf_buffer.lookup_transform('aruco_map', 'aruco_in_map_12', stamp, timeout)
+    assert marker_12.transform.translation.x == approx(0.2)
+    assert marker_12.transform.translation.y == approx(0.5)
+    assert marker_12.transform.translation.z == approx(0)
+
+def test_visualization(node):
+    vis = rospy.wait_for_message('aruco_detect/visualization', VisMarkerArray, timeout=5)
+    assert len(vis.markers) == 11
+
+def test_debug(node):
+    img = rospy.wait_for_message('aruco_detect/debug', Image, timeout=5)
+    assert img.width == 640
+    assert img.height == 480
+    assert img.header.frame_id == 'main_camera_optical'
+
+def test_map(node):
+    pose = rospy.wait_for_message('aruco_map/pose', PoseWithCovarianceStamped, timeout=5)
+    assert pose.header.frame_id == 'main_camera_optical'
+    assert pose.pose.pose.position.x == approx(-0.629167753342)
+    assert pose.pose.pose.position.y == approx(0.293822650809)
+    assert pose.pose.pose.position.z == approx(2.12641343155)
+    assert pose.pose.pose.orientation.x == approx(-0.998383794799)
+    assert pose.pose.pose.orientation.y == approx(-5.20919098575e-06)
+    assert pose.pose.pose.orientation.z == approx(-0.0300861070302)
+    assert pose.pose.pose.orientation.w == approx(0.0482143590507)
+
+def test_map_image(node):
+    img = rospy.wait_for_message('aruco_map/image', Image, timeout=5)
+    assert img.width == 2000
+    assert img.height == 2000
+    assert img.encoding in ('mono8', 'rgb8')
+
+def test_map_markers(node):
+    markers = rospy.wait_for_message('aruco_map/markers', MarkerArray, timeout=5)
+    assert markers.markers[0].id == 1
+    assert markers.markers[1].id == 2
+    assert markers.markers[2].id == 3
+    assert markers.markers[3].id == 4
+    assert markers.markers[4].id == 10
+    assert markers.markers[5].id == 11
+    assert markers.markers[6].id == 12
+
+    assert markers.markers[0].pose.position.x == 0
+    assert markers.markers[0].pose.position.y == 0
+    assert markers.markers[0].pose.position.z == 0
+    assert markers.markers[0].pose.orientation.x == 0
+    assert markers.markers[0].pose.orientation.y == 0
+    assert markers.markers[0].pose.orientation.z == 0
+    assert markers.markers[0].pose.orientation.w == 1
+    assert markers.markers[0].length == approx(0.33)
+
+    assert markers.markers[1].pose.position.x == 1
+    assert markers.markers[1].pose.position.y == 0
+    assert markers.markers[1].pose.position.z == 0
+    assert markers.markers[1].pose.orientation.x == 0
+    assert markers.markers[1].pose.orientation.y == 0
+    assert markers.markers[1].pose.orientation.z == 0
+    assert markers.markers[1].pose.orientation.w == 1
+    assert markers.markers[1].length == approx(0.33)
+
+    assert markers.markers[2].pose.position.x == 0
+    assert markers.markers[2].pose.position.y == 1
+    assert markers.markers[2].pose.position.z == 0
+    assert markers.markers[2].pose.orientation.x == 0
+    assert markers.markers[2].pose.orientation.y == 0
+    assert markers.markers[2].pose.orientation.z == 0
+    assert markers.markers[2].pose.orientation.w == 1
+    assert markers.markers[2].length == approx(0.33)
+
+    assert markers.markers[3].pose.position.x == 1
+    assert markers.markers[3].pose.position.y == 1
+    assert markers.markers[3].pose.position.z == 0
+    assert markers.markers[3].pose.orientation.x == 0
+    assert markers.markers[3].pose.orientation.y == 0
+    assert markers.markers[3].pose.orientation.z == 0
+    assert markers.markers[3].pose.orientation.w == 1
+    assert markers.markers[3].length == approx(0.33)
+
+    assert markers.markers[4].pose.position.x == approx(0.5)
+    assert markers.markers[4].pose.position.y == 2
+    assert markers.markers[4].pose.position.z == 0
+    assert markers.markers[4].pose.orientation.x == 0
+    assert markers.markers[4].pose.orientation.y == 0
+    assert markers.markers[4].pose.orientation.z == approx(0.5646424733950354)
+    assert markers.markers[4].pose.orientation.w == approx(0.8253356149096783)
+    assert markers.markers[4].length == approx(0.5)
+
+def test_map_visualization(node):
+    vis = rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5)
+
+def test_map_debug(node):
+    img = rospy.wait_for_message('aruco_map/debug', Image, timeout=5)

aruco_pose/test/basic.test

@@ -0,0 +1,31 @@
+<launch>
+    <node pkg="image_publisher" type="image_publisher" name="main_camera" args="$(find aruco_pose)/test/map.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="length_override/3" value="0.1"/>
+        <param name="estimate_poses" value="true"/>
+        <param name="send_tf" value="true"/>
+    </node>
+
+    <node name="aruco_map" pkg="nodelet" type="nodelet" args="load aruco_pose/aruco_map nodelet_manager" clear_params="true" required="true">
+        <remap from="image_raw" to="main_camera/image_raw"/>
+        <remap from="camera_info" to="main_camera/camera_info"/>
+        <remap from="markers" to="aruco_detect/markers"/>
+        <param name="type" value="map"/>
+        <param name="map" value="$(find aruco_pose)/test/basic.txt"/>
+        <param name="markers/frame_id" value="aruco_map"/>
+        <param name="markers/child_frame_id_prefix" value="aruco_in_map_"/>
+    </node>
+
+    <param name="test_module" value="$(find aruco_pose)/test/basic.py"/>
+    <test test-name="aruco_pose_test" pkg="ros_pytest" type="ros_pytest_runner"/>
+</launch>

aruco_pose/test/basic.txt

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

aruco_pose/test/camera_info.yaml

@@ -0,0 +1,21 @@
+# some random camera calibration for testing
+image_width: 640
+image_height: 480
+camera_name: test_camera
+camera_matrix:
+  rows: 3
+  cols: 3
+  data: [643.229809, 0.000000, 356.811289, 0.000000, 644.318982, 299.150067, 0.000000, 0.000000, 1.000000]
+distortion_model: plumb_bob
+distortion_coefficients:
+  rows: 1
+  cols: 5
+  data: [-0.422907, 0.202567, 0.000781, 0.000447, 0.000000]
+rectification_matrix:
+  rows: 3
+  cols: 3
+  data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000]
+projection_matrix:
+  rows: 3
+  cols: 4
+  data: [567.010193, 0.000000, 366.677428, 0.000000, 0.000000, 594.591980, 307.043423, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000]

aruco_pose/test/largemap.py

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

aruco_pose/test/largemap.test

@@ -0,0 +1,13 @@
+<launch>
+    <node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager" required="true"/>
+
+    <node name="aruco_map" pkg="nodelet" type="nodelet" args="load aruco_pose/aruco_map nodelet_manager" clear_params="true" required="true">
+        <remap from="image_raw" to="main_camera/image_raw"/>
+        <remap from="camera_info" to="main_camera/camera_info"/>
+        <remap from="markers" to="aruco_detect/markers"/>
+        <param name="type" value="map"/>
+        <param name="map" value="$(find aruco_pose)/test/largemap.txt"/>
+    </node>
+
+    <test test-name="test_aruco_pose_largemap" pkg="aruco_pose" type="largemap.py"/>
+</launch>

aruco_pose/test/largemap.txt

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

aruco_pose/test/test_node_failure.py

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

aruco_pose/test/test_node_failure.test

@@ -0,0 +1,14 @@
+<launch>
+    <node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager"/>
+
+    <node name="aruco_map" pkg="nodelet" type="nodelet" args="load aruco_pose/aruco_map nodelet_manager" clear_params="true">
+        <remap from="image_raw" to="main_camera/image_raw"/>
+        <remap from="camera_info" to="main_camera/camera_info"/>
+        <remap from="markers" to="aruco_detect/markers"/>
+        <param name="type" value="map"/>
+        <param name="map" value="$(find aruco_pose)/test/test_node_failure.txt"/>
+    </node>
+
+    <param name="test_module" value="$(find aruco_pose)/test/test_node_failure.py"/>
+    <test test-name="test_node_failure" pkg="ros_pytest" type="ros_pytest_runner"/>
+</launch>

aruco_pose/test/test_node_failure.txt

@@ -0,0 +1,4 @@
+# Any garbage data (pretty much anything apart from a comment starting with a hash starting
+# with a hash sign or a number) will be interpreted as garbage data; the node should fail
+# after reading it.
+// Don't try to put your comments this way, it will kill your node!

aruco_pose/test/test_parser_empty_map.py

@@ -0,0 +1,13 @@
+import rospy
+import pytest
+
+from visualization_msgs.msg import MarkerArray as VisMarkerArray
+
+
+@pytest.fixture
+def node():
+    return rospy.init_node('aruco_pose_test_empty_map', anonymous=True)
+
+def test_empty_map(node):
+    markers = rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5)
+    assert len(markers.markers) == 0