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,5 +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

.gitignore

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

.markdownlint.json

@@ -1,81 +1,22 @@
 {
     "MD003": false,
-    "MD010": {
-        "code_blocks": false
-    },
     "MD013": false,
-    "MD024": false,
-    "MD026" :{
-        "punctuation": ".,;:!"
-    },
     "MD033": false,
     "MD034": false,
-    "MD040": false,
     "MD044": {
         "names": [
             "MAVLink",
             "ROS",
-            "ROS Kinetic",
-            "OpenCV",
-            "GitHub",
-            "FPV",
-            "PPM",
-            "PWM",
             "Python",
             "C++",
             "PX4",
-            "px4.io",
-            "logs.px4.io",
-            "QGroundControl",
-            "QGC",
             "WireShark",
-            "FlightPlot",
-            "OFFBOARD",
-            "LPE",
-            "EKF2",
-            "SITL",
-            "PID",
             "Wi-Fi",
             "Raspberry Pi",
-            "RPi",
-            "Linux",
-            "Windows",
-            "Docker",
-            "Travis",
-            "travis-ci.org",
-            "travis-ci.com",
-            "macOS",
-            "iOS",
-            "Android",
-            "Bluetooth",
-            "Raspbian",
-            "Raspbian Jesse",
-            "Raspbian Stretch",
             "Pixhawk",
             "Pixracer",
-            "Arduino",
+            "ArUco"
-            "GPS",
-            "ArUco",
-            "LIRC",
-            "GPIO",
-            "HC-SR04",
-            "STM",
-            "LED",
-            "USB",
-            "FAT32",
-            "uORB",
-            "SSH",
-            "API",
-            "UART",
-            "GND",
-            "VCC",
-            "SCL",
-            "SDA",
-            "TCP",
-            "UDP",
-            "QR"
         ],
         "code_blocks": false
-    },
+    }
-    "MD045": false
 }

.travis.yml

@@ -4,89 +4,30 @@ 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
+  depth: 1
-jobs:
+before_script:
-  fast_finish: true
+  - docker pull ${DOCKER}
-  include:
+script:
-    - stage: Build
+  - docker run --privileged --rm -v /dev:/dev -v $(pwd):/builder/repo -e TRAVIS_TAG="${TRAVIS_TAG}" ${DOCKER}
-      name: "Raspberry Pi Image Build"
+before_deploy:
-      cache:
+  # Set up git user name and tag this commit
-        directories:
+  - git config --local user.name "urpylka"
-        - imgcache
+  - git config --local user.email "urpylka@gmail.com"
-      before_script:
+  - sudo chmod -R 777 *
-        - docker pull ${DOCKER}
+  - cd images && zip ${IMAGE_NAME}.zip ${IMAGE_NAME}
-# Check if there are any cached images, copy them to our "images" directory
+deploy:
-        - if [ -n "$(ls -A imgcache/*.zip)" ]; then mkdir -p images && cp imgcache/*.zip images; fi
+  provider: releases
-      script:
+  api_key: ${GITHUB_OAUTH_TOKEN}
-        - if [[ -z ${TRAVIS_TAG} && ${TRAVIS_PULL_REQUEST} ]]; then
+  file: ${IMAGE_NAME}.zip
-            echo "Commit range is ${TRAVIS_COMMIT_RANGE}" &&
+  skip_cleanup: true
-            if [ $(git diff --name-only ${TRAVIS_COMMIT_RANGE} | grep -v ^"docs/" | wc -l) -eq 0 ]; then
+  on:
-              echo " === Docs-only change; skipping build ===" &&
+    tags: true
-              export SKIP_BUILD=true;
+
-            fi;
-          fi
-        - if [ -z ${SKIP_BUILD} ]; then
-            docker run --privileged --rm -v /dev:/dev -v $(pwd):/builder/repo -e TRAVIS_TAG="${TRAVIS_TAG}" ${DOCKER};
-          fi
-      before_cache:
-        - cp images/*.zip imgcache
-      before_deploy:
-        # Set up git user name and tag this commit
-        - git config --local user.name "goldarte"
-        - git config --local user.email "goldartt@gmail.com"
-        - sudo chmod -R 777 *
-        - cd images && zip ${IMAGE_NAME}.zip ${IMAGE_NAME}
-      deploy:
-        provider: releases
-        api_key: ${GITHUB_OAUTH_TOKEN}
-        file: ${IMAGE_NAME}.zip
-        skip_cleanup: true
-        on:
-          tags: true
-        draft: true
-        name: ${TRAVIS_TAG}
-    - stage: Build
-      name: "Documentation"
-      language: node_js
-      node_js:
-        - "10"
-      before_script:
-        - npm install gitbook-cli -g
-        - npm install markdownlint-cli -g
-        - gitbook -V
-        - markdownlint -V
-      script:
-        - markdownlint docs
-        - gitbook install
-        - gitbook build
-      deploy:
-        provider: pages
-        local-dir: _book
-        skip-cleanup: true
-        github-token: ${GITHUB_OAUTH_TOKEN}
-        keep-history: true
-        target-branch: master
-        repo: CopterExpress/clever-gitbook
-        fqdn: clever.copterexpress.com
-        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
-stages:
-  - Build
-  - Annotate
 # More info there
 # https://github.com/travis-ci/travis-ci/issues/6893
 # https://docs.travis-ci.com/user/customizing-the-build/

README.md

@@ -2,7 +2,7 @@
 
 <img src="docs/assets/clever3.png" align="right" width="300px" alt="CLEVER drone">
 
-CLEVER (Russian: *"Клевер"*, meaning *"Clover"*) is an educational programmable drone kit consisting of an unassembled quadcopter, open source software and documentation. The kit includes Pixhawk/Pixracer autopilot running PX4 firmware, Raspberry Pi 3 as companion computer, a camera for computer vision navigation as well as additional sensors and peripheral devices.
+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.
 
 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.
 
@@ -14,7 +14,7 @@ Use it to learn how to assemble, configure, pilot and program autonomous CLEVER
 
 **Preconfigured image for Raspberry Pi 3 with installed and configured software, ready to fly, is available [in the Releases section](https://github.com/CopterExpress/clever/releases).**
 
-[![Build Status](https://travis-ci.org/CopterExpress/clever.svg?branch=master)](https://travis-ci.org/CopterExpress/clever)
+[![Build Status](https://travis-ci.org/urpylka/clever.svg?branch=master)](https://travis-ci.org/urpylka/clever)
 
 Image includes:
 
@@ -23,10 +23,9 @@ Image includes:
 * Configured networking
 * OpenCV
 * mavros
-* Periphery drivers (`pigpiod`, `rpi_ws281x`, etc)
 * CLEVER software bundle for autonomous drone control
 
-API description (in Russian) for autonomous flights is available [on GitBook](https://clever.copterexpress.com/simple_offboard.html).
+API description (in Russian) for autonomous flights is available [on GitBook](https://copterexpress.gitbooks.io/clever/simple_offboard.html).
 
 ## Manual installation
 

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,139 +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",
+      "role" : "longLook",
-      "subtype" : "40mm"
+      "subtype" : "42mm"
-    },
-    {
-      "size" : "50x50",
-      "idiom" : "watch",
-      "scale" : "2x",
-      "role" : "appLauncher",
-      "subtype" : "44mm"
     },
     {
       "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

@@ -23,7 +23,7 @@ find_package(catkin REQUIRED COMPONENTS
   message_generation
 )
 
-find_package(OpenCV 3 REQUIRED)
+find_package(OpenCV REQUIRED)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
@@ -139,10 +139,12 @@ include_directories(
 add_library(aruco_pose
   src/aruco_detect.cpp
   src/aruco_map.cpp
-  src/draw.cpp
 )
 
-add_dependencies(${PROJECT_NAME} aruco_pose_generate_messages_cpp)
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
 ## Declare a C++ executable
 ## With catkin_make all packages are built within a single CMake context
@@ -162,7 +164,7 @@ add_dependencies(${PROJECT_NAME} aruco_pose_generate_messages_cpp)
 ## Specify libraries to link a library or executable target against
 target_link_libraries(aruco_pose
   ${catkin_LIBRARIES}
-  ${OpenCV_LIBRARIES}
+  ${OpenCV_LIBS}
 )
 
 #############
@@ -212,11 +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)
-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
-rostest aruco_pose basic.test
-```
-
-## Copyright
-
-Copyright © 2018 Copter Express Technologies. Author: Oleg Kalachev.
-
-Distributed under MIT License (https://opensource.org/licenses/MIT).

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/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/PoseWithCovariance pose
-geometry_msgs/Pose pose
 Point2D c1
 Point2D c2
 Point2D c3

aruco_pose/package.xml

@@ -1,35 +1,40 @@
 <?xml version="1.0"?>
-<package format="2">
+<package>
   <name>aruco_pose</name>
-  <version>0.0.1</version>
+  <version>0.0.0</version>
-  <description>Positioning with ArUco markers</description>
+  <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>
+  <build_depend>tf</build_depend>
-  <depend>nodelet</depend>
+  <build_depend>tf2</build_depend>
-  <depend>tf</depend>
+  <build_depend>tf2_ros</build_depend>
-  <depend>tf2</depend>
+  <build_depend>tf2_geometry_msgs</build_depend>
-  <depend>tf2_ros</depend>
+  <build_depend>cv_bridge</build_depend>
-  <depend>tf2_geometry_msgs</depend>
+  <build_depend>dynamic_reconfigure</build_depend>
-  <depend>opencv3</depend>
+  <build_depend>image_transport</build_depend>
-  <depend>cv_bridge</depend>
+  <build_depend>message_generation</build_depend>
-  <depend>image_transport</depend>
+  <build_depend>message_runtime</build_depend>
-  <depend>message_generation</depend>
+  <build_depend>nodelet</build_depend>
-  <depend>message_runtime</depend>
+  <build_depend>roscpp</build_depend>
-  <depend>std_msgs</depend>
+  <build_depend>std_msgs</build_depend>
-  <depend>geometry_msgs</depend>
+  <run_depend>nodelet</run_depend>
-  <depend>visualization_msgs</depend>
+  <run_depend>roscpp</run_depend>
-  <depend>sensor_msgs</depend>
+  <run_depend>image_transport</run_depend>
-  <depend>rostest</depend>
+  <run_depend>cv_bridge</run_depend>
-
+  <run_depend>message_runtime</run_depend>
-  <test_depend>image_publisher</test_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,8 +17,6 @@
 #include <math.h>
 #include <vector>
 #include <string>
-#include <map>
-#include <unordered_map>
 #include <ros/ros.h>
 #include <nodelet/nodelet.h>
 #include <pluginlib/class_list_macros.h>
@@ -62,10 +60,9 @@ private:
 	image_transport::Publisher debug_pub_;
 	image_transport::CameraSubscriber img_sub_;
 	ros::Publisher markers_pub_, vis_markers_pub_;
-	bool estimate_poses_, send_tf_, auto_flip_;
+	bool estimate_poses_, send_tf_;
 	double length_;
-	std::unordered_map<int, double> length_override_;
+	std::string snap_orientation_;
-	std::string frame_id_prefix_, known_tilt_;
 	Mat camera_matrix_, dist_coeffs_;
 	aruco_pose::MarkerArray array_;
 	visualization_msgs::MarkerArray vis_array_;
@@ -84,12 +81,7 @@ public:
 			ROS_FATAL("aruco_detect: can't estimate marker's poses as ~length parameter is not defined");
 			ros::shutdown();
 		}
-		readLengthOverride();
+		nh_priv_.param<std::string>("snap_orientation", snap_orientation_, "");
-
-		nh_priv_.param<std::string>("known_tilt", known_tilt_, "");
-		nh_priv_.param("auto_flip", auto_flip_, false);
-
-		nh_priv_.param<std::string>("frame_id_prefix", frame_id_prefix_, "aruco_");
 
 		camera_matrix_ = cv::Mat::zeros(3, 3, CV_64F);
 		dist_coeffs_ = cv::Mat::zeros(8, 1, CV_64F);
@@ -135,27 +127,9 @@ private:
 				cv::aruco::estimatePoseSingleMarkers(corners, length_, camera_matrix_, dist_coeffs_,
 				                                     rvecs, tvecs);
 
-				// process length override, TODO: efficiency
+				if (!snap_orientation_.empty()) {
-				if (!length_override_.empty()) {
-					for (unsigned int i = 0; i < ids.size(); i++) {
-						int id = ids[i];
-						auto item = length_override_.find(id);
-						if (item != length_override_.end()) { // found override
-							vector<cv::Vec3d> rvecs_current, tvecs_current;
-							vector<vector<cv::Point2f>> corners_current;
-							corners_current.push_back(corners[i]);
-							cv::aruco::estimatePoseSingleMarkers(corners_current, item->second,
-							                                     camera_matrix_, dist_coeffs_,
-										                         rvecs_current, tvecs_current);
-							rvecs[i] = rvecs_current[0];
-							tvecs[i] = tvecs_current[0];
-						}
-					}
-				}
-
-				if (!known_tilt_.empty()) {
 					try {
-						snap_to = tf_buffer_.lookupTransform(msg->header.frame_id, known_tilt_,
+						snap_to = tf_buffer_.lookupTransform(msg->header.frame_id, snap_orientation_,
 						                                     msg->header.stamp, ros::Duration(0.02));
 					} catch (const tf2::TransformException& e) {
 						ROS_WARN_THROTTLE(5, "aruco_detect: can't snap: %s", e.what());
@@ -171,21 +145,20 @@ 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)
+					// snap orientation (if enabled and snap frame avaiable)
-					if (!known_tilt_.empty() && !snap_to.header.frame_id.empty()) {
+					if (!snap_orientation_.empty() && !snap_to.header.frame_id.empty()) {
-						snapOrientation(marker.pose.orientation, snap_to.transform.rotation, auto_flip_);
+						snapOrientation(marker.pose.pose, snap_to.transform.rotation);
 					}
 
 					// TODO: check IDs are unique
 					if (send_tf_) {
 						transform.child_frame_id = getChildFrameId(ids[i]);
-						transform.transform.rotation = marker.pose.orientation;
+						transform.transform.rotation = marker.pose.pose.orientation;
 						fillTranslation(transform.transform.translation, tvecs[i]);
 						br_.sendTransform(transform);
 					}
@@ -206,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,
+				pushVisMarkers(msg->header.frame_id, msg->header.stamp, array_.markers[i].pose.pose,
-				               getMarkerLength(ids[i]), ids[i], i);
+				               length_, ids[i], i);
 
 			vis_markers_pub_.publish(vis_array_);
 		}
@@ -218,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_,
+					cv::aruco::drawAxis(debug, camera_matrix_, dist_coeffs_, rvecs[i], tvecs[i], length_);
-					                    rvecs[i], tvecs[i], getMarkerLength(ids[i]));
 
 			cv_bridge::CvImage out_msg;
 			out_msg.header.frame_id = msg->header.frame_id;
@@ -302,28 +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);
+		return "aruco_" + 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_;
-		}
 	}
 };
 

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>
@@ -23,20 +23,15 @@
 #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_geometry_msgs/tf2_geometry_msgs.h>
-#include <message_filters/subscriber.h>
-#include <message_filters/synchronizer.h>
-#include <message_filters/sync_policies/exact_time.h>
 #include <geometry_msgs/TransformStamped.h>
 #include <geometry_msgs/PoseWithCovarianceStamped.h>
 #include <sensor_msgs/Image.h>
 #include <visualization_msgs/Marker.h>
 #include <visualization_msgs/MarkerArray.h>
-#include <algorithm>
 
 #include <aruco_pose/MarkerArray.h>
 #include <aruco_pose/Marker.h>
@@ -44,26 +39,17 @@
 #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_, vis_markers_pub_;
+	ros::Publisher img_pub_, pose_pub_;
-	image_transport::Publisher debug_pub_;
+	ros::Subscriber markers_sub_, cinfo_sub;
-	message_filters::Subscriber<Image> image_sub_;
-	message_filters::Subscriber<CameraInfo> info_sub_;
-	message_filters::Subscriber<MarkerArray> markers_sub_;
-	boost::shared_ptr<message_filters::Synchronizer<SyncPolicy> > sync_;
 	cv::Ptr<cv::aruco::Board> board_;
 	Mat camera_matrix_, dist_coeffs_;
 	geometry_msgs::TransformStamped transform_;
@@ -71,10 +57,9 @@ private:
 	tf2_ros::TransformBroadcaster br_;
 	tf2_ros::Buffer tf_buffer_;
 	tf2_ros::TransformListener tf_listener_{tf_buffer_};
-	visualization_msgs::MarkerArray vis_array_;
+	visualization_msgs::MarkerArray vis_markers;
-	std::string known_tilt_;
+	std::string snap_orientation_;
-	int image_width_, image_height_, image_margin_;
+	bool has_camera_info_ = false;
-	bool auto_flip_;
 
 public:
 	virtual void onInit()
@@ -93,16 +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);
-
-		// createStripLine();
 
 		if (type == "map") {
 			param(nh_priv_, "map", map);
@@ -114,40 +91,32 @@ public:
 			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);
+		// TODO: use synchronised subscriber
-		info_sub_.subscribe(nh_, "camera_info", 1);
+		markers_sub_ = nh_.subscribe("markers", 1, &ArucoMap::markersCallback, this);
-		markers_sub_.subscribe(nh_, "markers", 1);
+		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));
 
 		publishMapImage();
-		vis_markers_pub_.publish(vis_array_);
-
 		ROS_INFO("aruco_map: ready");
 	}
 
-	void callback(const sensor_msgs::ImageConstPtr& image,
+	void markersCallback(const aruco_pose::MarkerArray& markers)
-	              const sensor_msgs::CameraInfoConstPtr& cinfo,
-	              const aruco_pose::MarkerArrayConstPtr& markers)
 	{
-		int valid = 0;
+		if (!has_camera_info_) return;
-		int count = markers->markers.size();
+		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),
@@ -158,35 +127,37 @@ public:
 			corners.push_back(marker_corners);
 		}
 
-		if (known_tilt_.empty()) {
+		Mat obj_points, img_points;
-			// simple estimation
+		cv::Vec3d rvec, tvec;
-			valid = cv::aruco::estimatePoseBoard(corners, ids, board_, camera_matrix_, dist_coeffs_,
-			                                     rvec, tvec, false);
-			if (!valid) goto publish_debug;
 
-			transform_.header.stamp = markers->header.stamp;
+		if (snap_orientation_.empty()) {
-			transform_.header.frame_id = markers->header.frame_id;
+			// 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;
+			double center_x = 0, center_y = 0;
-			alignObjPointsToCenter(obj_points, center_x, center_y, center_z);
+			alignObjPointsToCenter(obj_points, center_x, center_y);
 
-			valid = solvePnP(obj_points, img_points, camera_matrix_, dist_coeffs_, rvec, tvec, false);
+			int res = solvePnP(obj_points, img_points, camera_matrix_, dist_coeffs_, rvec, tvec, false);
-			if (!valid) goto publish_debug;
+			if (!res) return;
 
 			fillTransform(transform_.transform, rvec, tvec);
 			try {
-				geometry_msgs::TransformStamped snap_to = tf_buffer_.lookupTransform(markers->header.frame_id,
+				geometry_msgs::TransformStamped snap_to = tf_buffer_.lookupTransform(markers.header.frame_id,
-				                                          known_tilt_, markers->header.stamp, ros::Duration(0.02));
+				                                          snap_orientation_, markers.header.stamp, ros::Duration(0.02));
-				snapOrientation(transform_.transform.rotation, snap_to.transform.rotation, auto_flip_);
+				snapOrientation(transform_.transform.rotation, snap_to.transform.rotation);
 			} catch (const tf2::TransformException& e) {
 				ROS_WARN_THROTTLE(1, "aruco_map: can't snap: %s", e.what());
 			}
@@ -194,64 +165,42 @@ public:
 			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
+			transform_.header.stamp = markers.header.stamp;
-			tvec[0] = transform_.transform.translation.x;
+			transform_.header.frame_id = markers.header.frame_id;
-			tvec[1] = transform_.transform.translation.y;
-			tvec[2] = transform_.transform.translation.z;
-
-			transform_.header.stamp = markers->header.stamp;
-			transform_.header.frame_id = markers->header.frame_id;
 			pose_.header = transform_.header;
 			transformToPose(transform_.transform, pose_.pose.pose);
 		}
 
-		if (!transform_.child_frame_id.empty()) {
+		br_.sendTransform(transform_);
-			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;
 		}
 	}
 
@@ -270,51 +219,12 @@ 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
+			if (!(s >> id >> length >> x >> y >> z >> yaw >> pitch >> roll)) {
-			char first = 0;
+				ROS_ERROR("aruco_map: cannot parse line: %s", line.c_str());
-			if (!(s >> first)) {
-				// No non-whitespace characters, must be a blank line
 				continue;
 			}
-
-			if (first == '#') {
-				ROS_DEBUG("aruco_map: 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
-				ROS_FATAL("aruco_map: Malformed input: %s", line.c_str());
-				ros::shutdown();
-				throw std::runtime_error("Malformed input");
-			}
-
-			if (!(s >> id >> length >> x >> y)) {
-				ROS_ERROR("aruco_map: 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)) {
-				ROS_DEBUG("aruco_map: No z coordinate provided for marker %d, assuming 0", id);
-				z = 0;
-			}
-			if (!(s >> yaw)) {
-				ROS_DEBUG("aruco_map: No yaw provided for marker %d, assuming 0", id);
-				yaw = 0;
-			}
-			if (!(s >> pitch)) {
-				ROS_DEBUG("aruco_map: No pitch provided for marker %d, assuming 0", id);
-				pitch = 0;
-			}
-			if (!(s >> roll)) {
-				ROS_DEBUG("aruco_map: No roll provided for marker %d, assuming 0", id);
-				roll = 0;
-			}
 			addMarker(id, length, x, y, z, yaw, pitch, roll);
 		}
 
@@ -340,7 +250,7 @@ publish_debug:
 		if (nh_priv_.getParam("marker_ids", marker_ids)) {
 			if ((unsigned int)(markers_x * markers_y) != marker_ids.size()) {
 				ROS_FATAL("~marker_ids length should be equal to ~markers_x * ~markers_y");
-				ros::shutdown();
+				exit(1);
 			}
 		} else {
 			// Fill marker_ids automatically
@@ -351,122 +261,44 @@ publish_debug:
 			}
 		}
 
-		double max_y = markers_y * markers_side + (markers_y - 1) * markers_sep_y;
+		createCustomGridBoard(board_, markers_x, markers_y, markers_side, markers_sep_x, markers_sep_y, marker_ids);
-		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;
-				ROS_INFO("add marker %d %g %g", marker_ids[y * markers_y + x], x_pos, y_pos);
-				addMarker(marker_ids[y * markers_y + x], markers_side, x_pos, y_pos, 0, 0, 0, 0);
-			}
-		}
 	}
 
-	// void createStripLine()
-	// {
-	// 	visualization_msgs::Marker marker;
-	// 	marker.header.frame_id = transform_.child_frame_id;
-	// 	marker.action = visualization_msgs::Marker::ADD;
-	// 	marker.ns = "aruco_map_link";
-	// 	marker.type = visualization_msgs::Marker::LINE_STRIP;
-	// 	marker.scale.x = 0.02;
-	// 	marker.color.g = 1;
-	// 	marker.color.a = 0.8;
-	// 	marker.frame_locked = true;
-	// 	marker.pose.orientation.w = 1;
-	// 	vis_array_.markers.push_back(marker);
-	// }
-
 	void addMarker(int id, double length, double x, double y, double z,
 				   double yaw, double pitch, double roll)
 	{
-		// Check whether the id is in range for current dictionary
-		int num_markers = board_->dictionary->bytesList.rows;
-		if (num_markers <= id) {
-			ROS_ERROR("aruco_map: Marker id %d is not in dictionary; current dictionary contains %d markers. "
-			          "Please see https://github.com/CopterExpress/clever/blob/master/aruco_pose/README.md#parameters for details",
-					  id, num_markers);
-			return;
-		}
-		// Check if marker is already in the board
-		if (std::count(board_->ids.begin(), board_->ids.end(), id) > 0) {
-			ROS_ERROR("aruco_map: 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));
+		tf::quaternionTFToMsg(q, t.transform.rotation);
-
-		/* 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())
-		};
 
+		// 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 visualization marker
+	void setMarkerObjectPoints(float x, float y, float z, float yaw, float length,
-		visualization_msgs::Marker marker;
+	                           vector<cv::Point3f>& obj_points)
-		marker.header.frame_id = transform_.child_frame_id;
+	{
-		// marker.header.stamp = stamp;
+		obj_points[0] = cv::Point3f(x - length / 2, y + length / 2, 0);
-		marker.action = visualization_msgs::Marker::ADD;
+		obj_points[1] = obj_points[0] + cv::Point3f(length, 0, 0);
-		marker.id = vis_array_.markers.size();
+		obj_points[2] = obj_points[0] + cv::Point3f(length, -length, 0);
-		marker.ns = "aruco_map_marker";
+		obj_points[3] = obj_points[0] + cv::Point3f(0, -length, 0);
-		marker.type = visualization_msgs::Marker::CUBE;
-		marker.scale.x = length;
-		marker.scale.y = length;
-		marker.scale.z = 0.001;
-		marker.color.r = 1;
-		marker.color.g = 0.5;
-		marker.color.b = 0.5;
-		marker.color.a = 0.8;
-		marker.pose.position.x = x;
-		marker.pose.position.y = y;
-		marker.pose.position.z = z;
-		tf::quaternionTFToMsg(q, marker.pose.orientation);
-		marker.frame_locked = true;
-		vis_array_.markers.push_back(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 publishMapImage()
 	{
-		cv::Size size(image_width_, image_height_);
 		cv::Mat image;
 		cv_bridge::CvImage msg;
-
+		cv::aruco::drawPlanarBoard(board_, cv::Size(2000, 2000), image, 50, 1);
-		if (!board_->ids.empty()) {
+		cv::cvtColor(image, image, CV_GRAY2BGR);
-			_drawPlanarBoard(board_, size, image, image_margin_, 1);
+		msg.encoding = sensor_msgs::image_encodings::BGR8;
-		} else {
-			// empty map
-			image.create(size, CV_8UC1);
-			image.setTo(cv::Scalar::all(255));
-		}
-
-		msg.encoding = sensor_msgs::image_encodings::MONO8;
 		msg.image = image;
 		img_pub_.publish(msg.toImageMsg());
 	}

aruco_pose/src/draw.cpp

@@ -1,797 +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"
-
-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) {
-
-	CV_Assert(outSize.area() > 0);
-	CV_Assert(marginSize >= 0);
-
-	_img.create(outSize, CV_8UC1);
-	Mat out = _img.getMat();
-	out.setTo(Scalar::all(255));
-	out.adjustROI(-marginSize, -marginSize, -marginSize, -marginSize);
-
-	// calculate max and min values in XY plane
-	CV_Assert(_board->objPoints.size() > 0);
-	float minX, maxX, minY, maxY;
-	minX = maxX = _board->objPoints[0][0].x;
-	minY = maxY = _board->objPoints[0][0].y;
-
-	for(unsigned int i = 0; i < _board->objPoints.size(); i++) {
-		for(int j = 0; j < 4; j++) {
-			minX = min(minX, _board->objPoints[i][j].x);
-			maxX = max(maxX, _board->objPoints[i][j].x);
-			minY = min(minY, _board->objPoints[i][j].y);
-			maxY = max(maxY, _board->objPoints[i][j].y);
-		}
-	}
-
-	float sizeX = maxX - minX;
-	float sizeY = maxY - minY;
-
-	// proportion transformations
-	float xReduction = sizeX / float(out.cols);
-	float yReduction = sizeY / float(out.rows);
-
-	// determine the zone where the markers are placed
-	if(xReduction > yReduction) {
-		int nRows = int(sizeY / xReduction);
-		int rowsMargins = (out.rows - nRows) / 2;
-		out.adjustROI(-rowsMargins, -rowsMargins, 0, 0);
-	} else {
-		int nCols = int(sizeX / yReduction);
-		int colsMargins = (out.cols - nCols) / 2;
-		out.adjustROI(0, 0, -colsMargins, -colsMargins);
-	}
-
-	// now paint each marker
-	Dictionary &dictionary = *(_board->dictionary);
-	Mat marker;
-	Point2f outCorners[3];
-	Point2f inCorners[3];
-	for(unsigned int m = 0; m < _board->objPoints.size(); m++) {
-		// transform corners to markerZone coordinates
-		for(int j = 0; j < 3; j++) {
-			Point2f pf = Point2f(_board->objPoints[m][j].x, _board->objPoints[m][j].y);
-			// move top left to 0, 0
-			pf -= Point2f(minX, minY);
-			pf.x = pf.x / sizeX * float(out.cols);
-			pf.y = (1.0f - pf.y / sizeY) * float(out.rows);
-			outCorners[j] = pf;
-		}
-
-		// get marker
-		Size dst_sz(outCorners[2] - outCorners[0]); // assuming CCW order
-		// dst_sz.width = dst_sz.height = std::min(dst_sz.width, dst_sz.height); //marker should be square
-		double diag = std::round(std::hypot(dst_sz.width, dst_sz.height));
-		int side = std::round(diag / std::sqrt(2));
-
-		dictionary.drawMarker(_board->ids[m], side, marker, borderBits);
-
-		// 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 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,8 +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);
-void _drawAxis(cv::InputOutputArray image, cv::InputArray cameraMatrix, cv::InputArray distCoeffs,
-              cv::InputArray rvec, cv::InputArray tvec, float length);

aruco_pose/src/genmap.py

@@ -1,53 +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]
-  genmap.py (-h | --help)
-
-Options:
-  <length>    Marker side length
-  <x>         Marker count along X axis
-  <y>         Marker count along Y axis
-  <dist_x>    Distance between markers along X axis
-  <dist_y>    Distance between markers along Y axis
-  <first>     First marker ID
-  --top-left  First marker is on top-left (not bottom-left)
-"""
-
-from __future__ import print_function
-
-from docopt import docopt
-
-
-arguments = docopt(__doc__)
-
-length = float(arguments['<length>'])
-first = int(arguments['<first>'])
-markers_x = int(arguments['<x>'])
-markers_y = int(arguments['<y>'])
-dist_x = float(arguments['<dist_x>'])
-dist_y = float(arguments['<dist_y>'])
-top_left = arguments['--top-left']
-
-max_y = (markers_y - 1) * dist_y
-
-for y in range(markers_y):
-    for x in range(markers_x):
-        pos_x = x * dist_x
-        pos_y = y * dist_y
-        if top_left:
-            pos_y = max_y - pos_y
-        print('{}\t{}\t{}\t{}\t{}\t{}\t{}\t{}\t'.format(first, length, pos_x, pos_y, 0, 0, 0, 0))
-        first += 1

aruco_pose/src/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>
@@ -102,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::Quaternion q;
-	tf::Matrix3x3(q).getEulerYPR(yaw, pitch, roll);
+	q.setRPY(0, 0, -tf::getYaw(to) + tf::getYaw(from));
-	return (abs(pitch) > M_PI / 2) || (abs(roll) > M_PI / 2);
+	tf::Quaternion pq;
-}
+	tf::quaternionMsgToTF(from, pq);
-
+	pq = pq * q;
-/* Set roll and pitch from "from" to "to", keeping yaw */
+	tf::quaternionTFToMsg(pq, to);
-inline void snapOrientation(geometry_msgs::Quaternion& to, const geometry_msgs::Quaternion& from, bool auto_flip = false)
-{
-	tf::Quaternion _from, _to;
-	tf::quaternionMsgToTF(from, _from);
-	tf::quaternionMsgToTF(to, _to);
-
-	if (auto_flip) {
-		if (!isFlipped(_from)) {
-			static const tf::Quaternion flip = tf::createQuaternionFromRPY(M_PI, 0, 0);
-			_from *= flip; // flip "from"
-		}
-	}
-
-	auto diff = tf::Matrix3x3(_to).transposeTimes(tf::Matrix3x3(_from));
-	double _, yaw;
-	diff.getRPY(_, _, yaw);
-	auto q = tf::createQuaternionFromRPY(0, 0, -yaw);
-	_from = _from * q; // set yaw from "to" to "from"
-	tf::quaternionTFToMsg(_from, to); // set "from" to "to"
 }
 
 inline void transformToPose(const geometry_msgs::Transform& transform, geometry_msgs::Pose& pose)

aruco_pose/test/basic.py

@@ -1,101 +0,0 @@
-#!/usr/bin/env python
-import sys
-import unittest
-import json
-import rospy
-import rostest
-
-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
-
-
-class TestArucoPose(unittest.TestCase):
-    def setUp(self):
-        rospy.init_node('test_aruco_detect', anonymous=True)
-
-    def test_markers(self):
-        markers = rospy.wait_for_message('aruco_detect/markers', MarkerArray, timeout=5)
-        self.assertEqual(len(markers.markers), 4)
-        self.assertEqual(markers.header.frame_id, 'main_camera_optical')
-
-        self.assertEqual(markers.markers[0].id, 2)
-        self.assertAlmostEqual(markers.markers[0].length, 0.33, places=4)
-        self.assertAlmostEqual(markers.markers[0].pose.position.x, 0.36706567854, places=4)
-        self.assertAlmostEqual(markers.markers[0].pose.position.y, 0.290484516644, places=4)
-        self.assertAlmostEqual(markers.markers[0].pose.position.z, 2.18787602301, places=4)
-        self.assertAlmostEqual(markers.markers[0].pose.orientation.x, 0.993997406299, places=4)
-        self.assertAlmostEqual(markers.markers[0].pose.orientation.y, -0.00532003481626, places=4)
-        self.assertAlmostEqual(markers.markers[0].pose.orientation.z, -0.107390951553, places=4)
-        self.assertAlmostEqual(markers.markers[0].pose.orientation.w, 0.0201999263402, places=4)
-        self.assertAlmostEqual(markers.markers[0].c1.x, 415.557739258, places=4)
-        self.assertAlmostEqual(markers.markers[0].c1.y, 335.557739258, places=4)
-        self.assertAlmostEqual(markers.markers[0].c2.x, 509.442260742, places=4)
-        self.assertAlmostEqual(markers.markers[0].c2.y, 335.557739258, places=4)
-        self.assertAlmostEqual(markers.markers[0].c3.x, 509.442260742, places=4)
-        self.assertAlmostEqual(markers.markers[0].c3.y, 429.442260742, places=4)
-        self.assertAlmostEqual(markers.markers[0].c4.x, 415.557739258, places=4)
-        self.assertAlmostEqual(markers.markers[0].c4.y, 429.442260742, places=4)
-
-        self.assertEqual(markers.markers[3].id, 3)
-        self.assertAlmostEqual(markers.markers[3].length, 0.1, places=4)
-        self.assertAlmostEqual(markers.markers[3].pose.position.x, -0.1805169666, places=4)
-        self.assertAlmostEqual(markers.markers[3].pose.position.y, -0.200697302327, places=4)
-        self.assertAlmostEqual(markers.markers[3].pose.position.z, 0.585767514823, places=4)
-        self.assertAlmostEqual(markers.markers[3].pose.orientation.x, -0.961738074009, places=4)
-        self.assertAlmostEqual(markers.markers[3].pose.orientation.y, -0.0375180244707, places=4)
-        self.assertAlmostEqual(markers.markers[3].pose.orientation.z, -0.0115387773672, places=4)
-        self.assertAlmostEqual(markers.markers[3].pose.orientation.w, 0.271144115664, places=4)
-        self.assertAlmostEqual(markers.markers[3].c1.x, 129.557723999, places=4)
-        self.assertAlmostEqual(markers.markers[3].c1.y, 49.557723999, places=4)
-        self.assertAlmostEqual(markers.markers[3].c2.x, 223.442276001, places=4)
-        self.assertAlmostEqual(markers.markers[3].c2.y, 49.557723999, places=4)
-        self.assertAlmostEqual(markers.markers[3].c3.x, 223.442276001, places=4)
-        self.assertAlmostEqual(markers.markers[3].c3.y, 143.442276001, places=4)
-        self.assertAlmostEqual(markers.markers[3].c4.x, 129.557723999, places=4)
-        self.assertAlmostEqual(markers.markers[3].c4.y, 143.442276001, places=4)
-
-        self.assertEqual(markers.markers[1].id, 1)
-        self.assertAlmostEqual(markers.markers[1].length, 0.33, places=4)
-        self.assertEqual(markers.markers[2].id, 4)
-        self.assertAlmostEqual(markers.markers[2].length, 0.33, places=4)
-
-    def test_visualization(self):
-        vis = rospy.wait_for_message('aruco_detect/visualization', VisMarkerArray, timeout=5)
-        self.assertEqual(len(vis.markers), 9)
-
-    def test_debug(self):
-        img = rospy.wait_for_message('aruco_detect/debug', Image, timeout=5)
-        self.assertEqual(img.width, 640)
-        self.assertEqual(img.height, 480)
-        self.assertEqual(img.header.frame_id, 'main_camera_optical')
-
-    def test_map(self):
-        pose = rospy.wait_for_message('aruco_map/pose', PoseWithCovarianceStamped, timeout=5)
-        self.assertEqual(pose.header.frame_id, 'main_camera_optical')
-        self.assertAlmostEqual(pose.pose.pose.position.x, -0.629167753342, places=4)
-        self.assertAlmostEqual(pose.pose.pose.position.y, 0.293822650809, places=4)
-        self.assertAlmostEqual(pose.pose.pose.position.z, 2.12641343155, places=4)
-        self.assertAlmostEqual(pose.pose.pose.orientation.x, -0.998383794799, places=4)
-        self.assertAlmostEqual(pose.pose.pose.orientation.y, -5.20919098575e-06, places=4)
-        self.assertAlmostEqual(pose.pose.pose.orientation.z, -0.0300861070302, places=4)
-        self.assertAlmostEqual(pose.pose.pose.orientation.w, 0.0482143590507, places=4)
-
-    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.assertEqual(img.encoding, 'mono8')
-
-    def test_map_visualization(self):
-        vis = rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5)
-
-    def test_map_debug(self):
-        img = rospy.wait_for_message('aruco_map/debug', Image, timeout=5)
-
-    # def test_transforms(self):
-    #     pass
-
-
-rostest.rosrun('aruco_pose', 'test_aruco_detect', TestArucoPose, sys.argv)

aruco_pose/test/basic.test

@@ -1,27 +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"/>
-
-    <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"/>
-        <param name="length_override/3" value="0.1"/>
-        <param name="estimate_poses" value="true"/>
-    </node>
-
-    <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/basic.txt"/>
-    </node>
-
-    <test test-name="test_aruco_pose" pkg="aruco_pose" type="basic.py"/>
-</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/test_node_failure.py

@@ -1,27 +0,0 @@
-#!/usr/bin/env python
-import sys
-import unittest
-import json
-import rospy
-import rostest
-
-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
-
-
-class TestArucoMapPass(unittest.TestCase):
-    def setUp(self):
-        rospy.init_node('test_parser_fail', anonymous=True)
-
-    def test_node_failure(self):
-        try:
-            markers = rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5)
-            did_post_message = True
-        except rospy.exceptions.ROSException:
-            did_post_message = False
-        self.assertFalse(did_post_message)
-
-
-rostest.rosrun('aruco_pose', 'test_aruco_map', TestArucoMapPass, sys.argv)

aruco_pose/test/test_node_failure.test

@@ -1,13 +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>
-
-    <test test-name="test_aruco_map_fail_dict" pkg="aruco_pose" type="test_node_failure.py"/>
-</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,24 +0,0 @@
-#!/usr/bin/env python
-import sys
-import unittest
-import json
-import rospy
-import rostest
-
-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
-
-
-class TestArucoMapPass(unittest.TestCase):
-    def setUp(self):
-        rospy.init_node('test_parser_fail', anonymous=True)
-
-
-    def test_node_failure(self):
-        markers = rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5)
-        self.assertEquals(len(markers.markers), 0)
-
-
-rostest.rosrun('aruco_pose', 'test_aruco_map', TestArucoMapPass, sys.argv)

aruco_pose/test/test_parser_empty_map.test

@@ -1,13 +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>
-
-    <test test-name="test_aruco_map_incomplete" pkg="aruco_pose" type="test_parser_empty_map.py"/>
-</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,75 +0,0 @@
-#!/usr/bin/env python
-import sys
-import unittest
-import json
-import rospy
-import rostest
-
-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
-
-
-class TestArucoMapPass(unittest.TestCase):
-    def setUp(self):
-        rospy.init_node('test_parser_pass', anonymous=True)
-
-    def test_markers(self):
-        markers = rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5)
-        
-        self.assertEqual(len(markers.markers), 6)
-# FIXME: visual marker id is not ArUco marker id
-#        self.assertEqual(markers.markers[0].id, 1)
-#        self.assertEqual(markers.markers[1].id, 2)
-#        self.assertEqual(markers.markers[2].id, 3)
-#        self.assertEqual(markers.markers[3].id, 4)
-
-        self.assertAlmostEqual(markers.markers[0].pose.position.x, 0, places=7)
-        self.assertAlmostEqual(markers.markers[0].pose.position.y, 0, places=7)
-        self.assertAlmostEqual(markers.markers[0].pose.position.z, 0, places=7)
-        
-        self.assertAlmostEqual(markers.markers[1].pose.position.x, 1, places=7)
-        self.assertAlmostEqual(markers.markers[1].pose.position.y, 1, places=7)
-        self.assertAlmostEqual(markers.markers[1].pose.position.z, 1, places=7)
-        
-        self.assertAlmostEqual(markers.markers[2].pose.position.x, 1, places=7)
-        self.assertAlmostEqual(markers.markers[2].pose.position.y, 0, places=7)
-        self.assertAlmostEqual(markers.markers[2].pose.position.z, 0.5, places=7)
-        
-        self.assertAlmostEqual(markers.markers[3].pose.position.x, 0, places=7)
-        self.assertAlmostEqual(markers.markers[3].pose.position.y, 1, places=7)
-        self.assertAlmostEqual(markers.markers[3].pose.position.z, 0, places=7)
-        
-        self.assertAlmostEqual(markers.markers[4].pose.position.x, 1, places=7)
-        self.assertAlmostEqual(markers.markers[4].pose.position.y, 0.5, places=7)
-        self.assertAlmostEqual(markers.markers[4].pose.position.z, 0, places=7)
-        
-        self.assertAlmostEqual(markers.markers[5].pose.position.x, 2.2, places=7)
-        self.assertAlmostEqual(markers.markers[5].pose.position.y, 0.2, places=7)
-        self.assertAlmostEqual(markers.markers[5].pose.position.z, 0, places=7)
-
-        self.assertAlmostEqual(markers.markers[0].scale.x, 0.33, places=7)
-        self.assertAlmostEqual(markers.markers[0].scale.y, 0.33, places=7)
-        self.assertAlmostEqual(markers.markers[1].scale.x, 0.225, places=7)
-        self.assertAlmostEqual(markers.markers[1].scale.y, 0.225, places=7)
-        self.assertAlmostEqual(markers.markers[2].scale.x, 0.45, places=7)
-        self.assertAlmostEqual(markers.markers[2].scale.y, 0.45, places=7)
-        self.assertAlmostEqual(markers.markers[3].scale.x, 0.15, places=7)
-        self.assertAlmostEqual(markers.markers[3].scale.y, 0.15, places=7)
-        self.assertAlmostEqual(markers.markers[4].scale.x, 0.25, places=7)
-        self.assertAlmostEqual(markers.markers[4].scale.y, 0.25, places=7)
-        self.assertAlmostEqual(markers.markers[5].scale.x, 0.35, places=7)
-        self.assertAlmostEqual(markers.markers[5].scale.y, 0.35, places=7)
-
-    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.assertEqual(img.encoding, 'mono8')
-
-    # def test_transforms(self):
-    #     pass
-
-
-rostest.rosrun('aruco_pose', 'test_aruco_map', TestArucoMapPass, sys.argv)

aruco_pose/test/test_parser_pass.test

@@ -1,13 +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_pass.txt"/>
-    </node>
-
-    <test test-name="test_aruco_map" pkg="aruco_pose" type="test_parser_pass.py"/>
-</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

book.json

@@ -4,26 +4,19 @@
     "author": "Copter Express",
     "language": "ru",
     "root": "docs/",
-    "gitbook": "^3.2.2",
+    "plugins": ["youtube", "richquotes", "versions", "yametrika"],
-    "plugins": [
-        "youtube",
-        "richquotes@https://github.com/okalachev/gitbook-plugin-richquotes.git",
-        "yametrika",
-        "anchors",
-        "validate-links",
-        "bulk-redirect@https://github.com/okalachev/gitbook-plugin-bulk-redirect.git",
-        "sitemap@https://github.com/okalachev/plugin-sitemap.git"
-    ],
     "pluginsConfig": {
+        "disqus": {
+            "shortName": "coex-clever"
+        },
+        "versions": {
+            "type": "tags"
+        },
         "yametrika": {
             "id": 49359238
-        },
-        "bulk-redirect": {
-            "basepath": "",
-            "redirectsFile": "redirects.json"
-        },
-        "sitemap": {
-            "hostname": "https://clever.copterexpress.com"
         }
+    },
+    "structure": {
+        "glossary": "_GLOSSARY.md"
     }
 }

builder/assets/99-px4fmu.rules

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

builder/assets/clever.service

@@ -4,11 +4,9 @@ Requires=roscore.service
 After=roscore.service
 
 [Service]
-User=pi
 EnvironmentFile=/lib/systemd/system/roscore.env
-ExecStart=/opt/ros/kinetic/bin/roslaunch clever clever.launch --wait --screen
+ExecStart=/opt/ros/kinetic/bin/roslaunch clever clever.launch --wait
-Restart=on-failure
+Restart=on-abort
-RestartSec=3
 
 [Install]
 WantedBy=multi-user.target

builder/assets/clever_rename

@@ -1,35 +0,0 @@
-#!/usr/bin/env bash
-
-# Set Clever hostname to the specified value
-
-set -e
-
-NEW_NAME_OPT=$1
-
-if [[ -z ${NEW_NAME_OPT} ]]; then
-  echo "Please specify new name for this Clever"
-  exit 1
-fi
-
-NEW_NAME=$(echo ${NEW_NAME_OPT} | tr '[:upper:]' '[:lower:]')
-
-echo "Setting name to ${NEW_NAME}"
-
-echo "Backing up /etc/hostname"
-cp /etc/hostname /etc/hostname.bak
-echo "Writing new /etc/hostname"
-echo ${NEW_NAME} > /etc/hostname
-
-echo "Backing up /etc/hosts"
-cp /etc/hosts /etc/hosts.bak
-echo "Rewriting /etc/hosts with new values"
-sed -i 's/127\.0\.1\.1.*/127.0.1.1\t'${NEW_NAME}'/g' /etc/hosts
-
-echo "Changing hostname in /lib/systemd/system/roscore.env"
-sed -i 's/ROS_HOSTNAME=.*/ROS_HOSTNAME='${NEW_NAME}'.local/g' /lib/systemd/system/roscore.env
-
-echo "Changing hostname in .bashrc"
-sed -i 's/export ROS_HOSTNAME=.*/export ROS_HOSTNAME='${NEW_NAME}'.local/g' /home/pi/.bashrc
-
-echo "Done, reboot your Clever to see the results"
-

builder/assets/hardware_setup.sh

@@ -8,10 +8,6 @@
 #
 # Author: Artem Smirnov <urpylka@gmail.com>
 #
-# Distributed under MIT License (available at https://opensource.org/licenses/MIT).
-# The above copyright notice and this permission notice shall be included in all
-# copies or substantial portions of the Software.
-#
 
 set -e # Exit immidiately on non-zero result