docs: some info on optical flow troubleshooting

.editorconfig

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

.gitattributes

@@ -1,6 +1,2 @@
+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
-aruco_pose/vendor/* linguist-vendored

.gitignore

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

.markdownlint.json

@@ -1,114 +1,22 @@
 {
     "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",
-            "ROS Melodic",
-            "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",
-            "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",
-            "Nvidia"
+            "ArUco"
         ],
         "code_blocks": false
-    },
-    "MD045": false
+    }
 }

.travis.yml

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

README.md

@@ -1,40 +1,77 @@
-# COEX Clover Drone Kit
+# CLEVER
 
-<img src="docs/assets/clever4-front-white.png" align="right" width="400px" alt="Clover Drone">
+<img src="docs/assets/clever3.png" align="right" width="300px" alt="CLEVER drone">
 
-Clover is an educational programmable drone kit consisting of an unassembled quadcopter, open source software and documentation. The kit includes Pixracer-compatible autopilot running PX4 firmware, Raspberry Pi 4 as companion computer, a camera for computer vision navigation as well as additional sensors and peripheral devices.
+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.
 
-The main documentation is available [on Gitbook](https://clover.coex.tech/).
+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.
 
-Official website: <a href="https://coex.tech/clover">coex.tech/clover</a>.
+**The main documentation in Russian is available [on our Gitbook](https://clever.copterexpress.com/).**
 
-## Video compilation
+Use it to learn how to assemble, configure, pilot and program autonomous CLEVER drone.
 
-[![Clover Drone Kit autonomy compilation](http://img.youtube.com/vi/u3omgsYC4Fk/hqdefault.jpg)](https://youtu.be/u3omgsYC4Fk)
+## Preconfigured RPi 3 image
 
-Clover drone is used on a wide range of educational events, including [Copter Hack](https://www.youtube.com/watch?v=xgXheg3TTs4), WorldSkills Drone Operation competition, [Autonomous Vehicles Track of NTI Olympics 2016–2020](https://www.youtube.com/watch?v=E1_ehvJRKxg), Quadro Hack 2019 (National University of Science and Technology MISiS), Russian Robot Olympiad (autonomous flights), and others.
+**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).**
 
-## Raspberry Pi image
+[![Build Status](https://travis-ci.org/urpylka/clever.svg?branch=master)](https://travis-ci.org/urpylka/clever)
 
-Preconfigured image for Raspberry Pi with installed and configured software, ready to fly, is available [in the Releases section](https://github.com/CopterExpress/clover/releases).
+Image includes:
 
-[![Build Status](https://travis-ci.org/CopterExpress/clover.svg?branch=master)](https://travis-ci.org/CopterExpress/clover)
-
-Image features:
-
-* Raspbian Buster
-* [ROS Melodic](http://wiki.ros.org/melodic)
+* Raspbian Stretch
+* ROS Kinetic
 * Configured networking
 * OpenCV
-* [`mavros`](http://wiki.ros.org/mavros)
-* Periphery drivers for ROS ([GPIO](https://clover.coex.tech/en/gpio.html), [LED strip](https://clover.coex.tech/en/leds.html), etc)
-* `aruco_pose` package for marker-assisted navigation
-* `clover` package for autonomous drone control
+* mavros
+* CLEVER software bundle for autonomous drone control
 
-API description for autonomous flights is available [on GitBook](https://clover.coex.tech/en/simple_offboard.html).
+API description (in Russian) for autonomous flights is available [on GitBook](https://copterexpress.gitbooks.io/clever/simple_offboard.html).
 
-For manual package installation and running see [`clover` package documentation](clover/README.md).
+## Manual installation
 
-## License
+Install ROS Kinetic according to the [documentation](http://wiki.ros.org/kinetic/Installation).
 
-While the Clover platform source code is available under the MIT License, note, that the [documentation](docs/) is licensed under the Creative Commons Attribution-NonCommercial-ShareAlike 4.0 International License.
+Clone repo to directory `/home/pi/catkin_ws/src/clever`:
+
+```bash
+cd ~/catkin_ws/src
+git clone https://github.com/CopterExpress/clever.git clever
+```
+
+Build ROS packages:
+
+```bash
+cd ~/catkin_ws
+catkin_make -j1
+```
+
+Enable systemd service `roscore` (if not enabled):
+
+```bash
+sudo systemctl enable /home/pi/catkin_ws/src/clever/deploy/roscore.service
+sudo systemctl start roscore
+```
+
+Enable systemd service `clever`:
+
+```bash
+sudo systemctl enable /home/pi/catkin_ws/src/clever/deploy/clever.service
+sudo systemctl start clever
+```
+
+### Dependencies
+
+[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`

apps/android/.gitignore

@@ -1,11 +0,0 @@
-*.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

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

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

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

apps/android/.idea/gradle.xml

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

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

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

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

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

apps/android/app/build.gradle

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

apps/ios/README.md

@@ -1,4 +1,5 @@
-# iOS-приложение для управления Клевером
+iOS-приложение для управления Клевером
+--------------------------------------
 
 Для установки зависимостей необходим [CocoaPods](https://cocoapods.org):
 
@@ -7,11 +8,3 @@ 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,15 +145,8 @@
       "size" : "44x44",
       "idiom" : "watch",
       "scale" : "2x",
-      "role" : "appLauncher",
-      "subtype" : "40mm"
-    },
-    {
-      "size" : "50x50",
-      "idiom" : "watch",
-      "scale" : "2x",
-      "role" : "appLauncher",
-      "subtype" : "44mm"
+      "role" : "longLook",
+      "subtype" : "42mm"
     },
     {
       "size" : "86x86",
@@ -169,24 +162,10 @@
       "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.2</string>
+	<string>1.1</string>
 	<key>CFBundleVersion</key>
-	<string>7</string>
+	<string>6</string>
 	<key>LSRequiresIPhoneOS</key>
 	<true/>
 	<key>UILaunchStoryboardName</key>

apps/ios/cleverrc/telemetry.js

@@ -64,19 +64,9 @@ 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

@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.0)
+cmake_minimum_required(VERSION 2.8.3)
 project(aruco_pose)
 
 add_definitions(-std=c++11 -Wall -g)
@@ -21,19 +21,9 @@ find_package(catkin REQUIRED COMPONENTS
   tf2_geometry_msgs
   sensor_msgs
   message_generation
-  dynamic_reconfigure
 )
 
-find_package(OpenCV 3 REQUIRED COMPONENTS core imgproc calib3d)
-if ("${OpenCV_VERSION_MINOR}" LESS "3")
-  message(STATUS "OpenCV version too low, using vendored ArUco package")
-  include(vendor/VendorOpenCV.cmake)
-else()
-  message(STATUS "Using system OpenCV ArUco package")
-  find_package(OpenCV 3 REQUIRED COMPONENTS aruco)
-endif()
-message(STATUS "OpenCV include dirs: ${OpenCV_INCLUDE_DIRS}")
-message(STATUS "OpenCV libraries: ${OpenCV_LIBRARIES}")
+find_package(OpenCV REQUIRED)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
@@ -112,9 +102,10 @@ generate_messages(
 ##     and list every .cfg file to be processed
 
 ## Generate dynamic reconfigure parameters in the 'cfg' folder
-generate_dynamic_reconfigure_options(
-  cfg/DetectorParams.cfg
-)
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
 
 ###################################
 ## catkin specific configuration ##
@@ -148,10 +139,12 @@ include_directories(
 add_library(aruco_pose
   src/aruco_detect.cpp
   src/aruco_map.cpp
-  src/draw.cpp
 )
 
-add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_generate_messages_cpp ${PROJECT_NAME}_gencfg)
+## 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})
 
 ## Declare a C++ executable
 ## With catkin_make all packages are built within a single CMake context
@@ -171,7 +164,7 @@ add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_generate_messages_cpp ${PROJECT
 ## Specify libraries to link a library or executable target against
 target_link_libraries(aruco_pose
   ${catkin_LIBRARIES}
-  ${OpenCV_LIBRARIES}
+  ${OpenCV_LIBS}
 )
 
 #############
@@ -182,7 +175,7 @@ target_link_libraries(aruco_pose
 # 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}
@@ -221,12 +214,3 @@ target_link_libraries(aruco_pose
 
 ## 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

@@ -1,120 +0,0 @@
-# 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/cfg/DetectorParams.cfg

@@ -1,107 +0,0 @@
-#!/usr/bin/env python
-PACKAGE = "aruco_pose"
-
-from dynamic_reconfigure.parameter_generator_catkin import *
-import cv2.aruco
-
-p = cv2.aruco.DetectorParameters_create()
-
-gen = ParameterGenerator()
-
-gen.add("adaptiveThreshConstant",                 double_t, 0,
-        "Constant for adaptive thresholding before finding contours",
-         p.adaptiveThreshConstant, 0, 100)
-
-gen.add("adaptiveThreshWinSizeMin",               int_t,    0,
-        "Minimum window size for adaptive thresholding before finding contours",
-         p.adaptiveThreshWinSizeMin, 1, 100)
-
-gen.add("adaptiveThreshWinSizeMax",               int_t,    0,
-        "Maximum window size for adaptive thresholding before finding contours",
-        p.adaptiveThreshWinSizeMax, 1, 100)
-
-gen.add("adaptiveThreshWinSizeStep",              int_t,    0,
-        "Increments from adaptiveThreshWinSizeMin to adaptiveThreshWinSizeMax during the thresholding",
-        p.adaptiveThreshWinSizeStep, 1, 100)
-
-gen.add("cornerRefinementMaxIterations",          int_t,    0,
-        "Maximum number of iterations for stop criteria of the corner refinement process",
-        p.cornerRefinementMaxIterations, 1, 1000)
-
-corner_refine_enum = gen.enum([ gen.const("CORNER_REFINE_NONE",      int_t, 0, "No refinement"),
-                       gen.const("CORNER_REFINE_SUBPIX",     int_t, 1, "Do subpixel refinement"),
-                       gen.const("CORNER_REFINE_CONTOUR",      int_t, 2, "Use contour-Points"),
-                       gen.const("CORNER_REFINE_APRILTAG", int_t, 3, "Use the AprilTag2 approach")],
-                     "An enum to set corner refinement method")
-
-gen.add("cornerRefinementMethod", int_t, 0, "Corner refinement method", 0, 0, 3, edit_method=corner_refine_enum)
-
-gen.add("cornerRefinementMinAccuracy",            double_t, 0,
-        "Minimum error for the stop criteria of the corner refinement process",
-        p.cornerRefinementMinAccuracy, 0, 1)
-
-gen.add("cornerRefinementWinSize",                int_t,    0,
-        "Window size for the corner refinement process (in pixels)",
-         p.cornerRefinementWinSize, 1, 100)
-
-gen.add("detectInvertedMarker",                   bool_t,   0,
-        "check if there is a white marker. In order to generate a 'white' marker just invert a normal marker by using a tilde",
-        False)
-
-gen.add("errorCorrectionRate",                    double_t, 0,
-        "Error correction rate respect to the maximum error correction capability for each dictionary",
-        p.errorCorrectionRate, 0, 1)
-
-gen.add("minCornerDistanceRate",                  double_t, 0,
-        "Minimum distance between corners for detected markers relative to its perimeter",
-        p.minCornerDistanceRate, 0, 0.25)
-
-gen.add("markerBorderBits",                       int_t,    0,
-        "Number of bits of the marker border, i.e. marker border width",
-        p.markerBorderBits, 1, 10)
-
-gen.add("maxErroneousBitsInBorderRate",           double_t, 0,
-        "Maximum number of accepted erroneous bits in the border (i.e. number of allowed white bits in the border)",
-        p.maxErroneousBitsInBorderRate, 0, 1)
-
-gen.add("minDistanceToBorder",                    int_t,    0,
-        "Minimum distance of any corner to the image border for detected markers (in pixels)",
-        p.minDistanceToBorder, 0, 1000)
-
-gen.add("minMarkerDistanceRate",                  double_t, 0,
-        "minimum mean distance beetween two marker corners to be considered similar, so that the smaller one is removed. The rate is relative to the smaller perimeter of the two markers",
-        p.minMarkerDistanceRate, 0, 1)
-
-gen.add("minMarkerPerimeterRate",                 double_t, 0,
-        "Determine minimum perimeter for marker contour to be detected. This is defined as a rate respect to the maximum dimension of the input image",
-        p.minMarkerPerimeterRate, 0, 4)
-
-gen.add("maxMarkerPerimeterRate",                 double_t, 0,
-        "Determine maximum perimeter for marker contour to be detected. This is defined as a rate respect to the maximum dimension of the input image",
-        p.maxMarkerPerimeterRate, 0, 4)
-
-gen.add("minOtsuStdDev",                          double_t, 0,
-        "Minimun standard deviation in pixels values during the decodification step to apply Otsu thresholding (otherwise, all the bits are set to 0 or 1 depending on mean higher than 128 or not)",
-        p.minOtsuStdDev, 0, 100)
-
-gen.add("perspectiveRemoveIgnoredMarginPerCell",  double_t, 0,
-        "Width of the margin of pixels on each cell not considered for the determination of the cell bit. Represents the rate respect to the total size of the cell, i.e. perpectiveRemovePixelPerCell",
-        p.perspectiveRemoveIgnoredMarginPerCell, 0, 1)
-
-gen.add("perspectiveRemovePixelPerCell",          int_t,    0,
-        "Number of bits (per dimension) for each cell of the marker when removing the perspective",
-        p.perspectiveRemovePixelPerCell, 1, 100)
-
-gen.add("polygonalApproxAccuracyRate",            double_t, 0,
-        "Minimum accuracy during the polygonal approximation process to determine which contours are squares",
-        p.polygonalApproxAccuracyRate, 0, 1)
-
-gen.add("aprilTagQuadDecimate",            double_t, 0,
-        "Detection of quads can be done on a lower-resolution image, improving speed at a cost of pose accuracy and a slight decrease in detection rate. Decoding the binary payload is still done at full resolution",
-        0, 0, 1000)
-
-gen.add("aprilTagQuadSigma",            double_t, 0,
-        "What Gaussian blur should be applied to the segmented image (used for quad detection?) Parameter is the standard deviation in pixels. Very noisy images benefit from non-zero values",
-        0, 0, 1000)
-
-exit(gen.generate(PACKAGE, "aruco_pose", "Detector"))

aruco_pose/launch/sample.launch

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

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

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

aruco_pose/map/office.txt

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

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

@@ -1,6 +1,5 @@
 uint32 id
-float32 length
-geometry_msgs/Pose pose
+geometry_msgs/PoseWithCovariance pose
 Point2D c1
 Point2D c2
 Point2D c3

aruco_pose/package.xml

@@ -1,36 +1,40 @@
 <?xml version="1.0"?>
-<package format="2">
+<package>
   <name>aruco_pose</name>
-  <version>0.0.1</version>
-  <description>Positioning with ArUco markers</description>
+  <version>0.0.0</version>
+  <description>Positioning by ArUco markers</description>
 
   <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>
+  <author email="urpylka@gmail.com">Artem Smirnov</author>
 
   <buildtool_depend>catkin</buildtool_depend>
 
-  <depend>roscpp</depend>
-  <depend>nodelet</depend>
-  <depend>tf</depend>
-  <depend>tf2</depend>
-  <depend>tf2_ros</depend>
-  <depend>tf2_geometry_msgs</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>
-  <depend>dynamic_reconfigure</depend>
-
-  <test_depend>image_publisher</test_depend>
-  <test_depend>ros_pytest</test_depend>
+  <build_depend>tf</build_depend>
+  <build_depend>tf2</build_depend>
+  <build_depend>tf2_ros</build_depend>
+  <build_depend>tf2_geometry_msgs</build_depend>
+  <build_depend>cv_bridge</build_depend>
+  <build_depend>dynamic_reconfigure</build_depend>
+  <build_depend>image_transport</build_depend>
+  <build_depend>message_generation</build_depend>
+  <build_depend>message_runtime</build_depend>
+  <build_depend>nodelet</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <run_depend>nodelet</run_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>image_transport</run_depend>
+  <run_depend>cv_bridge</run_depend>
+  <run_depend>message_runtime</run_depend>
+  <run_depend>std_msgs</run_depend>
+  <run_depend>tf</run_depend>
+  <run_depend>tf2</run_depend>
+  <run_depend>tf2_ros</run_depend>
+  <run_depend>tf2_geometry_msgs</run_depend>
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>

aruco_pose/src/aruco_detect.cpp

@@ -1,5 +1,5 @@
 /*
- * Detecting and pose estimation of ArUco markers
+ * Detecting and positioning ArUco markers
  * Copyright (C) 2018 Copter Express Technologies
  *
  * Author: Oleg Kalachev <okalachev@gmail.com>
@@ -17,9 +17,6 @@
 #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>
@@ -30,7 +27,6 @@
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 #include <image_transport/image_transport.h>
 #include <cv_bridge/cv_bridge.h>
-#include <dynamic_reconfigure/server.h>
 #include <geometry_msgs/Vector3.h>
 #include <geometry_msgs/Pose.h>
 #include <geometry_msgs/PoseStamped.h>
@@ -47,11 +43,8 @@
 
 #include <aruco_pose/Marker.h>
 #include <aruco_pose/MarkerArray.h>
-#include <aruco_pose/DetectorConfig.h>
 
 #include "utils.h"
-#include <memory>
-#include <functional>
 
 using std::vector;
 using cv::Mat;
@@ -62,20 +55,16 @@ private:
 	tf2_ros::TransformBroadcaster br_;
 	tf2_ros::Buffer tf_buffer_;
 	tf2_ros::TransformListener tf_listener_{tf_buffer_};
-	std::shared_ptr<dynamic_reconfigure::Server<aruco_pose::DetectorConfig>> dyn_srv_;
 	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_;
+	bool estimate_poses_, send_tf_;
 	double length_;
-	std::unordered_map<int, double> length_override_;
-	std::string frame_id_prefix_, known_tilt_;
+	std::string snap_orientation_;
 	Mat camera_matrix_, dist_coeffs_;
 	aruco_pose::MarkerArray array_;
-	std::unordered_set<int> map_markers_ids_;
 	visualization_msgs::MarkerArray vis_array_;
 
 public:
@@ -89,15 +78,10 @@ public:
 		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_FATAL("aruco_detect: 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_");
+		nh_priv_.param<std::string>("snap_orientation", snap_orientation_, "");
 
 		camera_matrix_ = cv::Mat::zeros(3, 3, CV_64F);
 		dist_coeffs_ = cv::Mat::zeros(8, 1, CV_64F);
@@ -109,19 +93,12 @@ public:
 		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);
 
-		dyn_srv_ = std::make_shared<dynamic_reconfigure::Server<aruco_pose::DetectorConfig>>(nh_priv_);
-		dynamic_reconfigure::Server<aruco_pose::DetectorConfig>::CallbackType cb;
-
-		cb = std::bind(&ArucoDetect::paramCallback, this, std::placeholders::_1, std::placeholders::_2);
-		dyn_srv_->setCallback(cb);
-
-		NODELET_INFO("ready");
+		ROS_INFO("aruco_detect: ready");
 	}
 
 private:
@@ -150,30 +127,12 @@ private:
 				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()) {
+				if (!snap_orientation_.empty()) {
 					try {
-						snap_to = tf_buffer_.lookupTransform(msg->header.frame_id, known_tilt_,
+						snap_to = tf_buffer_.lookupTransform(msg->header.frame_id, snap_orientation_,
 						                                     msg->header.stamp, ros::Duration(0.02));
 					} catch (const tf2::TransformException& e) {
-						NODELET_WARN_THROTTLE(5, "can't snap: %s", e.what());
+						ROS_WARN_THROTTLE(5, "aruco_detect: can't snap: %s", e.what());
 					}
 				}
 			}
@@ -186,27 +145,22 @@ private:
 
 			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]);
+					fillPose(marker.pose.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_);
+					// snap orientation (if enabled and snap frame avaiable)
+					if (!snap_orientation_.empty() && !snap_to.header.frame_id.empty()) {
+						snapOrientation(marker.pose.pose, snap_to.transform.rotation);
 					}
 
 					// 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);
-						}
+						transform.transform.rotation = marker.pose.pose.orientation;
+						fillTranslation(transform.transform.translation, tvecs[i]);
+						br_.sendTransform(transform);
 					}
 				}
 				array_.markers.push_back(marker);
@@ -225,8 +179,8 @@ private:
 			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);
+				pushVisMarkers(msg->header.frame_id, msg->header.stamp, array_.markers[i].pose.pose,
+				               length_, ids[i], i);
 
 			vis_markers_pub_.publish(vis_array_);
 		}
@@ -237,8 +191,7 @@ private:
 			cv::aruco::drawDetectedMarkers(debug, corners, ids); // draw markers
 			if (estimate_poses_)
 				for (unsigned int i = 0; i < ids.size(); i++)
-					cv::aruco::drawAxis(debug, camera_matrix_, dist_coeffs_,
-					                    rvecs[i], tvecs[i], getMarkerLength(ids[i]));
+					cv::aruco::drawAxis(debug, camera_matrix_, dist_coeffs_, rvecs[i], tvecs[i], length_);
 
 			cv_bridge::CvImage out_msg;
 			out_msg.header.frame_id = msg->header.frame_id;
@@ -321,67 +274,19 @@ private:
 		vis_array_.markers.push_back(marker);
 	}
 
+	void snapOrientation(geometry_msgs::Pose& pose, const geometry_msgs::Quaternion orientation)
+	{
+		tf::Quaternion q;
+		q.setRPY(0, 0, -tf::getYaw(pose.orientation) + tf::getYaw(orientation) + M_PI / 2);
+		tf::Quaternion pq;
+		tf::quaternionMsgToTF(orientation, pq);
+		pq = pq * q;
+		tf::quaternionTFToMsg(pq, pose.orientation);
+	}
+
 	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);
-		}
-	}
-
-	void paramCallback(aruco_pose::DetectorConfig &config, uint32_t level)
-	{
-		parameters_->adaptiveThreshConstant = config.adaptiveThreshConstant;
-		parameters_->adaptiveThreshWinSizeMin = config.adaptiveThreshWinSizeMin;
-		parameters_->adaptiveThreshWinSizeMax = config.adaptiveThreshWinSizeMax;
-		parameters_->adaptiveThreshWinSizeStep = config.adaptiveThreshWinSizeStep;
-		parameters_->cornerRefinementMaxIterations = config.cornerRefinementMaxIterations;
-		parameters_->cornerRefinementMethod = config.cornerRefinementMethod;
-		parameters_->cornerRefinementMinAccuracy = config.cornerRefinementMinAccuracy;
-		parameters_->cornerRefinementWinSize = config.cornerRefinementWinSize;
-#if ((CV_VERSION_MAJOR == 3) && (CV_VERSION_MINOR >= 4) && (CV_VERSION_REVISION >= 7)) || (CV_VERSION_MAJOR > 3)
-		parameters_->detectInvertedMarker = config.detectInvertedMarker;
-#endif
-		parameters_->errorCorrectionRate = config.errorCorrectionRate;
-		parameters_->minCornerDistanceRate = config.minCornerDistanceRate;
-		parameters_->markerBorderBits = config.markerBorderBits;
-		parameters_->maxErroneousBitsInBorderRate = config.maxErroneousBitsInBorderRate;
-		parameters_->minDistanceToBorder = config.minDistanceToBorder;
-		parameters_->minMarkerDistanceRate = config.minMarkerDistanceRate;
-		parameters_->minMarkerPerimeterRate = config.minMarkerPerimeterRate;
-		parameters_->maxMarkerPerimeterRate = config.maxMarkerPerimeterRate;
-		parameters_->minOtsuStdDev = config.minOtsuStdDev;
-		parameters_->perspectiveRemoveIgnoredMarginPerCell = config.perspectiveRemoveIgnoredMarginPerCell;
-		parameters_->perspectiveRemovePixelPerCell = config.perspectiveRemovePixelPerCell;
-		parameters_->polygonalApproxAccuracyRate = config.polygonalApproxAccuracyRate;
-#if ((CV_VERSION_MAJOR == 3) && (CV_VERSION_MINOR >= 4) && (CV_VERSION_REVISION >= 2)) || (CV_VERSION_MAJOR > 3)
-		parameters_->aprilTagQuadDecimate = config.aprilTagQuadDecimate;
-		parameters_->aprilTagQuadSigma = config.aprilTagQuadSigma;
-#endif
+		return "aruco_" + std::to_string(id);
 	}
 };
 

aruco_pose/src/aruco_map.cpp

@@ -1,5 +1,5 @@
 /*
- * Detecting and pose estimation of ArUco markers maps
+ * Positioning ArUco markers maps
  * Copyright (C) 2018 Copter Express Technologies
  *
  * Author: Oleg Kalachev <okalachev@gmail.com>
@@ -18,21 +18,15 @@
 #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>
@@ -45,40 +39,27 @@
 #include <opencv2/opencv.hpp>
 #include <opencv2/aruco.hpp>
 
-#include "draw.h"
 #include "utils.h"
+#include "gridboard.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_;
+	ros::Publisher img_pub_, pose_pub_;
+	ros::Subscriber markers_sub_, cinfo_sub;
 	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_;
+	visualization_msgs::MarkerArray vis_markers;
+	std::string snap_orientation_;
+	bool has_camera_info_ = false;
 
 public:
 	virtual void onInit()
@@ -90,7 +71,6 @@ public:
 
 		// TODO: why image_transport doesn't work here?
 		img_pub_ = nh_priv_.advertise<sensor_msgs::Image>("image", 1, true);
-		markers_pub_ = nh_priv_.advertise<aruco_pose::MarkerArray>("markers", 1, true);
 
 		board_ = cv::makePtr<cv::aruco::Board>();
 		board_->dictionary = cv::aruco::getPredefinedDictionary(
@@ -98,19 +78,8 @@ public:
 		camera_matrix_ = cv::Mat::zeros(3, 3, CV_64F);
 		dist_coeffs_ = cv::Mat::zeros(8, 1, CV_64F);
 
-		std::string type, map;
+		std::string type, map, map_name;
 		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);
@@ -118,46 +87,36 @@ public:
 		} else if (type == "gridboard") {
 			createGridBoard();
 		} else {
-			NODELET_FATAL("unknown type: %s", type.c_str());
+			ROS_FATAL("aruco_map: unknown type: %s", type.c_str());
 			ros::shutdown();
 		}
 
+		nh_priv_.param<std::string>("name", map_name, "map");
+		nh_priv_.param<std::string>("frame_id", transform_.child_frame_id, "aruco_map");
+		nh_priv_.param<std::string>("snap_orientation", snap_orientation_, "");
+
 		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);
+		// TODO: use synchronised subscriber
+		markers_sub_ = nh_.subscribe("markers", 1, &ArucoMap::markersCallback, this);
+		cinfo_sub = nh_.subscribe("camera_info", 1, &ArucoMap::cinfoCallback, this);
 
-		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");
+		ROS_INFO("aruco_map: ready");
 	}
 
-	void callback(const sensor_msgs::ImageConstPtr& image,
-	              const sensor_msgs::CameraInfoConstPtr& cinfo,
-	              const aruco_pose::MarkerArrayConstPtr& markers)
+	void markersCallback(const aruco_pose::MarkerArray& markers)
 	{
-		int valid = 0;
-		int count = markers->markers.size();
+		if (!has_camera_info_) return;
+		if (markers.markers.empty()) return;
+
+		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) {
+		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),
@@ -168,100 +127,80 @@ public:
 			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;
+		Mat obj_points, img_points;
+		cv::Vec3d rvec, tvec;
 
-			transform_.header.stamp = markers->header.stamp;
-			transform_.header.frame_id = markers->header.frame_id;
+		if (snap_orientation_.empty()) {
+			// simple estimation
+			int valid = cv::aruco::estimatePoseBoard(corners, ids, board_, camera_matrix_, dist_coeffs_,
+		                                             rvec, tvec, false);
+			if (!valid) return;
+
+			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;
+			if (obj_points.empty()) return;
 
-			double center_x = 0, center_y = 0, center_z = 0;
-			alignObjPointsToCenter(obj_points, center_x, center_y, center_z);
+			double center_x = 0, center_y = 0;
+			alignObjPointsToCenter(obj_points, center_x, center_y);
 
-			valid = solvePnP(obj_points, img_points, camera_matrix_, dist_coeffs_, rvec, tvec, false);
-			if (!valid) goto publish_debug;
+			int res = solvePnP(obj_points, img_points, camera_matrix_, dist_coeffs_, rvec, tvec, false);
+			if (!res) return;
 
 			fillTransform(transform_.transform, rvec, tvec);
 			try {
-				geometry_msgs::TransformStamped snap_to = tf_buffer_.lookupTransform(markers->header.frame_id,
-				                                          known_tilt_, markers->header.stamp, ros::Duration(0.02));
-				snapOrientation(transform_.transform.rotation, snap_to.transform.rotation, auto_flip_);
+				geometry_msgs::TransformStamped snap_to = tf_buffer_.lookupTransform(markers.header.frame_id,
+				                                          snap_orientation_, markers.header.stamp, ros::Duration(0.02));
+				snapOrientation(transform_.transform.rotation, snap_to.transform.rotation);
 			} catch (const tf2::TransformException& e) {
-				NODELET_WARN_THROTTLE(1, "can't snap: %s", e.what());
+				ROS_WARN_THROTTLE(1, "aruco_map: 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;
+			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_);
-		}
+		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
+	void cinfoCallback(const sensor_msgs::CameraInfoConstPtr& cinfo)
+	{
+		parseCameraInfo(cinfo, camera_matrix_, dist_coeffs_);
+		has_camera_info_ = true;
+	}
+
+	void alignObjPointsToCenter(Mat &obj_points, double &center_x, double &center_y) 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++) {
+		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++) {
+		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;
 		}
 	}
 
@@ -271,7 +210,7 @@ publish_debug:
 		std::string line;
 
 		if (!f.good()) {
-			NODELET_FATAL("%s - %s", strerror(errno), filename.c_str());
+			ROS_FATAL("aruco_map: %s - %s", strerror(errno), filename.c_str());
 			ros::shutdown();
 		}
 
@@ -280,61 +219,22 @@ publish_debug:
 			double length, x, y, z, yaw, pitch, roll;
 
 			std::istringstream s(line);
+			ROS_INFO("aruco_map: parse line: %s", line.c_str());
 
-			// 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
+			if (!(s >> id >> length >> x >> y >> z >> yaw >> pitch >> roll)) {
+				ROS_ERROR("aruco_map: cannot parse line: %s", line.c_str());
 				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()));
+		ROS_INFO("aruco_map: 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");
+		ROS_INFO("aruco_map: generate gridboard");
+		ROS_WARN("aruco_map: gridboard maps are deprecated");
 
 		int markers_x, markers_y, first_marker;
 		double markers_side, markers_sep_x, markers_sep_y;
@@ -349,8 +249,8 @@ publish_debug:
 
 		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();
+				ROS_FATAL("~marker_ids length should be equal to ~markers_x * ~markers_y");
+				exit(1);
 			}
 		} else {
 			// Fill marker_ids automatically
@@ -361,153 +261,44 @@ publish_debug:
 			}
 		}
 
-		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);
-			}
-		}
+		createCustomGridBoard(board_, markers_x, markers_y, markers_side, markers_sep_x, markers_sep_y, marker_ids);
 	}
 
-	// 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/clover/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
+		geometry_msgs::TransformStamped t;
+		t.transform.translation.x = x;
+		t.transform.translation.y = y;
+		t.transform.translation.z = z;
 		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())
-		};
+		tf::quaternionTFToMsg(q, t.transform.rotation);
 
+		// TODO: process roll pitch yaw
+		vector<cv::Point3f> obj_points(4);
+		setMarkerObjectPoints(x, y, z, yaw, length, obj_points);
 		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()
+	void setMarkerObjectPoints(float x, float y, float z, float yaw, float length,
+	                           vector<cv::Point3f>& obj_points)
 	{
-		if (!markers_transforms_.empty()) {
-			static_br_.sendTransform(markers_transforms_);
-		}
-	}
-
-	void publishMarkers()
-	{
-		markers_pub_.publish(markers_);
+		obj_points[0] = cv::Point3f(x - length / 2, y + length / 2, 0);
+		obj_points[1] = obj_points[0] + cv::Point3f(length, 0, 0);
+		obj_points[2] = obj_points[0] + cv::Point3f(length, -length, 0);
+		obj_points[3] = obj_points[0] + cv::Point3f(0, -length, 0);
 	}
 
 	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;
-		}
-
+		cv::aruco::drawPlanarBoard(board_, cv::Size(2000, 2000), image, 50, 1);
+		cv::cvtColor(image, image, CV_GRAY2BGR);
+		msg.encoding = sensor_msgs::image_encodings::BGR8;
 		msg.image = image;
 		img_pub_.publish(msg.toImageMsg());
 	}

aruco_pose/src/draw.cpp

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

@@ -1,9 +0,0 @@
-#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/genmap.py

@@ -1,58 +0,0 @@
-#!/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 | --bottom-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 [default: 0]
-  --top-left     First marker is on top-left (default)
-  --bottom-left  First marker is on bottom-left
-
-Example:
-  rosrun aruco_pose genmap.py 0.33 2 4 1 1 0 > $(catkin_find aruco_pose map)/test_map.txt
-"""
-
-from __future__ import print_function
-
-from docopt import docopt
-
-
-arguments = docopt(__doc__)
-
-length = float(arguments['<length>'])
-first = int(arguments['<first>'] if arguments['<first>'] is not None else 0)
-markers_x = int(arguments['<x>'])
-markers_y = int(arguments['<y>'])
-dist_x = float(arguments['<dist_x>'])
-dist_y = float(arguments['<dist_y>'])
-bottom_left = arguments['--bottom-left']
-
-max_y = (markers_y - 1) * dist_y
-
-print('# id\tlength\tx\ty\tz\trot_z\trot_y\trot_x')
-for y in range(markers_y):
-    for x in range(markers_x):
-        pos_x = x * dist_x
-        pos_y = y * dist_y
-        if not bottom_left:
-            pos_y = max_y - pos_y
-        print('{}\t{}\t{}\t{}\t{}\t{}\t{}\t{}\t'.format(first, length, pos_x, pos_y, 0, 0, 0, 0))
-        first += 1

aruco_pose/src/gridboard.h

@@ -0,0 +1,22 @@
+void createCustomGridBoard(cv::Ptr<cv::aruco::Board>& board, int markersX, int markersY, float markerLength,
+                           float markerSeparationX, float markerSeparationY, std::vector<int> ids)
+{
+	size_t totalMarkers = (size_t) markersX * markersY;
+	board->ids = ids;
+	board->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);
+			board->objPoints.push_back(corners);
+		}
+	}
+}

aruco_pose/src/utils.h

@@ -1,17 +1,5 @@
-/*
- * 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>
@@ -30,7 +18,8 @@ static void param(ros::NodeHandle nh, const std::string& param_name, T& param_va
 	}
 }
 
-static void parseCameraInfo(const sensor_msgs::CameraInfoConstPtr& cinfo, cv::Mat& matrix, cv::Mat& dist)
+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)
@@ -101,33 +90,14 @@ inline void fillTranslation(geometry_msgs::Vector3& translation, const cv::Vec3d
 	translation.z = tvec[2];
 }
 
-inline bool isFlipped(tf::Quaternion& q)
+inline void snapOrientation(geometry_msgs::Quaternion& to, const geometry_msgs::Quaternion& from)
 {
-	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"
+	tf::Quaternion q;
+	q.setRPY(0, 0, -tf::getYaw(to) + tf::getYaw(from));
+	tf::Quaternion pq;
+	tf::quaternionMsgToTF(from, pq);
+	pq = pq * q;
+	tf::quaternionTFToMsg(pq, to);
 }
 
 inline void transformToPose(const geometry_msgs::Transform& transform, geometry_msgs::Pose& pose)

aruco_pose/test/basic.py

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

@@ -1,32 +0,0 @@
-<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"/>
-        <param name="cornerRefinementMethod" value="1"/>
-    </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

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

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

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

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

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

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

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

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

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

aruco_pose/test/test_parser_empty_map.test

@@ -1,14 +0,0 @@
-<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_parser_empty_map.txt"/>
-    </node>
-
-    <param name="test_module" value="$(find aruco_pose)/test/test_parser_empty_map.py"/>
-    <test test-name="test_node_empty_map" pkg="ros_pytest" type="ros_pytest_runner"/>
-</launch>

aruco_pose/test/test_parser_empty_map.txt

@@ -1,6 +0,0 @@
-# Failing markers: Not enough parameters to add a marker
-1
-2 1
-3 0.5 1
-# Failing markers: Marker IDs outside of dictionary range
-1001 1 1 0

aruco_pose/test/test_parser_pass.py

@@ -1,61 +0,0 @@
-import rospy
-import pytest
-
-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)
-
-def approx(expected):
-    return pytest.approx(expected, abs=1e-4) # compare floats more roughly
-
-def test_markers(node):
-    markers = rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5)
-    assert len(markers.markers) == 6
-
-    assert markers.markers[0].pose.position.x == approx(0)
-    assert markers.markers[0].pose.position.y == approx(0)
-    assert markers.markers[0].pose.position.z == approx(0)
-
-    assert markers.markers[1].pose.position.x == approx(1)
-    assert markers.markers[1].pose.position.y == approx(1)
-    assert markers.markers[1].pose.position.z == approx(1)
-
-    assert markers.markers[2].pose.position.x == approx(1)
-    assert markers.markers[2].pose.position.y == approx(0)
-    assert markers.markers[2].pose.position.z == approx(0.5)
-
-    assert markers.markers[3].pose.position.x == approx(0)
-    assert markers.markers[3].pose.position.y == approx(1)
-    assert markers.markers[3].pose.position.z == approx(0)
-
-    assert markers.markers[4].pose.position.x == approx(1)
-    assert markers.markers[4].pose.position.y == approx(0.5)
-    assert markers.markers[4].pose.position.z == approx(0)
-
-    assert markers.markers[5].pose.position.x == approx(2.2)
-    assert markers.markers[5].pose.position.y == approx(0.2)
-    assert markers.markers[5].pose.position.z == approx(0)
-
-    assert markers.markers[0].scale.x == approx(0.33)
-    assert markers.markers[0].scale.y == approx(0.33)
-    assert markers.markers[1].scale.x == approx(0.225)
-    assert markers.markers[1].scale.y == approx(0.225)
-    assert markers.markers[2].scale.x == approx(0.45)
-    assert markers.markers[2].scale.y == approx(0.45)
-    assert markers.markers[3].scale.x == approx(0.15)
-    assert markers.markers[3].scale.y == approx(0.15)
-    assert markers.markers[4].scale.x == approx(0.25)
-    assert markers.markers[4].scale.y == approx(0.25)
-    assert markers.markers[5].scale.x == approx(0.35)
-    assert markers.markers[5].scale.y == approx(0.35)
-
-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')

aruco_pose/test/test_parser_pass.test

@@ -1,14 +0,0 @@
-<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/test_parser_pass.txt"/>
-    </node>
-
-    <param name="test_module" value="$(find aruco_pose)/test/test_parser_pass.py"/>
-    <test test-name="test_node_pass" pkg="ros_pytest" type="ros_pytest_runner"/>
-</launch>

aruco_pose/test/test_parser_pass.txt

@@ -1,23 +0,0 @@
-# Parser test #1 - correct file
-# 1. Commentary test
-#Commentary text without space after pound sign
-# Commentary text with space after pound sign
-    # Commentary text with spaces before pound sign
-	# Commentary text with tab before pound sign
-			# Text with tabs before pound sign
-# Empty line test:
-
-# All-whitespace line test:
-		    
-# 2. Default coordinates test
-# Fully filled marker description, tab-delimited:
-1	0.33	0	0	0	0	0	0
-# Fully filled marker description, space-delimited:
-2 0.225 1 1 1 0 0 0
-# Default roll, pitch, yaw angles
-3 0.45 1 0 0.5
-# Default roll, pitch, yaw, z
-4 0.15 0 1
-# Inline commentary
-5 0.25 1 0.5# Comment straight after digit
-6 0.35 2.2 0.2 # Comment with a space after digit