docs: some info on optical flow troubleshooting

.gitattributes

@@ -1,3 +1,2 @@
 apps/ios/cleverrc/roslib.js linguist-vendored
 apps/ios/cleverrc/BinUtils.swift linguist-vendored
-apps/android/app/src/main/assets/roslib.js linguist-vendored

.gitignore

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

.markdownlint.json

@@ -1,16 +1,8 @@
 {
     "MD003": false,
-    "MD010": {
-        "code_blocks": false
-    },
     "MD013": false,
-    "MD024": false,
-    "MD026" :{
-        "punctuation": ".,;:!"
-    },
     "MD033": false,
     "MD034": false,
-    "MD040": false,
     "MD044": {
         "names": [
             "MAVLink",
@@ -26,6 +18,5 @@
             "ArUco"
         ],
         "code_blocks": false
-    },
-    "MD045": false
+    }
 }

.travis.yml

@@ -4,7 +4,7 @@ services:
   - docker
 env:
   global:
-    - DOCKER="sfalexrog/img-tool:builder-mod"
+    - 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"
@@ -16,8 +16,8 @@ script:
   - docker run --privileged --rm -v /dev:/dev -v $(pwd):/builder/repo -e TRAVIS_TAG="${TRAVIS_TAG}" ${DOCKER}
 before_deploy:
   # Set up git user name and tag this commit
-  - git config --local user.name "goldarte"
-  - git config --local user.email "goldartt@gmail.com"
+  - git config --local user.name "urpylka"
+  - git config --local user.email "urpylka@gmail.com"
   - sudo chmod -R 777 *
   - cd images && zip ${IMAGE_NAME}.zip ${IMAGE_NAME}
 deploy:

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/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

@@ -16,12 +16,14 @@ find_package(catkin REQUIRED COMPONENTS
   image_transport
   cv_bridge
   tf
-  #tf2
-  #tf2_ros
-  #aruco_msgs
+  tf2
+  tf2_ros
+  tf2_geometry_msgs
+  sensor_msgs
+  message_generation
 )
 
-find_package(OpenCV 3 REQUIRED)
+find_package(OpenCV REQUIRED)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
@@ -57,11 +59,12 @@ find_package(OpenCV 3 REQUIRED)
 ##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
 
 ## Generate messages in the 'msg' folder
-#add_message_files(
-#  FILES
-#  Marker.msg
-#  MarkerArray.msg
-#)
+add_message_files(
+  FILES
+  Point2D.msg
+  Marker.msg
+  MarkerArray.msg
+)
 
 ## Generate services in the 'srv' folder
 # add_service_files(
@@ -78,10 +81,11 @@ find_package(OpenCV 3 REQUIRED)
 # )
 
 ## Generate added messages and services with any dependencies listed here
-#generate_messages(
-#  DEPENDENCIES
-#  std_msgs  # Or other packages containing msgs
-#)
+generate_messages(
+  DEPENDENCIES
+  std_msgs
+  geometry_msgs
+)
 
 ################################################
 ## Declare ROS dynamic reconfigure parameters ##
@@ -113,9 +117,9 @@ find_package(OpenCV 3 REQUIRED)
 ## CATKIN_DEPENDS: catkin_packages dependent projects also need
 ## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
-#  INCLUDE_DIRS include
+  INCLUDE_DIRS DEPENDS OpenCV
   LIBRARIES aruco_pose
-#  CATKIN_DEPENDS other_catkin_pkg
+  CATKIN_DEPENDS message_runtime
 #  DEPENDS system_lib
 )
 
@@ -128,11 +132,13 @@ catkin_package(
 include_directories(
 # include
   ${catkin_INCLUDE_DIRS}
+  ${OpenCV_INCLUDE_DIRS}
 )
 
 ## Declare a C++ library
-add_library(${PROJECT_NAME}
-  src/aruco_pose.cpp
+add_library(aruco_pose
+  src/aruco_detect.cpp
+  src/aruco_map.cpp
 )
 
 ## Add cmake target dependencies of the library
@@ -156,11 +162,9 @@ add_library(${PROJECT_NAME}
 # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
 ## Specify libraries to link a library or executable target against
-link_directories(/opt/ros/kinetic/lib)
-
-target_link_libraries(${PROJECT_NAME}
+target_link_libraries(aruco_pose
   ${catkin_LIBRARIES}
-  ${OpenCV_LIBRARIES}
+  ${OpenCV_LIBS}
 )
 
 #############

aruco_pose/map/map.txt

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

aruco_pose/msg/Marker.msg

@@ -0,0 +1,6 @@
+uint32 id
+geometry_msgs/PoseWithCovariance pose
+Point2D c1
+Point2D c2
+Point2D c3
+Point2D c4

aruco_pose/msg/MarkerArray.msg

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

aruco_pose/msg/Point2D.msg

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

aruco_pose/nodelet_plugins.xml

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

aruco_pose/package.xml

@@ -2,7 +2,7 @@
 <package>
   <name>aruco_pose</name>
   <version>0.0.0</version>
-  <description>ArUco maps precise pose estimation nodelet</description>
+  <description>Positioning by ArUco markers</description>
 
   <maintainer email="okalachev@gmail.com">Oleg Kalachev</maintainer>
   <license>MIT</license>
@@ -11,21 +11,30 @@
   <author email="okalachev@gmail.com">Oleg Kalachev</author>
   <author email="urpylka@gmail.com">Artem Smirnov</author>
 
-  <!-- Use build_depend for packages you need at compile time: -->
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>tf</build_depend>
+  <build_depend>tf2</build_depend>
+  <build_depend>tf2_ros</build_depend>
+  <build_depend>tf2_geometry_msgs</build_depend>
+  <build_depend>cv_bridge</build_depend>
+  <build_depend>dynamic_reconfigure</build_depend>
+  <build_depend>image_transport</build_depend>
+  <build_depend>message_generation</build_depend>
+  <build_depend>message_runtime</build_depend>
   <build_depend>nodelet</build_depend>
   <build_depend>roscpp</build_depend>
-  <build_depend>image_transport</build_depend>
-  <build_depend>cv_bridge</build_depend>
-  <build_depend>tf</build_depend>
-  <!-- Use buildtool_depend for build tool packages: -->
-  <buildtool_depend>catkin</buildtool_depend>
-  <!-- Use run_depend for packages you need at runtime: -->
+  <build_depend>std_msgs</build_depend>
   <run_depend>nodelet</run_depend>
   <run_depend>roscpp</run_depend>
   <run_depend>image_transport</run_depend>
   <run_depend>cv_bridge</run_depend>
-  <!-- Use test_depend for packages you need only for testing: -->
-  <!--   <test_depend>gtest</test_depend> -->
+  <run_depend>message_runtime</run_depend>
+  <run_depend>std_msgs</run_depend>
+  <run_depend>tf</run_depend>
+  <run_depend>tf2</run_depend>
+  <run_depend>tf2_ros</run_depend>
+  <run_depend>tf2_geometry_msgs</run_depend>
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>

aruco_pose/src/aruco_detect.cpp

@@ -0,0 +1,293 @@
+/*
+ * Detecting and positioning ArUco markers
+ * Copyright (C) 2018 Copter Express Technologies
+ *
+ * Author: Oleg Kalachev <okalachev@gmail.com>
+ *
+ * Distributed under MIT License (available at https://opensource.org/licenses/MIT).
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ */
+
+/*
+ * Code is based on https://github.com/UbiquityRobotics/fiducials, which is distributed
+ * under the BSD license.
+ */
+
+#include <math.h>
+#include <vector>
+#include <string>
+#include <ros/ros.h>
+#include <nodelet/nodelet.h>
+#include <pluginlib/class_list_macros.h>
+#include <tf/transform_datatypes.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <image_transport/image_transport.h>
+#include <cv_bridge/cv_bridge.h>
+#include <geometry_msgs/Vector3.h>
+#include <geometry_msgs/Pose.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
+#include <geometry_msgs/TransformStamped.h>
+#include <visualization_msgs/Marker.h>
+#include <visualization_msgs/MarkerArray.h>
+
+#include <opencv2/opencv.hpp>
+#include <opencv2/highgui.hpp>
+#include <opencv2/aruco.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/calib3d/calib3d.hpp>
+
+#include <aruco_pose/Marker.h>
+#include <aruco_pose/MarkerArray.h>
+
+#include "utils.h"
+
+using std::vector;
+using cv::Mat;
+
+class ArucoDetect : public nodelet::Nodelet {
+private:
+	ros::NodeHandle nh_, nh_priv_;
+	tf2_ros::TransformBroadcaster br_;
+	tf2_ros::Buffer tf_buffer_;
+	tf2_ros::TransformListener tf_listener_{tf_buffer_};
+	cv::Ptr<cv::aruco::Dictionary> dictionary_;
+	cv::Ptr<cv::aruco::DetectorParameters> parameters_;
+	image_transport::Publisher debug_pub_;
+	image_transport::CameraSubscriber img_sub_;
+	ros::Publisher markers_pub_, vis_markers_pub_;
+	bool estimate_poses_, send_tf_;
+	double length_;
+	std::string snap_orientation_;
+	Mat camera_matrix_, dist_coeffs_;
+	aruco_pose::MarkerArray array_;
+	visualization_msgs::MarkerArray vis_array_;
+
+public:
+	virtual void onInit()
+	{
+		nh_ = getNodeHandle();
+		nh_priv_ = getPrivateNodeHandle();
+
+		int dictionary;
+		nh_priv_.param("dictionary", dictionary, 2);
+		nh_priv_.param("estimate_poses", estimate_poses_, true);
+		nh_priv_.param("send_tf", send_tf_, true);
+		if (estimate_poses_ && !nh_priv_.getParam("length", length_)) {
+			ROS_FATAL("aruco_detect: can't estimate marker's poses as ~length parameter is not defined");
+			ros::shutdown();
+		}
+		nh_priv_.param<std::string>("snap_orientation", snap_orientation_, "");
+
+		camera_matrix_ = cv::Mat::zeros(3, 3, CV_64F);
+		dist_coeffs_ = cv::Mat::zeros(8, 1, CV_64F);
+
+		dictionary_ = cv::aruco::getPredefinedDictionary(static_cast<cv::aruco::PREDEFINED_DICTIONARY_NAME>(dictionary));
+		parameters_ = cv::aruco::DetectorParameters::create();
+		parameters_->cornerRefinementMethod = cv::aruco::CORNER_REFINE_SUBPIX;
+
+		image_transport::ImageTransport it(nh_);
+		image_transport::ImageTransport it_priv(nh_priv_);
+
+		debug_pub_ = it_priv.advertise("debug", 1);
+		markers_pub_ = nh_priv_.advertise<aruco_pose::MarkerArray>("markers", 1);
+		vis_markers_pub_ = nh_priv_.advertise<visualization_msgs::MarkerArray>("visualization", 1);
+		img_sub_ = it.subscribeCamera("image_raw", 1, &ArucoDetect::imageCallback, this);
+
+		ROS_INFO("aruco_detect: ready");
+	}
+
+private:
+	void imageCallback(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr &cinfo)
+	{
+		Mat image = cv_bridge::toCvShare(msg, "bgr8")->image;
+
+		vector<int> ids;
+		vector<vector<cv::Point2f>> corners, rejected;
+		vector<cv::Vec3d> rvecs, tvecs;
+		vector<cv::Point3f> obj_points;
+		geometry_msgs::TransformStamped snap_to;
+
+		// Detect markers
+		cv::aruco::detectMarkers(image, dictionary_, corners, ids, parameters_, rejected);
+
+		array_.header.stamp = msg->header.stamp;
+		array_.header.frame_id = msg->header.frame_id;
+		array_.markers.clear();
+
+		if (ids.size() != 0) {
+			parseCameraInfo(cinfo, camera_matrix_, dist_coeffs_);
+
+			// Estimate individual markers' poses
+			if (estimate_poses_) {
+				cv::aruco::estimatePoseSingleMarkers(corners, length_, camera_matrix_, dist_coeffs_,
+				                                     rvecs, tvecs);
+
+				if (!snap_orientation_.empty()) {
+					try {
+						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());
+					}
+				}
+			}
+
+			array_.markers.reserve(ids.size());
+			aruco_pose::Marker marker;
+			geometry_msgs::TransformStamped transform;
+			transform.header.stamp = msg->header.stamp;
+			transform.header.frame_id = msg->header.frame_id;
+
+			for (unsigned int i = 0; i < ids.size(); i++) {
+				marker.id = ids[i];
+				fillCorners(marker, corners[i]);
+
+				if (estimate_poses_) {
+					fillPose(marker.pose.pose, rvecs[i], tvecs[i]);
+
+					// snap orientation (if enabled and snap frame avaiable)
+					if (!snap_orientation_.empty() && !snap_to.header.frame_id.empty()) {
+						snapOrientation(marker.pose.pose, snap_to.transform.rotation);
+					}
+
+					// TODO: check IDs are unique
+					if (send_tf_) {
+						transform.child_frame_id = getChildFrameId(ids[i]);
+						transform.transform.rotation = marker.pose.pose.orientation;
+						fillTranslation(transform.transform.translation, tvecs[i]);
+						br_.sendTransform(transform);
+					}
+				}
+				array_.markers.push_back(marker);
+			}
+		}
+
+		markers_pub_.publish(array_);
+
+		// Publish visualization markers
+		if (estimate_poses_ && vis_markers_pub_.getNumSubscribers() != 0) {
+			// Delete all markers
+			visualization_msgs::Marker vis_marker;
+			vis_marker.action = visualization_msgs::Marker::DELETEALL;
+			vis_array_.markers.clear();
+			vis_array_.markers.reserve(ids.size() + 1);
+			vis_array_.markers.push_back(vis_marker);
+
+			for (unsigned int i = 0; i < ids.size(); i++)
+				pushVisMarkers(msg->header.frame_id, msg->header.stamp, array_.markers[i].pose.pose,
+				               length_, ids[i], i);
+
+			vis_markers_pub_.publish(vis_array_);
+		}
+
+		// Publish debug image
+		if (debug_pub_.getNumSubscribers() != 0) {
+			Mat debug = image.clone();
+			cv::aruco::drawDetectedMarkers(debug, corners, ids); // draw markers
+			if (estimate_poses_)
+				for (unsigned int i = 0; i < ids.size(); i++)
+					cv::aruco::drawAxis(debug, camera_matrix_, dist_coeffs_, rvecs[i], tvecs[i], length_);
+
+			cv_bridge::CvImage out_msg;
+			out_msg.header.frame_id = msg->header.frame_id;
+			out_msg.header.stamp = msg->header.stamp;
+			out_msg.encoding = sensor_msgs::image_encodings::BGR8;
+			out_msg.image = debug;
+			debug_pub_.publish(out_msg.toImageMsg());
+		}
+	}
+
+	inline void fillCorners(aruco_pose::Marker& marker, const vector<cv::Point2f>& corners) const
+	{
+		marker.c1.x = corners[0].x;
+		marker.c2.x = corners[1].x;
+		marker.c3.x = corners[2].x;
+		marker.c4.x = corners[3].x;
+		marker.c1.y = corners[0].y;
+		marker.c2.y = corners[1].y;
+		marker.c3.y = corners[2].y;
+		marker.c4.y = corners[3].y;
+	}
+
+	inline void fillPose(geometry_msgs::Pose& pose, const cv::Vec3d& rvec, const cv::Vec3d& tvec) const
+	{
+		pose.position.x = tvec[0];
+		pose.position.y = tvec[1];
+		pose.position.z = tvec[2];
+
+		double angle = norm(rvec);
+		cv::Vec3d axis = rvec / angle;
+
+		tf2::Quaternion q;
+		q.setRotation(tf2::Vector3(axis[0], axis[1], axis[2]), angle);
+
+		pose.orientation.w = q.w();
+		pose.orientation.x = q.x();
+		pose.orientation.y = q.y();
+		pose.orientation.z = q.z();
+	}
+
+	inline void fillTranslation(geometry_msgs::Vector3& translation, const cv::Vec3d& tvec) const
+	{
+		translation.x = tvec[0];
+		translation.y = tvec[1];
+		translation.z = tvec[2];
+	}
+
+	void pushVisMarkers(const std::string& frame_id, const ros::Time& stamp,
+	                    const geometry_msgs::Pose &pose, double length, int id, int index)
+	{
+		visualization_msgs::Marker marker;
+		marker.header.frame_id = frame_id;
+		marker.header.stamp = stamp;
+		marker.action = visualization_msgs::Marker::ADD;
+		marker.id = index;
+
+		// Marker
+		marker.ns = "aruco_marker";
+		marker.type = visualization_msgs::Marker::CUBE;
+		marker.scale.x = length;
+		marker.scale.y = length;
+		marker.scale.z = 0.001;
+		marker.color.r = 1;
+		marker.color.g = 1;
+		marker.color.b = 1;
+		marker.color.a = 0.9;
+		marker.pose = pose;
+		vis_array_.markers.push_back(marker);
+
+		// Label
+		marker.ns = "aruco_marker_label";
+		marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
+		marker.scale.z = length * 0.6;
+		marker.color.r = 0;
+		marker.color.g = 0;
+		marker.color.b = 0;
+		marker.color.a = 1;
+		marker.text = std::to_string(id);
+		marker.pose = pose;
+		vis_array_.markers.push_back(marker);
+	}
+
+	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 "aruco_" + std::to_string(id);
+	}
+};
+
+PLUGINLIB_EXPORT_CLASS(ArucoDetect, nodelet::Nodelet)

aruco_pose/src/aruco_map.cpp

@@ -0,0 +1,307 @@
+/*
+ * Positioning ArUco markers maps
+ * Copyright (C) 2018 Copter Express Technologies
+ *
+ * Author: Oleg Kalachev <okalachev@gmail.com>
+ *
+ * Distributed under MIT License (available at https://opensource.org/licenses/MIT).
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ */
+
+/*
+ * Code is based on https://github.com/UbiquityRobotics/fiducials, which is distributed
+ * under the BSD license.
+ */
+
+#include <math.h>
+#include <string>
+#include <vector>
+#include <fstream>
+#include <ros/ros.h>
+#include <nodelet/nodelet.h>
+#include <pluginlib/class_list_macros.h>
+#include <image_transport/image_transport.h>
+#include <cv_bridge/cv_bridge.h>
+#include <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 <geometry_msgs/TransformStamped.h>
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
+#include <sensor_msgs/Image.h>
+#include <visualization_msgs/Marker.h>
+#include <visualization_msgs/MarkerArray.h>
+
+#include <aruco_pose/MarkerArray.h>
+#include <aruco_pose/Marker.h>
+
+#include <opencv2/opencv.hpp>
+#include <opencv2/aruco.hpp>
+
+#include "utils.h"
+#include "gridboard.h"
+
+using std::vector;
+using cv::Mat;
+
+class ArucoMap : public nodelet::Nodelet {
+private:
+	ros::NodeHandle nh_, nh_priv_;
+	ros::Publisher img_pub_, pose_pub_;
+	ros::Subscriber markers_sub_, cinfo_sub;
+	cv::Ptr<cv::aruco::Board> board_;
+	Mat camera_matrix_, dist_coeffs_;
+	geometry_msgs::TransformStamped transform_;
+	geometry_msgs::PoseWithCovarianceStamped pose_;
+	tf2_ros::TransformBroadcaster br_;
+	tf2_ros::Buffer tf_buffer_;
+	tf2_ros::TransformListener tf_listener_{tf_buffer_};
+	visualization_msgs::MarkerArray vis_markers;
+	std::string snap_orientation_;
+	bool has_camera_info_ = false;
+
+public:
+	virtual void onInit()
+	{
+		nh_ = getNodeHandle();
+		nh_priv_ = getPrivateNodeHandle();
+
+		image_transport::ImageTransport it_priv(nh_priv_);
+
+		// TODO: why image_transport doesn't work here?
+		img_pub_ = nh_priv_.advertise<sensor_msgs::Image>("image", 1, true);
+
+		board_ = cv::makePtr<cv::aruco::Board>();
+		board_->dictionary = cv::aruco::getPredefinedDictionary(
+			                 static_cast<cv::aruco::PREDEFINED_DICTIONARY_NAME>(nh_priv_.param("dictionary", 2)));
+		camera_matrix_ = cv::Mat::zeros(3, 3, CV_64F);
+		dist_coeffs_ = cv::Mat::zeros(8, 1, CV_64F);
+
+		std::string type, map, map_name;
+		nh_priv_.param<std::string>("type", type, "map");
+
+		if (type == "map") {
+			param(nh_priv_, "map", map);
+			loadMap(map);
+		} else if (type == "gridboard") {
+			createGridBoard();
+		} else {
+			ROS_FATAL("aruco_map: unknown type: %s", type.c_str());
+			ros::shutdown();
+		}
+
+		nh_priv_.param<std::string>("name", map_name, "map");
+		nh_priv_.param<std::string>("frame_id", transform_.child_frame_id, "aruco_map");
+		nh_priv_.param<std::string>("snap_orientation", snap_orientation_, "");
+
+		pose_pub_ = nh_priv_.advertise<geometry_msgs::PoseWithCovarianceStamped>("pose", 1);
+
+		// TODO: use synchronised subscriber
+		markers_sub_ = nh_.subscribe("markers", 1, &ArucoMap::markersCallback, this);
+		cinfo_sub = nh_.subscribe("camera_info", 1, &ArucoMap::cinfoCallback, this);
+
+		publishMapImage();
+		ROS_INFO("aruco_map: ready");
+	}
+
+	void markersCallback(const aruco_pose::MarkerArray& markers)
+	{
+		if (!has_camera_info_) return;
+		if (markers.markers.empty()) return;
+
+		int count = markers.markers.size();
+		std::vector<int> ids;
+		std::vector<std::vector<cv::Point2f>> corners;
+		ids.reserve(count);
+		corners.reserve(count);
+
+		for(auto const &marker : markers.markers) {
+			ids.push_back(marker.id);
+			std::vector<cv::Point2f> marker_corners = {
+				cv::Point2f(marker.c1.x, marker.c1.y),
+				cv::Point2f(marker.c2.x, marker.c2.y),
+				cv::Point2f(marker.c3.x, marker.c3.y),
+				cv::Point2f(marker.c4.x, marker.c4.y)
+			};
+			corners.push_back(marker_corners);
+		}
+
+		Mat obj_points, img_points;
+		cv::Vec3d rvec, tvec;
+
+		if (snap_orientation_.empty()) {
+			// simple estimation
+			int valid = cv::aruco::estimatePoseBoard(corners, ids, board_, camera_matrix_, dist_coeffs_,
+		                                             rvec, tvec, false);
+			if (!valid) return;
+
+			transform_.header.stamp = markers.header.stamp;
+			transform_.header.frame_id = markers.header.frame_id;
+			pose_.header = transform_.header;
+			fillPose(pose_.pose.pose, rvec, tvec);
+			fillTransform(transform_.transform, rvec, tvec);
+
+		} else {
+			// estimation with "snapping"
+			cv::aruco::getBoardObjectAndImagePoints(board_, corners, ids, obj_points, img_points);
+			if (obj_points.empty()) return;
+
+			double center_x = 0, center_y = 0;
+			alignObjPointsToCenter(obj_points, center_x, center_y);
+
+			int res = solvePnP(obj_points, img_points, camera_matrix_, dist_coeffs_, rvec, tvec, false);
+			if (!res) return;
+
+			fillTransform(transform_.transform, rvec, tvec);
+			try {
+				geometry_msgs::TransformStamped snap_to = tf_buffer_.lookupTransform(markers.header.frame_id,
+				                                          snap_orientation_, markers.header.stamp, ros::Duration(0.02));
+				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());
+			}
+
+			geometry_msgs::TransformStamped shift;
+			shift.transform.translation.x = -center_x;
+			shift.transform.translation.y = -center_y;
+			shift.transform.rotation.w = 1;
+			tf2::doTransform(shift, transform_, transform_);
+
+			transform_.header.stamp = markers.header.stamp;
+			transform_.header.frame_id = markers.header.frame_id;
+			pose_.header = transform_.header;
+			transformToPose(transform_.transform, pose_.pose.pose);
+		}
+
+		br_.sendTransform(transform_);
+		pose_pub_.publish(pose_);
+	}
+
+	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;
+
+		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);
+		}
+
+		center_x = sum_x / obj_points.rows;
+		center_y = sum_y / obj_points.rows;
+
+		for (int i = 0; i < obj_points.rows; i++) 		{
+			obj_points.at<float>(i, 0) -= center_x;
+			obj_points.at<float>(i, 1) -= center_y;
+		}
+	}
+
+	void loadMap(std::string filename)
+	{
+		std::ifstream f(filename);
+		std::string line;
+
+		if (!f.good()) {
+			ROS_FATAL("aruco_map: %s - %s", strerror(errno), filename.c_str());
+			ros::shutdown();
+		}
+
+		while (std::getline(f, line)) {
+			int id;
+			double length, x, y, z, yaw, pitch, roll;
+
+			std::istringstream s(line);
+			ROS_INFO("aruco_map: parse line: %s", line.c_str());
+
+			if (!(s >> id >> length >> x >> y >> z >> yaw >> pitch >> roll)) {
+				ROS_ERROR("aruco_map: cannot parse line: %s", line.c_str());
+				continue;
+			}
+			addMarker(id, length, x, y, z, yaw, pitch, roll);
+		}
+
+		ROS_INFO("aruco_map: loading %s complete (%d markers)", filename.c_str(), static_cast<int>(board_->ids.size()));
+	}
+
+	void createGridBoard()
+	{
+		ROS_INFO("aruco_map: generate gridboard");
+		ROS_WARN("aruco_map: gridboard maps are deprecated");
+
+		int markers_x, markers_y, first_marker;
+		double markers_side, markers_sep_x, markers_sep_y;
+		std::vector<int> marker_ids;
+		nh_priv_.param<int>("markers_x", markers_x, 10);
+		nh_priv_.param<int>("markers_y", markers_y, 10);
+		nh_priv_.param<int>("first_marker", first_marker, 0);
+
+		param(nh_priv_, "markers_side", markers_side);
+		param(nh_priv_, "markers_sep_x", markers_sep_x);
+		param(nh_priv_, "markers_sep_y", markers_sep_y);
+
+		if (nh_priv_.getParam("marker_ids", marker_ids)) {
+			if ((unsigned int)(markers_x * markers_y) != marker_ids.size()) {
+				ROS_FATAL("~marker_ids length should be equal to ~markers_x * ~markers_y");
+				exit(1);
+			}
+		} else {
+			// Fill marker_ids automatically
+			marker_ids.resize(markers_x * markers_y);
+			for (int i = 0; i < markers_x * markers_y; i++)
+			{
+				marker_ids.at(i) = first_marker++;
+			}
+		}
+
+		createCustomGridBoard(board_, markers_x, markers_y, markers_side, markers_sep_x, markers_sep_y, marker_ids);
+	}
+
+	void addMarker(int id, double length, double x, double y, double z,
+				   double yaw, double pitch, double roll)
+	{
+		// 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::quaternionTFToMsg(q, t.transform.rotation);
+
+		// TODO: process roll pitch yaw
+		vector<cv::Point3f> obj_points(4);
+		setMarkerObjectPoints(x, y, z, yaw, length, obj_points);
+		board_->ids.push_back(id);
+		board_->objPoints.push_back(obj_points);
+	}
+
+	void setMarkerObjectPoints(float x, float y, float z, float yaw, float length,
+	                           vector<cv::Point3f>& obj_points)
+	{
+		obj_points[0] = cv::Point3f(x - length / 2, y + length / 2, 0);
+		obj_points[1] = obj_points[0] + cv::Point3f(length, 0, 0);
+		obj_points[2] = obj_points[0] + cv::Point3f(length, -length, 0);
+		obj_points[3] = obj_points[0] + cv::Point3f(0, -length, 0);
+	}
+
+	void publishMapImage()
+	{
+		cv::Mat image;
+		cv_bridge::CvImage msg;
+		cv::aruco::drawPlanarBoard(board_, cv::Size(2000, 2000), image, 50, 1);
+		cv::cvtColor(image, image, CV_GRAY2BGR);
+		msg.encoding = sensor_msgs::image_encodings::BGR8;
+		msg.image = image;
+		img_pub_.publish(msg.toImageMsg());
+	}
+};
+
+PLUGINLIB_EXPORT_CLASS(ArucoMap, nodelet::Nodelet)

aruco_pose/src/aruco_pose.cpp

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

aruco_pose/src/fix.cpp

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

aruco_pose/src/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/util.h

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

aruco_pose/src/utils.h

@@ -0,0 +1,109 @@
+#pragma once
+
+#include <ros/ros.h>
+#include <tf/transform_datatypes.h>
+#include <geometry_msgs/Quaternion.h>
+#include <geometry_msgs/Pose.h>
+#include <geometry_msgs/Vector3.h>
+#include <sensor_msgs/CameraInfo.h>
+#include <opencv2/opencv.hpp>
+
+// Read required param or shutdown the node
+template<typename T>
+static void param(ros::NodeHandle nh, const std::string& param_name, T& param_val)
+{
+	if (!nh.getParam(param_name, param_val)) {
+		ROS_FATAL("Required param %s is not defined", param_name.c_str());
+		ros::shutdown();
+	}
+}
+
+static void parseCameraInfo(const sensor_msgs::CameraInfoConstPtr& cinfo,
+                     cv::Mat& matrix, cv::Mat& dist)
+{
+	for (unsigned int i = 0; i < 3; ++i)
+		for (unsigned int j = 0; j < 3; ++j)
+			matrix.at<double>(i, j) = cinfo->K[3 * i + j];
+
+	for (unsigned int k = 0; k < cinfo->D.size(); k++)
+		dist.at<double>(k) = cinfo->D[k];
+}
+
+inline void rotatePoint(cv::Point3f& p, cv::Point3f origin, float angle)
+{
+	float s = sin(angle);
+	float c = cos(angle);
+
+	// translate point back to origin:
+	p.x -= origin.x;
+	p.y -= origin.y;
+
+	// rotate point
+	float xnew = p.x * c - p.y * s;
+	float ynew = p.x * s + p.y * c;
+
+	// translate point back:
+	p.x = xnew + origin.x;
+	p.y = ynew + origin.y;
+}
+
+inline void fillPose(geometry_msgs::Pose& pose, const cv::Vec3d& rvec, const cv::Vec3d& tvec)
+{
+	pose.position.x = tvec[0];
+	pose.position.y = tvec[1];
+	pose.position.z = tvec[2];
+
+	double angle = norm(rvec);
+	cv::Vec3d axis = rvec / angle;
+
+	tf2::Quaternion q;
+	q.setRotation(tf2::Vector3(axis[0], axis[1], axis[2]), angle);
+
+	pose.orientation.w = q.w();
+	pose.orientation.x = q.x();
+	pose.orientation.y = q.y();
+	pose.orientation.z = q.z();
+}
+
+inline void fillTransform(geometry_msgs::Transform& transform, const cv::Vec3d& rvec, const cv::Vec3d& tvec)
+{
+	transform.translation.x = tvec[0];
+	transform.translation.y = tvec[1];
+	transform.translation.z = tvec[2];
+
+	double angle = norm(rvec);
+	cv::Vec3d axis = rvec / angle;
+
+	tf2::Quaternion q;
+	q.setRotation(tf2::Vector3(axis[0], axis[1], axis[2]), angle);
+
+	transform.rotation.w = q.w();
+	transform.rotation.x = q.x();
+	transform.rotation.y = q.y();
+	transform.rotation.z = q.z();
+}
+
+inline void fillTranslation(geometry_msgs::Vector3& translation, const cv::Vec3d& tvec)
+{
+	translation.x = tvec[0];
+	translation.y = tvec[1];
+	translation.z = tvec[2];
+}
+
+inline void snapOrientation(geometry_msgs::Quaternion& to, const geometry_msgs::Quaternion& from)
+{
+	tf::Quaternion q;
+	q.setRPY(0, 0, -tf::getYaw(to) + tf::getYaw(from));
+	tf::Quaternion pq;
+	tf::quaternionMsgToTF(from, pq);
+	pq = pq * q;
+	tf::quaternionTFToMsg(pq, to);
+}
+
+inline void transformToPose(const geometry_msgs::Transform& transform, geometry_msgs::Pose& pose)
+{
+	pose.position.x = transform.translation.x;
+	pose.position.y = transform.translation.y;
+	pose.position.z = transform.translation.z;
+	pose.orientation = transform.rotation;
+}

book.json

@@ -4,21 +4,19 @@
     "author": "Copter Express",
     "language": "ru",
     "root": "docs/",
-    "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"
-    ],
+    "plugins": ["youtube", "richquotes", "versions", "yametrika"],
     "pluginsConfig": {
+        "disqus": {
+            "shortName": "coex-clever"
+        },
+        "versions": {
+            "type": "tags"
+        },
         "yametrika": {
             "id": 49359238
-        },
-        "bulk-redirect": {
-            "basepath": "/",
-            "redirectsFile": "redirects.json"
         }
+    },
+    "structure": {
+        "glossary": "_GLOSSARY.md"
     }
 }

builder/assets/index.html

@@ -4,7 +4,6 @@
 	<!-- <li><a href="">View user reference</a> (<a href="http://clever.copterexpress.com">http://clever.copterexpress.com</a> snapshot)</li> -->
 	<li><a href="" id="wvs">View image topics</a> (<code>web_video_server</code>)</li>
 	<li><a href="" id="butterfly">Open web terminal</a> (<code>Butterfly</code>)</li>
-	<li><a href="/docs">Documentation</a> (<code>gitbook</code>)</li>
 	<!-- <li><a href="viz.html">View 3D visualization</a> (<code>ros3djs</code>)</li> -->
 </ul>
 

builder/assets/kinetic-rosdep-clever.yaml

@@ -526,6 +526,3 @@ theora_image_transport:
 libogg:
   debian:
     stretch: [libtheora0=1.1.1+dfsg.1-14]
-vl53l1x:
-  debian:
-    stretch: [ros-kinetic-vl53l1x]

builder/image-build.sh

@@ -84,15 +84,6 @@ ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} exec ${SCRIPTS_DIR}'/image-init.sh'
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/monkey-clever' '/root/'
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/index.html' '/usr/share/monkey-static/'
 
-# Gitbook
-apt-get install -y curl gnupg
-curl -sL https://deb.nodesource.com/setup_11.x | bash -
-apt-get install -y nodejs
-npm install gitbook-cli -g
-gitbook build ${REPO_DIR}'/docs'
-${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${REPO_DIR}'/docs/_book/' '/usr/share/monkey-static/docs/'
-
-
 # Butterfly
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/butterfly.service' '/lib/systemd/system/'
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/butterfly.socket' '/lib/systemd/system/'

builder/image-ros.sh

@@ -92,7 +92,7 @@ if [ "${INSTALL_ROS_PACK_SOURCES}" = "true" ]; then
     echo_stamp "Preparing other ROS-packages to kinetic-custom_ros.rosinstall" \
     && cd /home/pi/ros_catkin_ws \
     && rosinstall_generator \
-    actionlib actionlib_msgs angles async_web_server_cpp bond bond_core bondcpp bondpy camera_calibration_parsers camera_info_manager catkin class_loader cmake_modules cpp_common cv_bridge cv_camera diagnostic_msgs diagnostic_updater dynamic_reconfigure eigen_conversions gencpp geneus genlisp genmsg gennodejs genpy geographic_msgs geometry_msgs geometry2 image_transport compressed_image_transport libmavconn mavlink mavros_msgs message_filters message_generation message_runtime mk nav_msgs nodelet orocos_kdl pluginlib python_orocos_kdl ros ros_comm rosapi rosauth rosbag rosbag_migration_rule rosbag_storage rosbash rosboost_cfg rosbridge_library rosbridge_server rosbridge_suite rosbuild rosclean rosconsole rosconsole_bridge roscpp roscpp_serialization roscpp_traits roscreate rosgraph rosgraph_msgs roslang roslaunch roslib roslint roslisp roslz4 rosmake rosmaster rosmsg rosnode rosout rospack rosparam rospy rospy_tutorials rosserial rosserial_client rosserial_msgs rosserial_python rosservice rostest rostime rostopic rosunit roswtf sensor_msgs smclib std_msgs std_srvs stereo_msgs tf tf2 tf2_bullet tf2_eigen tf2_geometry_msgs tf2_kdl tf2_msgs tf2_py tf2_ros tf2_sensor_msgs tf2_tools topic_tools trajectory_msgs urdf urdf_parser_plugin usb_cam uuid_msgs visualization_msgs web_video_server xmlrpcpp mavros opencv3 mavros_extras interactive_markers tf2_web_republisher interactive_marker_proxy vl53l1x \
+    actionlib actionlib_msgs angles async_web_server_cpp bond bond_core bondcpp bondpy camera_calibration_parsers camera_info_manager catkin class_loader cmake_modules cpp_common cv_bridge cv_camera diagnostic_msgs diagnostic_updater dynamic_reconfigure eigen_conversions gencpp geneus genlisp genmsg gennodejs genpy geographic_msgs geometry_msgs geometry2 image_transport compressed_image_transport libmavconn mavlink mavros_msgs message_filters message_generation message_runtime mk nav_msgs nodelet orocos_kdl pluginlib python_orocos_kdl ros ros_comm rosapi rosauth rosbag rosbag_migration_rule rosbag_storage rosbash rosboost_cfg rosbridge_library rosbridge_server rosbridge_suite rosbuild rosclean rosconsole rosconsole_bridge roscpp roscpp_serialization roscpp_traits roscreate rosgraph rosgraph_msgs roslang roslaunch roslib roslint roslisp roslz4 rosmake rosmaster rosmsg rosnode rosout rospack rosparam rospy rospy_tutorials rosserial rosserial_client rosserial_msgs rosserial_python rosservice rostest rostime rostopic rosunit roswtf sensor_msgs smclib std_msgs std_srvs stereo_msgs tf tf2 tf2_bullet tf2_eigen tf2_geometry_msgs tf2_kdl tf2_msgs tf2_py tf2_ros tf2_sensor_msgs tf2_tools topic_tools trajectory_msgs urdf urdf_parser_plugin usb_cam uuid_msgs visualization_msgs web_video_server xmlrpcpp mavros opencv3 mavros_extras interactive_markers tf2_web_republisher interactive_marker_proxy \
     --rosdistro kinetic --deps --wet-only --tar > kinetic-custom_ros.rosinstall \
     && wstool merge -j${NUMBER_THREADS} -t src kinetic-custom_ros.rosinstall \
     && wstool update -j${NUMBER_THREADS} -t src \
@@ -148,15 +148,6 @@ echo_stamp "Installing CLEVER" \
 && echo_stamp "All CLEVER was installed!" "SUCCESS" \
 || (echo_stamp "CLEVER installation was failed!" "ERROR"; exit 1)
 
-echo_stamp "Installing additional ROS packages"
-apt-get install -y --no-install-recommends \
-    ros-kinetic-dynamic-reconfigure \
-    ros-kinetic-compressed-image-transport \
-    ros-kinetic-rosbridge-suite \
-    ros-kinetic-rosserial \
-    ros-kinetic-usb-cam \
-    ros-kinetic-vl53l1x
-
 # TODO move GeographicLib datasets to Mavros debian package
 echo_stamp "Install GeographicLib datasets (needs for mavros)" \
 && wget -qO- https://raw.githubusercontent.com/mavlink/mavros/master/mavros/scripts/install_geographiclib_datasets.sh | bash

builder/image-software.sh

@@ -58,7 +58,7 @@ echo_stamp "Install apt keys & repos"
 # TODO: This STDOUT consist 'OK'
 curl http://repo.coex.space/aptly_repo_signing.key 2> /dev/null | apt-key add -
 apt-get update \
-&& apt-get install --no-install-recommends -y -qq dirmngr=2.1.18-8~deb9u3 > /dev/null \
+&& apt-get install --no-install-recommends -y -qq dirmngr=2.1.18-8~deb9u2 > /dev/null \
 && apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key 421C365BD9FF1F717815A3895523BAEEB01FA116
 
 echo "deb http://packages.ros.org/ros/ubuntu stretch main" > /etc/apt/sources.list.d/ros-latest.list
@@ -92,15 +92,13 @@ libjpeg8-dev=8d1-2 \
 tcpdump \
 ltrace \
 libpoco-dev=1.7.6+dfsg1-5+deb9u1 \
-python-rosdep=0.14.0-1 \
+python-rosdep=0.12.2-1 \
 python-rosinstall-generator=0.1.14-1 \
 python-wstool=0.1.17-1 \
 python-rosinstall=0.7.8-1 \
 build-essential=12.3 \
 libffi-dev \
 monkey=1.6.9-1 \
-pigpio python-pigpio python3-pigpio \
-i2c-tools \
 && echo_stamp "Everything was installed!" "SUCCESS" \
 || (echo_stamp "Some packages wasn't installed!" "ERROR"; exit 1)
 
@@ -108,24 +106,15 @@ i2c-tools \
 sed -i "s/updates_available//" /usr/share/byobu/status/status
 # sed -i "s/updates_available//" /home/pi/.byobu/status
 
-#echo_stamp "Upgrade pip"
-#my_travis_retry pip install --upgrade pip
-#my_travis_retry pip3 install --upgrade pip
-
-echo_stamp "Not upgrading system pip due to https://github.com/pypa/pip/issues/5599"
-
-echo_stamp "Make sure both pip and pip3 are installed"
-pip --version
-pip3 --version
+echo_stamp "Upgrade pip"
+my_travis_retry pip install --upgrade pip
+my_travis_retry pip3 install --upgrade pip3
 
 echo_stamp "Install and enable Butterfly (web terminal)"
 my_travis_retry pip3 install butterfly
 my_travis_retry pip3 install butterfly[systemd]
 systemctl enable butterfly.socket
 
-echo_stamp "Install ws281x library"
-my_travis_retry pip install rpi_ws281x
-
 echo_stamp "Setup Monkey"
 mv /etc/monkey/sites/default /etc/monkey/sites/default.orig
 mv /root/monkey-clever /etc/monkey/sites/default
@@ -138,10 +127,4 @@ syntax on
 autocmd BufNewFile,BufRead *.launch set syntax=xml
 EOF
 
-echo_stamp "Attempting to kill dirmngr"
-gpgconf --kill dirmngr
-# dirmngr is only used by apt-key, so we can safely kill it.
-# We ignore killall's exit value as well.
-killall -w -9 dirmngr || true
-
 echo_stamp "End of software installation"

clever/CMakeLists.txt

@@ -26,9 +26,6 @@ find_package(catkin REQUIRED COMPONENTS
   cv_bridge
 )
 
-list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/cmake")
-
-find_package(GeographicLib REQUIRED)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
@@ -77,6 +74,7 @@ add_service_files(
   Navigate.srv
   NavigateGlobal.srv
   SetPosition.srv
+  SetPositionGlobal.srv
   SetVelocity.srv
   SetAttitude.srv
   SetRates.srv
@@ -140,7 +138,6 @@ catkin_package(
 include_directories(
 # include
   ${catkin_INCLUDE_DIRS}
-  ${GeographicLib_INCLUDE_DIRS}
 )
 
 # Declare a C++ library
@@ -148,6 +145,10 @@ add_library(clever
   src/optical_flow.cpp
 )
 
+add_library(fcu_horiz
+  src/fcu_horiz.cpp
+)
+
 add_library(aruco_vpe
   src/aruco_vpe.cpp
 )
@@ -160,18 +161,13 @@ add_library(aruco_vpe
 ## Declare a C++ executable
 ## With catkin_make all packages are built within a single CMake context
 ## The recommended prefix ensures that target names across packages don't collide
-add_executable(simple_offboard src/simple_offboard.cpp)
-
 add_executable(rc src/rc.cpp)
 
 add_executable(camera_markers src/camera_markers.cpp)
 
 add_executable(frames src/frames.cpp)
 
-target_link_libraries(simple_offboard
-  ${catkin_LIBRARIES}
-  ${GeographicLib_LIBRARIES}
-)
+add_executable(vpe_publisher src/vpe_publisher.cpp)
 
 target_link_libraries(rc ${catkin_LIBRARIES})
 
@@ -179,7 +175,7 @@ target_link_libraries(camera_markers ${catkin_LIBRARIES})
 
 target_link_libraries(frames ${catkin_LIBRARIES})
 
-add_dependencies(simple_offboard clever_generate_messages_cpp)
+target_link_libraries(vpe_publisher ${catkin_LIBRARIES})
 
 ## Rename C++ executable without prefix
 ## The above recommended prefix causes long target names, the following renames the
@@ -196,6 +192,11 @@ target_link_libraries(clever
   ${catkin_LIBRARIES}
 )
 
+target_link_libraries(fcu_horiz
+  ${catkin_LIBRARIES}
+  "/opt/ros/kinetic/lib/libtf2_ros.so"
+)
+
 target_link_libraries(aruco_vpe
   ${catkin_LIBRARIES}
 )

clever/cmake/FindGeographicLib.cmake

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

clever/launch/aruco.launch

@@ -1,24 +1,35 @@
 <launch>
-    <remap from="image" to="main_camera/image_raw"/>
-    <remap from="camera_info" to="main_camera/camera_info"/>
+    <arg name="aruco_detect" default="true"/>
+    <arg name="aruco_map" default="false"/>
+    <arg name="aruco_vpe" default="false"/>
 
-    <node pkg="nodelet" type="nodelet" name="aruco_pose" args="load aruco_pose/aruco_pose nodelet_manager" clear_params="true">
-        <param name="frame_id" value="aruco_map_raw"/>
-        <param name="type" value="gridboard"/>
-        <param name="markers_x" value="1"/>
-        <param name="markers_y" value="6"/>
-        <param name="first_marker" value="240"/>
-        <param name="markers_side" value="0.3362"/>
-        <param name="markers_sep" value="0.46"/>
+    <!-- detect aruco markers, estimate poses -->
+    <node pkg="nodelet" if="$(arg aruco_detect)" clear_params="true" type="nodelet" name="aruco_detect" args="load aruco_pose/aruco_detect nodelet_manager" output="screen">
+        <remap from="image_raw" to="main_camera/image_raw"/>
+        <remap from="camera_info" to="main_camera/camera_info"/>
 
-        <!-- Custom gridboard: -->
-        <!--<rosparam param="marker_ids">[6, 5, 4, 3, 2, 1]</rosparam>-->
+        <param name="length" value="0.32"/>
+        <param name="snap_orientation" value="local_origin"/>
+        <param name="estimate_poses" value="true"/>
     </node>
 
-    <node pkg="nodelet" type="nodelet" name="aruco_vpe" args="load clever/aruco_vpe nodelet_manager" clear_params="true">
-        <param name="aruco_orientation" value="map"/>
-        <!--<param name="aruco_orientation" value="map_upside_down"/>-->
+    <!-- estimate aruco map pose -->
+    <node pkg="nodelet" clear_params="true" type="nodelet" name="aruco_map" if="$(arg aruco_map)" args="load aruco_pose/aruco_map nodelet_manager" output="screen">
+        <remap from="markers" to="aruco_detect/markers"/>
+        <remap from="camera_info" to="main_camera/camera_info"/>
+        <param name="type" value="map"/>
+        <param name="map" value="$(find aruco_pose)/map/map.txt"/>
+        <param name="snap_orientation" value="local_origin"/>
+    </node>
 
-        <param name="use_mocap" value="true"/>
+    <!-- vpe publisher from aruco markers -->
+    <node name="vpe_publisher" pkg="clever" type="vpe_publisher" output="screen" if="$(arg aruco_vpe)">
+        <!-- <remap from="~markers" to="aruco_detect/markers"/> -->
+        <!-- <remap from="~pose_pub" to="mavros/mocap/pose"/> --> <!-- publish MOCAP -->
+        <remap from="~pose_cov" to="aruco_map/pose"/>
+        <remap from="~pose_pub" to="mavros/vision_pose/pose"/> <!-- publish VPE -->
+        <param name="frame_id" value="aruco_map"/>
+        <param name="child_frame_id" value="fcu"/>
+        <param name="publish_zero" value="true"/>
     </node>
 </launch>

clever/launch/clever.launch

@@ -8,8 +8,8 @@
     <arg name="optical_flow" default="false"/>
     <arg name="aruco" default="false"/>
     <arg name="rc" default="true"/>
-    <arg name="rangefinder_vl53l1x" default="false"/>
     <arg name="arduino" default="false"/>
+    <arg name="vl53l1x" default="false"/>
 
     <!-- mavros -->
     <include file="$(find clever)/launch/mavros.launch">
@@ -22,7 +22,7 @@
     <!-- web video server -->
     <node name="web_video_server" pkg="web_video_server" type="web_video_server" if="$(arg web_video_server)" required="false" respawn="true" respawn_delay="5"/>
 
-    <!-- aruco vpe -->
+    <!-- aruco -->
     <include file="$(find clever)/launch/aruco.launch" if="$(arg aruco)"/>
 
     <!-- optical flow -->
@@ -33,24 +33,20 @@
 
     <!-- main nodelet manager -->
     <node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager" output="screen" clear_params="true">
-        <param name="num_worker_threads" value="2"/>
+        <param name="num_worker_threads" value="4"/>
     </node>
 
-    <node pkg="tf2_ros" type="static_transform_publisher" name="map_upside_down_frame" args="0 0 0 3.1415926 3.1415926 0 map map_upside_down"/>
+    <node pkg="tf2_ros" type="static_transform_publisher" name="local_origin_upside_down_frame" args="0 0 0 3.1415926 3.1415926 0 local_origin local_origin_upside_down"/>
+
 
     <!-- simplified offboard control -->
-    <node name="simple_offboard" pkg="clever" type="simple_offboard" output="screen" clear_params="true">
-        <rosparam param="reference_frames">
-            body: map
-            base_link: map
-        </rosparam>
-    </node>
-
+    <node name="simple_offboard" pkg="clever" type="simple_offboard.py" output="screen"/>
     <!-- Auxiliary frames -->
     <node name="frames" pkg="clever" type="frames" output="screen">
-        <param name="body/frame_id" value="body"/>
+        <param name="body/frame_id" value="fcu_horiz"/>
     </node>
 
+
     <!-- main camera -->
     <include file="$(find clever)/launch/main_camera.launch" if="$(arg main_camera)"/>
 
@@ -58,10 +54,9 @@
     <include file="$(find rosbridge_server)/launch/rosbridge_websocket.launch" if="$(eval rosbridge or rc)"/>
 
     <!-- vl53l1x ToF rangefinder -->
-    <node name="vl53l1x" pkg="vl53l1x" type="vl53l1x_node" output="screen" if="$(arg rangefinder_vl53l1x)">
-        <param name="frame_id" value="rangefinder"/>
-        <param name="offset" value="-0.05"/>
-        <remap from="~range" to="mavros/distance_sensor/rangefinder_3_sub"/> <!-- redirect data to FCU -->
+    <node name="vl53l1x" pkg="clever" type="vl53l1x.py" output="screen" if="$(arg vl53l1x)">
+        <param name="z_shift" value="-0.05"/>
+        <!-- <remap from="~range" to="mavros/distance_sensor/rangefinder_3_sub"/> -->
     </node>
 
     <!-- rc backend -->

clever/launch/main_camera.launch

@@ -1,20 +1,15 @@
 <launch>
-    <!-- Camera position and orientation are represented by base_link -> main_camera_optical transform -->
+    <!-- Camera position and orientation are represented by fcu -> main_camera_optical transform -->
     <!-- static_transform_publisher arguments: x y z yaw pitch roll frame_id child_frame_id -->
 
-    <!-- article about camera setup: https://clever.copterexpress.com/camera_frame.html -->
+    <!-- clever 2 -->
+    <!--<node pkg="tf2_ros" type="static_transform_publisher" name="main_camera_frame" args="0 0 -0.07 -1.5707963 0 3.1415926 fcu main_camera_optical"/>-->
 
-    <!-- camera is oriented downward, camera cable goes backward [option 1] -->
-    <node pkg="tf2_ros" type="static_transform_publisher" name="main_camera_frame" args="0.05 0 -0.07 -1.5707963 0 3.1415926 base_link main_camera_optical"/>
+    <!-- clever 3 -->
+    <node pkg="tf2_ros" type="static_transform_publisher" name="main_camera_frame" args="0.05 0 -0.07 1.5707963 0 3.1415926 fcu main_camera_optical"/>
 
-    <!-- camera is oriented downward, camera cable goes forward  [option 2] -->
-    <!--<node pkg="tf2_ros" type="static_transform_publisher" name="main_camera_frame" args="0.05 0 -0.07 1.5707963 0 3.1415926 base_link main_camera_optical"/>-->
-
-    <!-- camera is oriented upward, camera cable goes backward   [option 3] -->
-    <!--<node pkg="tf2_ros" type="static_transform_publisher" name="main_camera_frame" args="0.05 0 0.07 1.5707963 0 0 base_link main_camera_optical"/>-->
-
-    <!-- camera is oriented upward, camera cable goes forward    [option 4] -->
-    <!--<node pkg="tf2_ros" type="static_transform_publisher" name="main_camera_frame" args="0.05 0 0.07 -1.5707963 0 0 base_link main_camera_optical"/>-->
+    <!-- clever 3, upwards -->
+    <!--<node pkg="tf2_ros" type="static_transform_publisher" name="main_camera_frame" args="0.05 0 0.07 -1.5707963 0 0 fcu main_camera_optical"/>-->
 
     <!-- camera node -->
     <node pkg="nodelet" type="nodelet" name="main_camera" args="load cv_camera/CvCameraNodelet nodelet_manager" clear_params="true">

clever/launch/mavros.launch

@@ -5,7 +5,7 @@
     <arg name="viz" default="true"/>
     <arg name="respawn" default="true"/>
 
-    <node pkg="mavros" type="mavros_node" name="mavros" required="false" clear_params="true" respawn="$(arg respawn)" respawn_delay="2" output="screen">
+    <node pkg="mavros" type="mavros_node" name="mavros" required="false" clear_params="true" respawn="$(arg respawn)" respawn_delay="5" output="screen">
         <!-- UART connection -->
         <param name="fcu_url" value="/dev/ttyAMA0:921600" if="$(eval fcu_conn is None or fcu_conn == 'uart')"/>
 
@@ -27,9 +27,6 @@
         <!-- default px4 params -->
         <rosparam command="load" file="$(find mavros)/launch/px4_config.yaml"/>
 
-        <!-- enable setpoint_attitude/attitude -->
-        <param name="setpoint_attitude/use_quaternion" value="true"/>
-
         <!-- rangefinders -->
         <rosparam>
             distance_sensor:
@@ -54,12 +51,12 @@
         </rosparam>
 
         <!-- additional params -->
-        <param name="local_position/frame_id" value="map"/>
+        <param name="local_position/frame_id" value="local_origin"/>
         <param name="local_position/tf/send" value="true"/>
-        <param name="local_position/tf/frame_id" value="map"/>
-        <param name="local_position/tf/child_frame_id" value="base_link"/>
+        <param name="local_position/tf/frame_id" value="local_origin"/>
+        <param name="local_position/tf/child_frame_id" value="fcu"/>
         <param name="global_position/tf/send" value="false"/>
-        <param name="imu/frame_id" value="base_link"/>
+        <param name="imu/frame_id" value="fcu"/>
         <rosparam param="plugin_blacklist">
             - safety_area
             - image_pub
@@ -84,14 +81,14 @@
     </node>
 
     <!-- Rangefinders frame -->
-    <node pkg="tf2_ros" type="static_transform_publisher" name="rangefinder_frame" args="0 0 -0.05 0 1.5707963268 0 base_link rangefinder"/>
+    <node pkg="tf2_ros" type="static_transform_publisher" name="rangefinder_frame" args="0 0 -0.05 0 1.5707963268 0 fcu rangefinder"/>
 
     <!-- Copter visualization -->
     <node name="copter_visualization" pkg="mavros_extras" type="copter_visualization" if="$(arg viz)">
         <remap to="mavros/local_position/pose" from="local_position"/>
         <remap to="mavros/setpoint_position/local" from="local_setpoint"/>
-        <param name="fixed_frame_id" value="map"/>
-        <param name="child_frame_id" value="base_link"/>
+        <param name="fixed_frame_id" value="local_origin"/>
+        <param name="child_frame_id" value="fcu"/>
         <param name="marker_scale" value="1"/>
         <param name="max_track_size" value="20"/>
         <param name="num_rotors" value="4"/>

clever/launch/sitl.launch

@@ -8,11 +8,12 @@
         <arg name="fcu_ip" value="$(arg ip)"/>
         <arg name="gcs_bridge" value="false"/>
         <arg name="optical_flow" value="false"/>
+        <arg name="web_server" default="false"/>
         <arg name="web_video_server" default="false"/>
         <arg name="main_camera" default="false"/>
         <arg name="rosbridge" value="$(arg rosbridge)"/>
         <arg name="aruco" default="false"/>
-        <arg name="rangefinder_vl53l1x" default="false"/>
+        <arg name="vl53l1x" default="false"/>
         <arg name="rc" default="false"/>
     </include>
 </launch>

clever/nodelet_plugins.xml

@@ -3,6 +3,11 @@
       <description/>
    </class>
 </library>
+<library path="lib/libfcu_horiz">
+   <class name="clever/fcu_horiz" type="FcuHoriz" base_class_type="nodelet::Nodelet">
+      <description/>
+   </class>
+</library>
 <library path="lib/libaruco_vpe">
    <class name="clever/aruco_vpe" type="ArucoVPE" base_class_type="nodelet::Nodelet">
       <description/>

clever/package.xml

@@ -1,6 +1,5 @@
 <?xml version="1.0"?>
-<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
-<package format="2">
+<package>
   <name>clever</name>
   <version>0.0.1</version>
   <description>The CLEVER package</description>
@@ -12,23 +11,25 @@
   <author email="okalachev@gmail.com">Oleg Kalachev</author>
   <author email="urpylka@gmail.com">Artem Smirnov</author>
 
+  <!-- Use build_depend for packages you need at compile time: -->
+  <build_depend>nodelet</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>visualization_msgs</build_depend>
+  <build_depend>tf2_geometry_msgs</build_depend>
+  <!-- Use buildtool_depend for build tool packages: -->
   <buildtool_depend>catkin</buildtool_depend>
-
-  <!-- Package format specifier version 2.0 allows specifying dependencies for both
-   build- and runtime in a single <depend> element -->
-  <depend>visualization_msgs</depend>
-  <depend>tf2_geometry_msgs</depend>
-  <depend>geographiclib</depend>
-  <depend>roscpp</depend>
-  <depend>nodelet</depend>
-  <depend>mavros</depend>
-  <depend>mavros_extras</depend>
-  <depend>lxml</depend>
-  <depend>cv_camera</depend>
-  <depend>mjpg-streamer</depend>
-  <depend>rosbridge_server</depend>
-  <depend>web_video_server</depend>
-  <depend>ros_comm</depend>
+  <!-- Use run_depend for packages you need at runtime: -->
+  <run_depend>catkin</run_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>nodelet</run_depend>
+  <run_depend>mavros</run_depend>
+  <run_depend>mavros_extras</run_depend>
+  <run_depend>lxml</run_depend>
+  <run_depend>cv_camera</run_depend>
+  <run_depend>mjpg-streamer</run_depend>
+  <run_depend>rosbridge_server</run_depend>
+  <run_depend>web_video_server</run_depend>
+  <run_depend>ros_comm</run_depend>r
   <!-- Use test_depend for packages you need only for testing: -->
   <!--   <test_depend>gtest</test_depend> -->
 

clever/src/aruco_vpe.cpp

@@ -39,7 +39,7 @@ private:
         ros::NodeHandle& nh = getNodeHandle();
         ros::NodeHandle& nh_priv = getPrivateNodeHandle();
 
-        nh_priv.param<string>("aruco_orientation", aruco_orientation_, "map");
+        nh_priv.param<string>("aruco_orientation", aruco_orientation_, "local_origin");
         bool use_mocap;
         nh_priv.param<bool>("use_mocap", use_mocap, false);
         nh_priv.param<bool>("reset_vpe", reset_vpe_, !use_mocap);
@@ -107,20 +107,20 @@ private:
                (reset_vpe_ && (ros::Time::now() - last_published_ > reset_timeout_))) // vpe origin outdated
             {
                 ROS_INFO("Reset VPE");
-                t = tf_buffer.lookupTransform("map", "aruco_map_vision", stamp, lookup_timeout_);
+                t = tf_buffer.lookupTransform("local_origin", "aruco_map_vision", stamp, lookup_timeout_);
                 t.child_frame_id = "aruco_map";
                 static_br.sendTransform(t);
             }
 
             // Calculate VPE
-            ps.header.frame_id = "body";
+            ps.header.frame_id = "fcu_horiz";
             ps.header.stamp = stamp;
             ps.pose.orientation.w = 1;
 
             tf_buffer.transform(ps, vpe_raw, "aruco_map_vision", lookup_timeout_);
 
             vpe_raw.header.frame_id = "aruco_map";
-            tf_buffer.transform(vpe_raw, vpe, "map", lookup_timeout_);
+            tf_buffer.transform(vpe_raw, vpe, "local_origin", lookup_timeout_);
 
             vision_position_pub_.publish(vpe);
 

clever/src/fcu_horiz.cpp

@@ -0,0 +1,40 @@
+#include <nodelet/nodelet.h>
+#include <pluginlib/class_list_macros.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include <geometry_msgs/TransformStamped.h>
+#include <geometry_msgs/PoseStamped.h>
+
+#include "util.h"
+
+class FcuHoriz : public nodelet::Nodelet
+{
+    geometry_msgs::TransformStamped t_;
+
+    void handlePose(const geometry_msgs::PoseStampedConstPtr& msg)
+    {
+        static tf2_ros::TransformBroadcaster br;
+        double roll, pitch, yaw;
+
+        t_.header.stamp = msg->header.stamp;
+        t_.header.frame_id = msg->header.frame_id;
+        t_.transform.translation.x = msg->pose.position.x;
+        t_.transform.translation.y = msg->pose.position.y;
+        t_.transform.translation.z = msg->pose.position.z;
+
+        // Warning: this is not thead-safe
+        quaternionToEuler(msg->pose.orientation, roll, pitch, yaw);
+        eulerToQuaternion(t_.transform.rotation, 0, 0, yaw);
+
+        br.sendTransform(t_);
+    }
+
+    void onInit()
+    {
+        t_.child_frame_id = "fcu_horiz";
+        t_.transform.rotation.w = 1;
+        static ros::Subscriber pose_sub = getNodeHandle().subscribe("mavros/local_position/pose", 1, &FcuHoriz::handlePose, this);
+        ROS_INFO("fcu_horiz initialized");
+    }
+};
+
+PLUGINLIB_EXPORT_CLASS(FcuHoriz, nodelet::Nodelet)

clever/src/global_local.py

@@ -0,0 +1,43 @@
+import rospy
+import math
+import geopy
+from geometry_msgs.msg import PoseStamped
+from geopy.distance import VincentyDistance, vincenty
+from sensor_msgs.msg import NavSatFix
+
+
+def global_to_local(lat, lon):
+    # TODO: refactor
+
+    try:
+        position_global = rospy.wait_for_message('mavros/global_position/global', NavSatFix, timeout=0.5)
+    except rospy.exceptions.ROSException:
+        raise Exception('No global position')
+
+    try:
+        pose = rospy.wait_for_message('mavros/local_position/pose', PoseStamped, timeout=0.5)
+    except rospy.exceptions.ROSException:
+        raise Exception('No local position')
+
+    d = math.hypot(pose.pose.position.x, pose.pose.position.y)
+
+    bearing = math.degrees(math.atan2(-pose.pose.position.x, -pose.pose.position.y))
+    if bearing < 0:
+        bearing += 360
+
+    cur = geopy.Point(position_global.latitude, position_global.longitude)
+    origin = VincentyDistance(meters=d).destination(cur, bearing)
+
+    _origin = origin.latitude, origin.longitude
+    olat_tlon = origin.latitude, lon
+    tlat_olon = lat, origin.longitude
+
+    N = vincenty(_origin, tlat_olon)
+    if lat < origin.latitude:
+        N = -N
+
+    E = vincenty(_origin, olat_tlon)
+    if lon < origin.longitude:
+        E = -E
+
+    return E.meters, N.meters

clever/src/interactive.py

@@ -35,7 +35,7 @@ def make_box_control(msg):
 
 def make_quadcopter_marker():
     marker = InteractiveMarker()
-    marker.header.frame_id = 'base_link'
+    marker.header.frame_id = 'fcu'
     marker.header.stamp = rospy.get_rostime()
     marker.scale = 1
     marker.pose.orientation.w = 1

clever/src/optical_flow.cpp

@@ -19,7 +19,6 @@
 #include <tf/transform_datatypes.h>
 #include <tf2/exceptions.h>
 #include <tf2/convert.h>
-#include <tf2/utils.h>
 #include <tf2_ros/transform_listener.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 #include <mavros_msgs/OpticalFlowRad.h>
@@ -42,7 +41,7 @@ public:
 private:
 	ros::Publisher flow_pub_, velo_pub_, shift_pub_;
 	ros::Time prev_stamp_;
-	std::string fcu_frame_id_, local_frame_id_;
+	std::string fcu_frame_id_;
 	image_transport::CameraSubscriber img_sub_;
 	image_transport::Publisher img_pub_;
 	mavros_msgs::OpticalFlowRad flow_;
@@ -52,7 +51,6 @@ private:
 	Mat camera_matrix_, dist_coeffs_;
 	tf2_ros::Buffer tf_buffer_;
 	tf2_ros::TransformListener tf_listener_;
-	bool calc_flow_gyro_;
 
 	void onInit()
 	{
@@ -61,11 +59,9 @@ private:
 		image_transport::ImageTransport it(nh);
 		image_transport::ImageTransport it_priv(nh_priv);
 
-		nh.param<std::string>("mavros/local_position/tf/frame_id", local_frame_id_, "map");
-		nh.param<std::string>("mavros/local_position/tf/child_frame_id", fcu_frame_id_, "base_link");
+		nh_priv.param<std::string>("mavros/local_position/tf/child_frame_id", fcu_frame_id_, "fcu");
 		nh_priv.param("roi", roi_, 128);
 		roi_2_ = roi_ / 2;
-		nh_priv.param("calc_flow_gyro", calc_flow_gyro_, false);
 
 		img_sub_ = it.subscribeCamera("image", 1, &OpticalFlow::flow, this);
 		img_pub_ = it_priv.advertise("debug", 1);
@@ -167,19 +163,6 @@ private:
 			ros::Duration integration_time = msg->header.stamp - prev_stamp_;
 			uint32_t integration_time_us = integration_time.toSec() * 1.0e6;
 
-			if (calc_flow_gyro_) {
-				try {
-					auto flow_gyro_camera = calcFlowGyro(msg->header.frame_id, prev_stamp_, msg->header.stamp);
-					static geometry_msgs::Vector3Stamped flow_gyro_fcu;
-					tf_buffer_.transform(flow_gyro_camera, flow_gyro_fcu, fcu_frame_id_);
-					flow_.integrated_xgyro = flow_gyro_fcu.vector.x;
-					flow_.integrated_ygyro = flow_gyro_fcu.vector.y;
-					flow_.integrated_zgyro = flow_gyro_fcu.vector.z;
-				} catch (const tf2::TransformException& e) {
-					return;
-				}
-			}
-
 			// Publish flow in fcu frame
 			flow_.header.stamp = /*prev_stamp_*/ msg->header.stamp;
 			flow_.integration_time_us = integration_time_us;
@@ -212,23 +195,6 @@ private:
 			prev_stamp_ = msg->header.stamp;
 		}
 	}
-
-	geometry_msgs::Vector3Stamped calcFlowGyro(const std::string& frame_id, const ros::Time& prev, const ros::Time& curr)
-	{
-		tf2::Quaternion prev_rot, curr_rot;
-		tf2::fromMsg(tf_buffer_.lookupTransform(frame_id, local_frame_id_, prev).transform.rotation, prev_rot);
-		tf2::fromMsg(tf_buffer_.lookupTransform(frame_id, local_frame_id_, curr).transform.rotation, curr_rot);
-
-		geometry_msgs::Vector3Stamped flow;
-		flow.header.frame_id = frame_id;
-		flow.header.stamp = curr;
-		auto diff = ((curr_rot - prev_rot) * prev_rot.inverse()) * 2.0f;
-		flow.vector.x = diff.x();
-		flow.vector.y = diff.y();
-		flow.vector.z = diff.z();
-
-		return flow;
-	}
 };
 
 PLUGINLIB_EXPORT_CLASS(OpticalFlow, nodelet::Nodelet)

clever/src/selfcheck.py

@@ -9,14 +9,14 @@ from std_srvs.srv import Trigger
 from sensor_msgs.msg import Image, CameraInfo, NavSatFix, Imu, Range
 from mavros_msgs.msg import State, OpticalFlowRad
 from geometry_msgs.msg import PoseStamped, TwistStamped
-import tf.transformations as t
+from aruco_pose.msg import MarkerArray
 
 
 # TODO: roscore is running
 # TODO: clever.service is running
 # TODO: check attitude is present
 # TODO: disk free space
-# TODO: map, base_link, body
+# TODO: local_origin, fcu, fcu_horiz
 # TODO: rc service
 # TODO: perform commander check, ekf2 status on PX4
 # TODO: check if FCU params setter succeed
@@ -56,9 +56,9 @@ def check_fcu():
     try:
         state = rospy.wait_for_message('mavros/state', State, timeout=3)
         if not state.connected:
-            failure('no connection to the FCU (check wiring)')
+            failure('No connection to the FCU (check wiring)')
     except rospy.ROSException:
-        failure('no MAVROS state (check wiring)')
+        failure('No MAVROS state (check wiring)')
 
 
 @check('Camera')
@@ -66,58 +66,37 @@ def check_camera(name):
     try:
         img = rospy.wait_for_message(name + '/image_raw', Image, timeout=1)
     except rospy.ROSException:
-        failure('%s: no images (is the camera connected properly?)', name)
+        failure('%s: No images (is the camera connected properly?)', name)
         return
     try:
         info = rospy.wait_for_message(name + '/camera_info', CameraInfo, timeout=1)
     except rospy.ROSException:
-        failure('%s: no calibration info', name)
+        failure('%s: No calibration info', name)
         return
 
     if img.width != info.width:
-        failure('%s: calibration width doesn\'t match image width (%d != %d)', name, info.width, img.width)
+        failure('%s: Calibration width doesn\'t match image width (%d != %d)', name, info.width, img.width)
     if img.height != info.height:
-        failure('%s: calibration height doesn\'t match image height (%d != %d))', name, info.height, img.height)
+        failure('%s: Calibration height doesn\'t match image height (%d != %d))', name, info.height, img.height)
 
 
 @check('Aruco detector')
 def check_aruco():
     try:
-        rospy.wait_for_message('aruco_pose/debug', Image, timeout=1)
+        rospy.wait_for_message('aruco_detect/markers', MarkerArray, timeout=0.5)
     except rospy.ROSException:
-        failure('no aruco_pose/debug messages')
+        failure('No aruco markers detection')
 
 
-@check('Vision position estimate')
+@check('Visual position estimate')
 def check_vpe():
     try:
-        vis = rospy.wait_for_message('mavros/vision_pose/pose', PoseStamped, timeout=1)
+        rospy.wait_for_message('mavros/vision_pose/pose', PoseStamped, timeout=1)
     except rospy.ROSException:
         try:
-            vis = rospy.wait_for_message('mavros/mocap/pose', PoseStamped, timeout=1)
+            rospy.wait_for_message('mavros/mocap/pose', PoseStamped, timeout=1)
         except rospy.ROSException:
-            failure('no VPE or MoCap messages')
-            return
-
-    # check vision pose and estimated pose inconsistency
-    try:
-        pose = rospy.wait_for_message('mavros/local_position/pose', PoseStamped, timeout=1)
-    except:
-        return
-    horiz = math.hypot(vis.pose.position.x - pose.pose.position.x, vis.pose.position.y - pose.pose.position.y)
-    if horiz > 0.5:
-        failure('horizontal position inconsistency: %.2f m', horiz)
-    vert = vis.pose.position.z - pose.pose.position.z
-    if abs(vert) > 0.5:
-        failure('vertical position inconsistency: %.2f m', vert)
-    op = pose.pose.orientation
-    ov = vis.pose.orientation
-    yawp, _, _ = t.euler_from_quaternion((op.x, op.y, op.z, op.w), axes='rzyx')
-    yawv, _, _ = t.euler_from_quaternion((ov.x, ov.y, ov.z, ov.w), axes='rzyx')
-    yawdiff = yawp - yawv
-    yawdiff = math.degrees((yawdiff + 180) % 360 - 180)
-    if abs(yawdiff) > 8:
-        failure('yaw inconsistency: %.2f deg', yawdiff)
+            failure('No VPE or MoCap messages')
 
 
 @check('Simple offboard node')
@@ -127,7 +106,7 @@ def check_simpleoffboard():
         rospy.wait_for_service('get_telemetry', timeout=3)
         rospy.wait_for_service('land', timeout=3)
     except rospy.ROSException:
-        failure('no simple_offboard services')
+        failure('No simple_offboard services')
 
 
 @check('IMU')
@@ -135,25 +114,15 @@ def check_imu():
     try:
         rospy.wait_for_message('mavros/imu/data', Imu, timeout=1)
     except rospy.ROSException:
-        failure('no IMU data (check flight controller calibration)')
+        failure('No IMU data (check flight controller calibration)')
 
 
 @check('Local position')
 def check_local_position():
     try:
-        pose = rospy.wait_for_message('mavros/local_position/pose', PoseStamped, timeout=1)
-        o = pose.pose.orientation
-        _, pitch, roll = t.euler_from_quaternion((o.x, o.y, o.z, o.w), axes='rzyx')
-        MAX_ANGLE = math.radians(2)
-        if abs(pitch) > MAX_ANGLE:
-            failure('pitch is %.2f deg; place copter horizontally or redo level horizon calib',
-                    math.degrees(pitch))
-        if abs(roll) > MAX_ANGLE:
-            failure('roll is %.2f deg; place copter horizontally or redo level horizon calib',
-                    math.degrees(roll))
-
+        rospy.wait_for_message('mavros/local_position/pose', PoseStamped, timeout=1)
     except rospy.ROSException:
-        failure('no local position')
+        failure('No local position')
 
 
 @check('Velocity estimation')
@@ -163,23 +132,23 @@ def check_velocity():
         horiz = math.hypot(velocity.twist.linear.x, velocity.twist.linear.y)
         vert = velocity.twist.linear.z
         if abs(horiz) > 0.1:
-            failure('horizontal velocity estimation is %.2f m/s; is copter staying still?' % horiz)
+            failure('Horizontal velocity estimation is %.2f m/s; is copter staying still?' % horiz)
         if abs(vert) > 0.1:
-            failure('vertical velocity estimation is %.2f m/s; is copter staying still?' % vert)
+            failure('Vertical velocity estimation is %.2f m/s; is copter staying still?' % vert)
 
         angular = velocity.twist.angular
         ANGULAR_VELOCITY_LIMIT = 0.01
         if abs(angular.x) > ANGULAR_VELOCITY_LIMIT:
-            failure('pitch rate estimation is %.2f rad/s (%.2f deg/s); is copter staying still?',
+            failure('Pitch rate estimation is %.2f rad/s (%.2f deg/s); is copter staying still?',
                     angular.x, math.degrees(angular.x))
         if abs(angular.y) > ANGULAR_VELOCITY_LIMIT:
-            failure('pitch rate estimation is %.2f rad/s (%.2f deg/s); is copter staying still?',
+            failure('Pitch rate estimation is %.2f rad/s (%.2f deg/s); is copter staying still?',
                     angular.y, math.degrees(angular.y))
         if abs(angular.z) > ANGULAR_VELOCITY_LIMIT:
-            failure('pitch rate estimation is %.2f rad/s (%.2f deg/s); is copter staying still?',
+            failure('Pitch rate estimation is %.2f rad/s (%.2f deg/s); is copter staying still?',
                     angular.z, math.degrees(angular.z))
     except rospy.ROSException:
-        failure('no velocity estimation')
+        failure('No velocity estimation')
 
 
 @check('Global position (GPS)')
@@ -187,7 +156,7 @@ def check_global_position():
     try:
         rospy.wait_for_message('mavros/global_position/global', NavSatFix, timeout=1)
     except rospy.ROSException:
-        failure('no global position')
+        failure('No global position')
 
 
 @check('Optical flow')
@@ -196,7 +165,7 @@ def check_optical_flow():
     try:
         rospy.wait_for_message('mavros/px4flow/raw/send', OpticalFlowRad, timeout=0.5)
     except rospy.ROSException:
-        failure('no optical flow data (from Raspberry)')
+        failure('No optical flow data (from Raspberry)')
 
 
 @check('Rangefinder')
@@ -205,11 +174,11 @@ def check_rangefinder():
     try:
         rospy.wait_for_message('mavros/distance_sensor/rangefinder_3_sub', Range, timeout=0.5)
     except rospy.ROSException:
-        failure('no randefinder data from Raspberry')
+        failure('No randefinder data from Raspberry')
     try:
         rospy.wait_for_message('mavros/distance_sensor/rangefinder_0', Range, timeout=0.5)
     except rospy.ROSException:
-        failure('no rangefinder data from PX4')
+        failure('No rangefinder data from PX4')
 
 
 @check('Boot duration')
@@ -237,7 +206,7 @@ def check_cpu_usage():
         pid, cpu, cmd = process.split('\t')
 
         if cmd.strip() not in WHITELIST and float(cpu) > 30:
-            failure('high CPU usage (%s%%) detected: %s (PID %s)',
+            failure('High CPU usage (%s%%) detected: %s (PID %s)',
                     cpu.strip(), cmd.strip(), pid.strip())
 
 

clever/src/simple_offboard.cpp

@@ -1,742 +0,0 @@
-/*
- * Simplified copter control in OFFBOARD mode
- * Copyright (C) 2019 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.
- */
-
-#include <algorithm>
-#include <string>
-#include <cmath>
-#include <boost/format.hpp>
-#include <stdexcept>
-#include <GeographicLib/Geodesic.hpp>
-#include <ros/ros.h>
-#include <tf/transform_datatypes.h>
-#include <tf2/utils.h>
-#include <tf2_ros/transform_listener.h>
-#include <tf2_ros/transform_broadcaster.h>
-#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
-#include <std_srvs/Trigger.h>
-#include <geometry_msgs/PoseStamped.h>
-#include <geometry_msgs/TwistStamped.h>
-#include <geometry_msgs/Vector3Stamped.h>
-#include <geometry_msgs/QuaternionStamped.h>
-#include <sensor_msgs/NavSatFix.h>
-#include <sensor_msgs/BatteryState.h>
-#include <mavros_msgs/CommandBool.h>
-#include <mavros_msgs/SetMode.h>
-#include <mavros_msgs/PositionTarget.h>
-#include <mavros_msgs/AttitudeTarget.h>
-#include <mavros_msgs/Thrust.h>
-#include <mavros_msgs/State.h>
-
-#include <clever/GetTelemetry.h>
-#include <clever/Navigate.h>
-#include <clever/NavigateGlobal.h>
-#include <clever/SetPosition.h>
-#include <clever/SetVelocity.h>
-#include <clever/SetAttitude.h>
-#include <clever/SetRates.h>
-
-using std::string;
-using namespace geometry_msgs;
-using namespace sensor_msgs;
-using namespace clever;
-using mavros_msgs::PositionTarget;
-using mavros_msgs::AttitudeTarget;
-using mavros_msgs::Thrust;
-
-// tf2
-tf2_ros::Buffer tf_buffer;
-
-// Parameters
-string local_frame;
-string fcu_frame;
-ros::Duration transform_timeout;
-ros::Duration telemetry_transform_timeout;
-ros::Duration offboard_timeout;
-ros::Duration land_timeout;
-ros::Duration arming_timeout;
-ros::Duration local_position_timeout;
-ros::Duration state_timeout;
-ros::Duration velocity_timeout;
-ros::Duration global_position_timeout;
-ros::Duration battery_timeout;
-float default_speed;
-bool auto_release;
-std::map<string, string> reference_frames;
-
-// Publishers
-ros::Publisher attitude_pub, attitude_raw_pub, position_pub, position_raw_pub, rates_pub, thrust_pub;
-
-// Service clients
-ros::ServiceClient arming, set_mode;
-
-// Containers
-ros::Timer setpoint_timer;
-tf::Quaternion tq;
-PoseStamped position_msg;
-PositionTarget position_raw_msg;
-AttitudeTarget att_raw_msg;
-Thrust thrust_msg;
-TwistStamped rates_msg;
-TransformStamped target;
-
-// State
-PoseStamped nav_start;
-PoseStamped setpoint_position, setpoint_position_transformed;
-Vector3Stamped setpoint_velocity, setpoint_velocity_transformed;
-QuaternionStamped setpoint_attitude, setpoint_attitude_transformed;
-float setpoint_yaw_rate;
-float nav_speed;
-bool busy = false;
-bool wait_armed = false;
-
-enum setpoint_type_t {
-	NONE,
-	NAVIGATE,
-	NAVIGATE_GLOBAL,
-	POSITION,
-	VELOCITY,
-	ATTITUDE,
-	RATES
-};
-
-enum setpoint_type_t setpoint_type = NONE;
-
-enum { YAW, YAW_RATE, TOWARDS } setpoint_yaw_type;
-
-// Last received telemetry messages
-mavros_msgs::State state;
-PoseStamped local_position;
-TwistStamped velocity;
-NavSatFix global_position;
-BatteryState battery;
-
-// Common subcriber callback template that stores message to the variable
-template<typename T, T& STORAGE>
-void handleMessage(const T& msg)
-{
-	STORAGE = msg;
-}
-
-// wait for transform without interrupting publishing setpoints
-inline bool waitTransform(const string& target, const string& source,
-                          const ros::Time& stamp, const ros::Duration& timeout)
-{
-	ros::Rate r(10);
-	auto start = ros::Time::now();
-	while (ros::ok()) {
-		if (ros::Time::now() - start > timeout) return false;
-		if (tf_buffer.canTransform(target, source, stamp)) return true;
-		ros::spinOnce();
-		r.sleep();
-	}
-}
-
-#define TIMEOUT(msg, timeout) (ros::Time::now() - msg.header.stamp > timeout)
-
-bool getTelemetry(GetTelemetry::Request& req, GetTelemetry::Response& res)
-{
-	ros::Time stamp = ros::Time::now();
-
-	if (req.frame_id.empty())
-		req.frame_id = local_frame;
-
-	res.frame_id = req.frame_id;
-	res.x = NAN;
-	res.y = NAN;
-	res.z = NAN;
-	res.lat = NAN;
-	res.lon = NAN;
-	res.alt = NAN;
-	res.vx = NAN;
-	res.vy = NAN;
-	res.vz = NAN;
-	res.pitch = NAN;
-	res.roll = NAN;
-	res.yaw = NAN;
-	res.pitch_rate = NAN;
-	res.roll_rate = NAN;
-	res.yaw_rate = NAN;
-	res.voltage = NAN;
-	res.cell_voltage = NAN;
-
-	if (!TIMEOUT(state, state_timeout)) {
-		res.connected = state.connected;
-		res.armed = state.armed;
-		res.mode = state.mode;
-	}
-
-	waitTransform(local_frame, req.frame_id, stamp, telemetry_transform_timeout);
-
-	if (!TIMEOUT(local_position, local_position_timeout)) {
-		try {
-			// transform pose
-			PoseStamped pose;
-			tf_buffer.transform(local_position, pose, req.frame_id);
-			res.x = pose.pose.position.x;
-			res.y = pose.pose.position.y;
-			res.z = pose.pose.position.z;
-
-			// Tait-Bryan angles, order z-y-x
-			double yaw, pitch, roll;
-			tf2::getEulerYPR(pose.pose.orientation, yaw, pitch, roll);
-			res.yaw = yaw;
-			res.pitch = pitch;
-			res.roll = roll;
-		} catch (const tf2::TransformException& e) {}
-	}
-
-	if (!TIMEOUT(velocity, velocity_timeout)) {
-		try {
-			// transform velocity
-			Vector3Stamped vec, vec_out;
-			vec.header = velocity.header;
-			vec.vector = velocity.twist.linear;
-			tf_buffer.transform(vec, vec_out, req.frame_id);
-
-			res.vx = vec_out.vector.x;
-			res.vy = vec_out.vector.y;
-			res.vz = vec_out.vector.z;
-		} catch (const tf2::TransformException& e) {}
-
-		// use angular velocities as they are
-		res.yaw_rate = velocity.twist.angular.z;
-		res.pitch_rate = velocity.twist.angular.y;
-		res.roll_rate = velocity.twist.angular.x;
-	}
-
-	if (!TIMEOUT(global_position, global_position_timeout)) {
-		res.lat = global_position.latitude;
-		res.lon = global_position.longitude;
-		res.alt = global_position.altitude;
-	}
-
-	if (!TIMEOUT(battery, battery_timeout)) {
-		res.voltage = battery.voltage;
-		if (!battery.cell_voltage.empty()) {
-			res.cell_voltage = battery.cell_voltage[0];
-		}
-	}
-
-	return true;
-}
-
-// throws std::runtime_error
-void offboardAndArm()
-{
-	ros::Rate r(10);
-
-	if (state.mode != "OFFBOARD") {
-		auto start = ros::Time::now();
-		ROS_INFO("simple_offboard: switch to OFFBOARD");
-		static mavros_msgs::SetMode sm;
-		sm.request.custom_mode = "OFFBOARD";
-
-		if (!set_mode.call(sm))
-			throw std::runtime_error("Error calling set_mode service");
-
-		// wait for OFFBOARD mode
-		while (ros::ok()) {
-			ros::spinOnce();
-			if (state.mode == "OFFBOARD") {
-				break;
-			} else if (ros::Time::now() - start > offboard_timeout) {
-				throw std::runtime_error("OFFBOARD request timed out");
-			}
-			ros::spinOnce();
-			r.sleep();
-		}
-	}
-
-	if (!state.armed) {
-		ros::Time start = ros::Time::now();
-		ROS_INFO("simple_offboard: arming");
-		mavros_msgs::CommandBool srv;
-		srv.request.value = true;
-		if (!arming.call(srv)) {
-			throw std::runtime_error("Error calling arming service");
-		}
-
-		// wait until armed
-		while (ros::ok()) {
-			ros::spinOnce();
-			if (state.armed) {
-				break;
-			} else if (ros::Time::now() - start > arming_timeout) {
-				throw std::runtime_error("Arming timed out");
-			}
-			ros::spinOnce();
-			r.sleep();
-		}
-	}
-}
-
-inline double hypot(double x, double y, double z)
-{
-	return std::sqrt(x * x + y * y + z * z);
-}
-
-inline float getDistance(const Point& from, const Point& to)
-{
-	return hypot(from.x - to.x, from.y - to.y, from.z - to.z);
-}
-
-void getNavigateSetpoint(const ros::Time& stamp, float speed, Point& nav_setpoint)
-{
-	if (wait_armed) {
-		// don't start navigating if we're waiting arming
-		nav_start.header.stamp = stamp;
-	}
-
-	float distance = getDistance(nav_start.pose.position, setpoint_position_transformed.pose.position);
-	float time = distance / speed;
-	float passed = std::min((stamp - nav_start.header.stamp).toSec() / time, 1.0);
-
-	nav_setpoint.x = nav_start.pose.position.x + (setpoint_position_transformed.pose.position.x - nav_start.pose.position.x) * passed;
-	nav_setpoint.y = nav_start.pose.position.y + (setpoint_position_transformed.pose.position.y - nav_start.pose.position.y) * passed;
-	nav_setpoint.z = nav_start.pose.position.z + (setpoint_position_transformed.pose.position.z - nav_start.pose.position.z) * passed;
-}
-
-PoseStamped globalToLocal(double lat, double lon)
-{
-	auto earth = GeographicLib::Geodesic::WGS84();
-
-	// Determine azimuth and distance between current and destination point
-	double _, distance, azimuth;
-	earth.Inverse(global_position.latitude, global_position.longitude, lat, lon, distance, _, azimuth);
-
-	double x_offset, y_offset;
-	double azimuth_radians = azimuth * M_PI / 180;
-	x_offset = distance * sin(azimuth_radians);
-	y_offset = distance * cos(azimuth_radians);
-
-	auto local = tf_buffer.lookupTransform(local_frame, fcu_frame, global_position.header.stamp);
-
-	PoseStamped pose;
-	pose.header.stamp = global_position.header.stamp; // TODO: ?
-	pose.header.frame_id = local_frame;
-	pose.pose.position.x = local.transform.translation.x + x_offset;
-	pose.pose.position.y = local.transform.translation.y + y_offset;
-	pose.pose.orientation.w = 1;
-	return pose;
-}
-
-void publish(const ros::Time stamp)
-{
-	if (setpoint_type == NONE) return;
-
-	position_raw_msg.header.stamp = stamp;
-	thrust_msg.header.stamp = stamp;
-	rates_msg.header.stamp = stamp;
-
-	try {
-		// transform position and/or yaw
-		if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == POSITION || setpoint_type == VELOCITY || setpoint_type == ATTITUDE) {
-			setpoint_position.header.stamp = stamp;
-			tf_buffer.transform(setpoint_position, setpoint_position_transformed, local_frame, ros::Duration(0.05));
-		}
-
-		// transform velocity
-		if (setpoint_type == VELOCITY) {
-			setpoint_velocity.header.stamp = stamp;
-			tf_buffer.transform(setpoint_velocity, setpoint_velocity_transformed, local_frame, ros::Duration(0.05));
-		}
-
-	} catch (const tf2::TransformException& e) {
-		ROS_WARN_THROTTLE(10, "simple_offboard: can't transform");
-	}
-
-	if (!target.child_frame_id.empty()) {
-		if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == POSITION) {
-			static tf2_ros::TransformBroadcaster tf_broadcaster;
-			target.header = setpoint_position_transformed.header;
-			target.transform.translation.x = setpoint_position_transformed.pose.position.x;
-			target.transform.translation.y = setpoint_position_transformed.pose.position.y;
-			target.transform.translation.z = setpoint_position_transformed.pose.position.z;
-			target.transform.rotation = setpoint_position_transformed.pose.orientation;
-			tf_broadcaster.sendTransform(target);
-		}
-	}
-
-	if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL) {
-		position_msg.pose.orientation = setpoint_position_transformed.pose.orientation; // copy yaw
-		getNavigateSetpoint(stamp, nav_speed, position_msg.pose.position);
-
-		if (setpoint_yaw_type == TOWARDS) {
-			double yaw_towards = atan2(position_msg.pose.position.y - nav_start.pose.position.y,
-			                           position_msg.pose.position.x - nav_start.pose.position.x);
-			position_msg.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, yaw_towards);
-		}
-	}
-
-	if (setpoint_type == POSITION) {
-		position_msg = setpoint_position_transformed;
-	}
-
-	if (setpoint_type == POSITION || setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL) {
-		if (setpoint_yaw_type == YAW || setpoint_yaw_type == TOWARDS) {
-			position_msg.header.stamp = stamp;
-			position_pub.publish(position_msg);
-
-		} else {
-			position_raw_msg.type_mask = PositionTarget::IGNORE_VX +
-			                             PositionTarget::IGNORE_VY +
-			                             PositionTarget::IGNORE_VZ +
-			                             PositionTarget::IGNORE_AFX +
-			                             PositionTarget::IGNORE_AFY +
-			                             PositionTarget::IGNORE_AFZ +
-			                             PositionTarget::IGNORE_YAW;
-			position_raw_msg.yaw_rate = setpoint_yaw_rate;
-			position_raw_msg.position = position_msg.pose.position;
-			position_raw_pub.publish(position_raw_msg);
-		}
-	}
-
-	if (setpoint_type == VELOCITY) {
-		position_raw_msg.type_mask = PositionTarget::IGNORE_PX +
-		                             PositionTarget::IGNORE_PY +
-		                             PositionTarget::IGNORE_PZ +
-		                             PositionTarget::IGNORE_AFX +
-		                             PositionTarget::IGNORE_AFY +
-		                             PositionTarget::IGNORE_AFZ;
-		position_raw_msg.type_mask += setpoint_yaw_type == YAW ? PositionTarget::IGNORE_YAW_RATE : PositionTarget::IGNORE_YAW;
-		position_raw_msg.velocity = setpoint_velocity_transformed.vector;
-		position_raw_msg.yaw = tf2::getYaw(setpoint_position_transformed.pose.orientation);
-		position_raw_msg.yaw_rate = setpoint_yaw_rate;
-		position_raw_pub.publish(position_raw_msg);
-	}
-
-	if (setpoint_type == ATTITUDE) {
-		attitude_pub.publish(setpoint_position_transformed);
-		thrust_pub.publish(thrust_msg);
-	}
-
-	if (setpoint_type == RATES) {
-		// rates_pub.publish(rates_msg);
-		// thrust_pub.publish(thrust_msg);
-		// mavros rates topics waits for rates in local frame
-		// use rates in body frame for simplicity
-		att_raw_msg.header.stamp = stamp;
-		att_raw_msg.header.frame_id = fcu_frame;
-		att_raw_msg.type_mask = AttitudeTarget::IGNORE_ATTITUDE;
-		att_raw_msg.body_rate = rates_msg.twist.angular;
-		att_raw_msg.thrust = thrust_msg.thrust;
-		attitude_raw_pub.publish(att_raw_msg);
-	}
-}
-
-void publishSetpoint(const ros::TimerEvent& event)
-{
-	publish(event.current_real);
-}
-
-inline void checkState()
-{
-	if (TIMEOUT(state, state_timeout))
-		throw std::runtime_error("State timeout, check mavros settings");
-
-	if (!state.connected)
-		throw std::runtime_error("No connection to FCU, https://clever.copterexpress.com/connection.html");
-}
-
-inline void serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, float vy, float vz,
-                  float pitch, float roll, float yaw, float pitch_rate, float roll_rate, float yaw_rate,
-                  float lat, float lon, float thrust, float speed, string frame_id, bool auto_arm,
-                  uint8_t& success, string& message)
-{
-	auto stamp = ros::Time::now();
-
-	try {
-		if (busy)
-			throw std::runtime_error("Busy");
-
-		busy = true;
-
-		// Checks
-		checkState();
-
-		if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL) {
-			if (TIMEOUT(local_position, local_position_timeout))
-				throw std::runtime_error("No local position, check settings");
-
-			if (speed < 0)
-				throw std::runtime_error("Navigate speed must be positive, " + std::to_string(speed) + " passed");
-
-			if (speed == 0)
-				speed = default_speed;
-		}
-
-		if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == POSITION || sp_type == VELOCITY) {
-			if (yaw_rate != 0 && !std::isnan(yaw))
-				throw std::runtime_error("Yaw value should be NaN for setting yaw rate");
-
-			if (std::isnan(yaw_rate) && std::isnan(yaw))
-				throw std::runtime_error("Both yaw and yaw_rate cannot be NaN");
-		}
-
-		if (sp_type == NAVIGATE_GLOBAL) {
-			if (TIMEOUT(global_position, global_position_timeout))
-				throw std::runtime_error("No global position");
-		}
-
-		// default frame is local frame
-		if (frame_id.empty())
-			frame_id = local_frame;
-
-		// look up for reference frame
-		auto search = reference_frames.find(frame_id);
-		const string& reference_frame = search == reference_frames.end() ? frame_id : search->second;
-
-		if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == POSITION || sp_type == VELOCITY || sp_type == ATTITUDE) {
-			// make sure transform from frame_id to reference frame available
-			if (!waitTransform(reference_frame, frame_id, stamp, transform_timeout))
-				throw std::runtime_error("Can't transform from " + frame_id + " to " + reference_frame);
-
-			// make sure transform from reference frame to local frame available
-			if (!waitTransform(local_frame, reference_frame, stamp, transform_timeout))
-				throw std::runtime_error("Can't transform from " + reference_frame + " to " + local_frame);
-		}
-
-		if (sp_type == NAVIGATE_GLOBAL) {
-			// Calculate x and from lat and lot in request's frame
-			auto xy_in_req_frame = tf_buffer.transform(globalToLocal(lat, lon), frame_id);
-			x = xy_in_req_frame.pose.position.x;
-			y = xy_in_req_frame.pose.position.y;
-		}
-
-		// Everything fine - switch setpoint type
-		setpoint_type = sp_type;
-
-		if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL) {
-			// starting point
-			nav_start = local_position;
-			nav_speed = speed;
-		}
-
-		if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == POSITION || sp_type == VELOCITY) {
-			if (std::isnan(yaw) && yaw_rate == 0) {
-				// keep yaw unchanged
-				yaw = tf2::getYaw(local_position.pose.orientation);
-			}
-		}
-
-		if (sp_type == POSITION || sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == VELOCITY || sp_type == ATTITUDE) {
-			// destination point and/or yaw
-			static PoseStamped ps;
-			ps.header.frame_id = frame_id;
-			ps.header.stamp = stamp;
-			ps.pose.position.x = x;
-			ps.pose.position.y = y;
-			ps.pose.position.z = z;
-
-			if (std::isnan(yaw)) {
-				setpoint_yaw_type = YAW_RATE;
-				setpoint_yaw_rate = yaw_rate;
-			} else if (std::isinf(yaw) && yaw > 0) {
-				// yaw towards
-				setpoint_yaw_type = TOWARDS;
-				yaw = 0;
-				setpoint_yaw_rate = 0;
-			} else {
-				setpoint_yaw_type = YAW;
-				setpoint_yaw_rate = 0;
-				ps.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);
-			}
-
-			tf_buffer.transform(ps, setpoint_position, reference_frame);
-		}
-
-		if (sp_type == VELOCITY) {
-			static Vector3Stamped vel;
-			vel.header.frame_id = frame_id;
-			vel.header.stamp = stamp;
-			vel.vector.x = vx;
-			vel.vector.y = vy;
-			vel.vector.z = vz;
-			tf_buffer.transform(vel, setpoint_velocity, reference_frame);
-		}
-
-		if (sp_type == ATTITUDE || sp_type == RATES) {
-			thrust_msg.thrust = thrust;
-		}
-
-		if (sp_type == RATES) {
-			rates_msg.twist.angular.x = roll_rate;
-			rates_msg.twist.angular.y = pitch_rate;
-			rates_msg.twist.angular.z = yaw_rate;
-		}
-
-		wait_armed = auto_arm;
-
-		publish(stamp); // calculate initial transformed messages first
-		setpoint_timer.start();
-
-		if (auto_arm) {
-			offboardAndArm();
-			wait_armed = false;
-		} else if (state.mode != "OFFBOARD") {
-			setpoint_timer.stop();
-			throw std::runtime_error("Copter is not in OFFBOARD mode, use auto_arm?");
-		} else if (!state.armed) {
-			setpoint_timer.stop();
-			throw std::runtime_error("Copter is not armed, use auto_arm?");
-		}
-
-	} catch (const std::exception& e) {
-		message = e.what();
-		ROS_INFO("simple_offboard: %s", message.c_str());
-		busy = false;
-		return;
-	}
-
-	success = true;
-	busy = false;
-	return;
-}
-
-bool navigate(Navigate::Request& req, Navigate::Response& res) {
-	serve(NAVIGATE, req.x, req.y, req.z, 0, 0, 0, 0, 0, req.yaw, 0, 0, req.yaw_rate, 0, 0, 0, req.speed, req.frame_id, req.auto_arm, res.success, res.message);
-	return true;
-}
-
-bool navigateGlobal(NavigateGlobal::Request& req, NavigateGlobal::Response& res) {
-	serve(NAVIGATE_GLOBAL, 0, 0, req.z, 0, 0, 0, 0, 0, req.yaw, 0, 0, req.yaw_rate, req.lat, req.lon, 0, req.speed, req.frame_id, req.auto_arm, res.success, res.message);
-	return true;
-}
-
-bool setPosition(SetPosition::Request& req, SetPosition::Response& res) {
-	serve(POSITION, req.x, req.y, req.z, 0, 0, 0, 0, 0, req.yaw, 0, 0, req.yaw_rate, 0, 0, 0, 0, req.frame_id, req.auto_arm, res.success, res.message);
-	return true;
-}
-
-bool setVelocity(SetVelocity::Request& req, SetVelocity::Response& res) {
-	serve(VELOCITY, 0, 0, 0, req.vx, req.vy, req.vz, 0, 0, req.yaw, 0, 0, req.yaw_rate, 0, 0, 0, 0, req.frame_id, req.auto_arm, res.success, res.message);
-	return true;
-}
-
-bool setAttitude(SetAttitude::Request& req, SetAttitude::Response& res) {
-	serve(ATTITUDE, 0, 0, 0, 0, 0, 0, req.pitch, req.roll, req.yaw, 0, 0, 0, 0, 0, req.thrust, 0, req.frame_id, req.auto_arm, res.success, res.message);
-	return true;
-}
-
-bool setRates(SetRates::Request& req, SetRates::Response& res) {
-	serve(RATES, 0, 0, 0, 0, 0, 0, 0, 0, 0, req.pitch_rate, req.roll_rate, req.yaw_rate, 0, 0, req.thrust, 0, "", req.auto_arm, res.success, res.message);
-	return true;
-}
-
-bool land(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)
-{
-	try {
-		if (busy)
-			throw std::runtime_error("Busy");
-
-		busy = true;
-
-		checkState();
-
-		static mavros_msgs::SetMode sm;
-		sm.request.custom_mode = "AUTO.LAND";
-
-		if (!set_mode.call(sm))
-			throw std::runtime_error("Can't call set_mode service");
-
-		if (!sm.response.mode_sent)
-			throw std::runtime_error("Can't send set_mode request");
-
-		static ros::Rate r(10);
-		auto start = ros::Time::now();
-		while (ros::ok()) {
-			if (state.mode == "AUTO.LAND") {
-				res.success = true;
-				busy = false;
-				return true;
-			}
-			if (ros::Time::now() - start > land_timeout)
-				throw std::runtime_error("Land request timed out");
-
-			ros::spinOnce();
-			r.sleep();
-		}
-
-	} catch (const std::exception& e) {
-		res.message = e.what();
-		ROS_INFO("simple_offboard: %s", e.what());
-		busy = false;
-		return true;
-	}
-}
-
-int main(int argc, char **argv)
-{
-	ros::init(argc, argv, "simple_offboard");
-	ros::NodeHandle nh, nh_priv("~");
-
-	tf2_ros::TransformListener tf_listener(tf_buffer);
-
-	// Params
-	nh.param<string>("mavros/local_position/tf/frame_id", local_frame, "map");
-	nh.param<string>("mavros/local_position/tf/child_frame_id", fcu_frame, "base_link");
-	nh_priv.param("target_frame", target.child_frame_id, string("target"));
-	nh_priv.param("auto_release", auto_release, true);
-	nh_priv.param("default_speed", default_speed, 0.5f);
-	nh_priv.getParam("reference_frames", reference_frames);
-
-	state_timeout = ros::Duration(nh_priv.param("state_timeout", 3.0));
-	local_position_timeout = ros::Duration(nh_priv.param("local_position_timeout", 2.0));
-	velocity_timeout = ros::Duration(nh_priv.param("velocity_timeout", 2.0));
-	global_position_timeout = ros::Duration(nh_priv.param("global_position_timeout", 10.0));
-	battery_timeout = ros::Duration(nh_priv.param("battery_timeout", 2.0));
-
-	transform_timeout = ros::Duration(nh_priv.param("transform_timeout", 0.5));
-	telemetry_transform_timeout = ros::Duration(nh_priv.param("telemetry_transform_timeout", 0.5));
-	offboard_timeout = ros::Duration(nh_priv.param("offboard_timeout", 3.0));
-	land_timeout = ros::Duration(nh_priv.param("land_timeout", 3.0));
-	arming_timeout = ros::Duration(nh_priv.param("arming_timeout", 4.0));
-
-	// Service clients
-	arming = nh.serviceClient<mavros_msgs::CommandBool>("mavros/cmd/arming");
-	set_mode = nh.serviceClient<mavros_msgs::SetMode>("mavros/set_mode");
-
-	// Telemetry subscribers
-	auto state_sub = nh.subscribe("mavros/state", 1, &handleMessage<mavros_msgs::State, state>);
-	auto local_position_sub = nh.subscribe("mavros/local_position/pose", 1, &handleMessage<PoseStamped, local_position>);
-	auto velocity_sub = nh.subscribe("mavros/local_position/velocity", 1, &handleMessage<TwistStamped, velocity>);
-	auto global_position_sub = nh.subscribe("mavros/global_position/global", 1, &handleMessage<NavSatFix, global_position>);
-	auto battery_sub = nh.subscribe("mavros/battery", 1, &handleMessage<BatteryState, battery>);
-
-	// Setpoint publishers
-	position_pub = nh.advertise<PoseStamped>("mavros/setpoint_position/local", 1);
-	position_raw_pub = nh.advertise<PositionTarget>("mavros/setpoint_raw/local", 1);
-	attitude_pub = nh.advertise<PoseStamped>("mavros/setpoint_attitude/attitude", 1);
-	attitude_raw_pub = nh.advertise<AttitudeTarget>("mavros/setpoint_raw/attitude", 1);
-	rates_pub = nh.advertise<TwistStamped>("mavros/setpoint_attitude/cmd_vel", 1);
-	thrust_pub = nh.advertise<Thrust>("mavros/setpoint_attitude/thrust", 1);
-
-	 // Service servers
-	auto gt_serv = nh.advertiseService("get_telemetry", &getTelemetry);
-	auto na_serv = nh.advertiseService("navigate", &navigate);
-	auto ng_serv = nh.advertiseService("navigate_global", &navigateGlobal);
-	auto sp_serv = nh.advertiseService("set_position", &setPosition);
-	auto sv_serv = nh.advertiseService("set_velocity", &setVelocity);
-	auto sa_serv = nh.advertiseService("set_attitude", &setAttitude);
-	auto sr_serv = nh.advertiseService("set_rates", &setRates);
-	auto ld_serv = nh.advertiseService("land", &land);
-
-	// Setpoint timer
-	setpoint_timer = nh.createTimer(ros::Duration(1 / nh_priv.param("setpoint_rate", 30.0)), &publishSetpoint, false, false);
-
-	position_msg.header.frame_id = local_frame;
-	position_raw_msg.header.frame_id = local_frame;
-	position_raw_msg.coordinate_frame = PositionTarget::FRAME_LOCAL_NED;
-	rates_msg.header.frame_id = fcu_frame;
-
-	ROS_INFO("simple_offboard: ready");
-	ros::spin();
-}

clever/src/simple_offboard.py

@@ -0,0 +1,480 @@
+#!/usr/bin/env python
+from __future__ import division
+
+import rospy
+from geometry_msgs.msg import TransformStamped, PoseStamped, Point, PointStamped, Vector3, \
+    Vector3Stamped, TwistStamped, QuaternionStamped
+from sensor_msgs.msg import NavSatFix, BatteryState
+import tf2_ros
+import tf2_geometry_msgs
+from mavros_msgs.msg import PositionTarget, AttitudeTarget, State
+from mavros_msgs.srv import CommandBool, SetMode
+from threading import Lock
+import math
+
+from global_local import global_to_local
+from util import euler_from_orientation, vector3_from_point, orientation_from_euler
+from std_srvs.srv import Trigger
+from clever import srv
+
+
+rospy.init_node('simple_offboard')
+
+
+# TF2 stuff
+tf_broadcaster = tf2_ros.TransformBroadcaster()
+static_tf_broadcaster = tf2_ros.StaticTransformBroadcaster()
+
+tf_buffer = tf2_ros.Buffer()
+tf_listener = tf2_ros.TransformListener(tf_buffer)
+
+
+position_pub = rospy.Publisher('/mavros/setpoint_raw/local', PositionTarget, queue_size=1)
+attitude_pub = rospy.Publisher('/mavros/setpoint_raw/attitude', AttitudeTarget, queue_size=1)
+target_pub = rospy.Publisher('~target', PoseStamped, queue_size=1)
+
+arming = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool, persistent=True)
+set_mode = rospy.ServiceProxy('/mavros/set_mode', SetMode, persistent=True)
+
+
+pose = None
+global_position = None
+velocity = None
+state = None
+battery = None
+
+
+def pose_update(data):
+    global pose
+    pose = data
+
+
+def global_position_update(data):
+    global global_position
+    global_position = data
+
+
+def velocity_update(data):
+    global velocity
+    velocity = data
+
+
+def state_update(data):
+    global state
+    state = data
+
+
+def battery_update(data):
+    global battery
+    battery = data
+
+
+rospy.Subscriber('/mavros/state', State, state_update)
+rospy.Subscriber('/mavros/local_position/pose', PoseStamped, pose_update)
+rospy.Subscriber('/mavros/local_position/velocity', TwistStamped, velocity_update)
+rospy.Subscriber('/mavros/global_position/global', NavSatFix, global_position_update)
+rospy.Subscriber('/mavros/battery', BatteryState, battery_update)
+
+
+PT = PositionTarget
+AT = AttitudeTarget
+AUTO_OFFBOARD = rospy.get_param('~auto_offboard', True)
+AUTO_ARM = AUTO_OFFBOARD and rospy.get_param('~auto_arm', True)
+OFFBOARD_TIMEOUT = rospy.Duration(rospy.get_param('~offboard_timeout', 3))
+ARM_TIMEOUT = rospy.Duration(rospy.get_param('~arm_timeout', 5))
+LOCAL_POSITION_TIMEOUT = rospy.Duration(rospy.get_param('~local_position_timeout', 0.5))
+NAVIGATE_AFTER_ARMED = rospy.Duration(rospy.get_param('~navigate_after_armed', True))
+TRANSFORM_TIMEOUT = rospy.Duration(rospy.get_param('~transform_timeout', 3))
+SETPOINT_RATE = rospy.get_param('~setpoint_rate', 30)
+LOCAL_FRAME = rospy.get_param('mavros/local_position/frame_id', 'local_origin')
+LAND_MODE = rospy.get_param('~land_mode', 'AUTO.LAND')
+LAND_TIMEOUT = rospy.Duration(rospy.get_param('~land_timeout', 2))
+DEFAULT_SPEED = rospy.get_param('~default_speed', 0.5)
+
+
+def offboard_and_arm():
+    if AUTO_OFFBOARD and state.mode != 'OFFBOARD':
+        rospy.sleep(.3)
+        rospy.loginfo('Switch mode to OFFBOARD')
+        res = set_mode(base_mode=0, custom_mode='OFFBOARD')
+
+        start = rospy.get_rostime()
+        while True:
+            if state.mode == 'OFFBOARD':
+                break
+            if rospy.get_rostime() - start > OFFBOARD_TIMEOUT:
+                raise Exception('OFFBOARD request timed out')
+            rospy.sleep(0.1)
+
+    if AUTO_ARM and not state.armed:
+        rospy.loginfo('Arming')
+        res = arming(True)
+
+        start = rospy.get_rostime()
+        while True:
+            if state.armed:
+                return True
+            if rospy.get_rostime() - start > ARM_TIMEOUT:
+                raise Exception('Arming timed out')
+            rospy.sleep(0.1)
+
+
+ps = PoseStamped()
+vs = Vector3Stamped()
+pt = PositionTarget()
+at = AttitudeTarget()
+
+
+BRAKE_TIME = rospy.Duration(0)
+
+
+def get_navigate_setpoint(stamp, start, finish, start_stamp, speed):
+    distance = math.sqrt((finish.z - start.z)**2 + (finish.x - start.x)**2 + (finish.y - start.y)**2)
+    time = rospy.Duration(distance / speed)
+    if time == rospy.Duration(0):
+        k = 0
+    else:
+        k = (stamp - start_stamp) / time
+    time_left = start_stamp + time - stamp
+
+    if BRAKE_TIME and time_left < BRAKE_TIME:
+        # time to brake
+        time_before_braking = time - BRAKE_TIME
+        brake_time_passed = (stamp - start_stamp - time_before_braking)
+
+        if brake_time_passed > 2 * BRAKE_TIME:
+            # finish
+            k = 1
+        else:
+            # brake!
+            k_before_braking = time_before_braking / time
+            k_after_braking = (speed * brake_time_passed.to_sec() - brake_time_passed.to_sec() ** 2 * speed / 4 / BRAKE_TIME.to_sec()) / distance
+            k = k_before_braking + k_after_braking
+
+    k = min(k, 1)
+
+    p = Point()
+    p.x = start.x + (finish.x - start.x) * k
+    p.y = start.y + (finish.y - start.y) * k
+    p.z = start.z + (finish.z - start.z) * k
+    return p
+
+
+def get_publisher_and_message(req, stamp, continued=True, update_frame=True):
+    ps.header.stamp = stamp
+    vs.header.stamp = stamp
+
+    # don't block on setpoints publishing
+    transform_timeout = rospy.Duration(0.1) if continued else TRANSFORM_TIMEOUT
+
+    if isinstance(req, (srv.NavigateRequest, srv.NavigateGlobalRequest)):
+        global current_nav_start, current_nav_start_stamp, current_nav_finish
+
+        if update_frame:
+            ps.header.frame_id = req.frame_id or LOCAL_FRAME
+            ps.pose.position = Point(getattr(req, 'x', 0), getattr(req, 'y', 0), req.z)
+            ps.pose.orientation = orientation_from_euler(0, 0, req.yaw, axes='sxyz')
+            current_nav_finish = tf_buffer.transform(ps, LOCAL_FRAME, transform_timeout)
+
+            if isinstance(req, srv.NavigateGlobalRequest):
+                # Recalculate x and y from lat and lon
+                current_nav_finish.pose.position.x, current_nav_finish.pose.position.y = \
+                    global_to_local(req.lat, req.lon)
+
+        if not continued:
+            current_nav_start = pose.pose.position
+            current_nav_start_stamp = stamp
+
+        if NAVIGATE_AFTER_ARMED and not state.armed:
+            current_nav_start_stamp = stamp
+
+        setpoint = get_navigate_setpoint(stamp, current_nav_start, current_nav_finish.pose.position,
+                                         current_nav_start_stamp, req.speed)
+
+        yaw_rate_flag = math.isnan(req.yaw)
+        msg = pt
+        msg.coordinate_frame = PT.FRAME_LOCAL_NED
+        msg.type_mask = PT.IGNORE_VX + PT.IGNORE_VY + PT.IGNORE_VZ + \
+                        PT.IGNORE_AFX + PT.IGNORE_AFY + PT.IGNORE_AFZ + \
+                        (PT.IGNORE_YAW if yaw_rate_flag else PT.IGNORE_YAW_RATE)
+        msg.position = setpoint
+        msg.yaw = euler_from_orientation(current_nav_finish.pose.orientation, 'sxyz')[2]
+        msg.yaw_rate = req.yaw_rate
+        return position_pub, msg
+
+    elif isinstance(req, (srv.SetPositionRequest, srv.SetPositionGlobalRequest)):
+        ps.header.frame_id = req.frame_id or LOCAL_FRAME
+        ps.pose.position = Point(getattr(req, 'x', 0), getattr(req, 'y', 0), req.z)
+        ps.pose.orientation = orientation_from_euler(0, 0, req.yaw)
+        pose_local = tf_buffer.transform(ps, LOCAL_FRAME, transform_timeout)
+
+        if isinstance(req, srv.SetPositionGlobalRequest):
+            pose_local.pose.position.x, pose_local.pose.position.y = global_to_local(req.lat, req.lon)
+
+        yaw_rate_flag = math.isnan(req.yaw)
+        msg = pt
+        msg.coordinate_frame = PT.FRAME_LOCAL_NED
+        msg.type_mask = PT.IGNORE_VX + PT.IGNORE_VY + PT.IGNORE_VZ + \
+                        PT.IGNORE_AFX + PT.IGNORE_AFY + PT.IGNORE_AFZ + \
+                        (PT.IGNORE_YAW if yaw_rate_flag else PT.IGNORE_YAW_RATE)
+        msg.position = pose_local.pose.position
+        msg.yaw = euler_from_orientation(pose_local.pose.orientation, 'sxyz')[2]
+        msg.yaw_rate = req.yaw_rate
+        return position_pub, msg
+
+    elif isinstance(req, srv.SetVelocityRequest):
+        vs.vector = Vector3(req.vx, req.vy, req.vz)
+        vs.header.frame_id = req.frame_id or LOCAL_FRAME
+        ps.header.frame_id = req.frame_id or LOCAL_FRAME
+        ps.pose.orientation = orientation_from_euler(0, 0, req.yaw)
+        pose_local = tf_buffer.transform(ps, LOCAL_FRAME, transform_timeout)
+        vector_local = tf_buffer.transform(vs, LOCAL_FRAME, transform_timeout)
+
+        yaw_rate_flag = math.isnan(req.yaw)
+        msg = pt
+        msg.coordinate_frame = PT.FRAME_LOCAL_NED
+        msg.type_mask = PT.IGNORE_PX + PT.IGNORE_PY + PT.IGNORE_PZ + \
+                                       PT.IGNORE_AFX + PT.IGNORE_AFY + PT.IGNORE_AFZ + \
+                                       (PT.IGNORE_YAW if yaw_rate_flag else PT.IGNORE_YAW_RATE)
+        msg.velocity = vector_local.vector
+        msg.yaw = euler_from_orientation(pose_local.pose.orientation, 'sxyz')[2]
+        msg.yaw_rate = req.yaw_rate
+        return position_pub, msg
+
+    elif isinstance(req, srv.SetAttitudeRequest):
+        ps.header.frame_id = req.frame_id or LOCAL_FRAME
+        ps.pose.orientation = orientation_from_euler(req.roll, req.pitch, req.yaw)
+        pose_local = tf_buffer.transform(ps, LOCAL_FRAME, transform_timeout)
+        msg = at
+        msg.orientation = pose_local.pose.orientation
+        msg.thrust = req.thrust
+        msg.type_mask = AT.IGNORE_YAW_RATE + AT.IGNORE_PITCH_RATE + AT.IGNORE_ROLL_RATE
+        return attitude_pub, msg
+
+    elif isinstance(req, srv.SetRatesRequest):
+        msg = at
+        msg.thrust = req.thrust
+        msg.type_mask = AT.IGNORE_ATTITUDE
+        msg.body_rate.x = req.roll_rate
+        msg.body_rate.y = req.pitch_rate
+        msg.body_rate.z = req.yaw_rate
+        return attitude_pub, msg
+
+
+current_pub = None
+current_msg = None
+current_req = None
+current_nav_start = None
+current_nav_finish = None
+current_nav_start_stamp = None
+handle_lock = Lock()
+
+
+def handle(req):
+    global current_pub, current_msg, current_req
+
+    if not state or not state.connected:
+        rospy.logwarn('No connection to the FCU')
+        return {'message': 'No connection to the FCU'}
+
+    if isinstance(req, (srv.NavigateRequest, srv.NavigateGlobalRequest)):
+        if req.speed < 0:
+            rospy.logwarn('Navigate speed must be positive, %s passed')
+            return {'message': 'Navigate speed must be positive, %s passed' % req.speed}
+        elif req.speed == 0:
+            req.speed = DEFAULT_SPEED
+
+    if isinstance(req, (srv.NavigateRequest, srv.NavigateGlobalRequest)) and \
+            (pose is None or rospy.get_rostime() - pose.header.stamp > LOCAL_POSITION_TIMEOUT):
+        rospy.logwarn('No local position')
+        return {'message': 'No local position'}
+
+    if getattr(req, 'yaw_rate', 0) != 0 and not math.isnan(getattr(req, 'yaw')):
+        rospy.logwarn('Yaw value should be NaN for setting yaw rate')
+        return {'message': 'Yaw value should be NaN for setting yaw rate'}
+
+    if math.isnan(getattr(req, 'yaw', 0)) and math.isnan(getattr(req, 'yaw_rate', 0)):
+        rospy.logwarn('Both yaw and yaw_rate cannot be NaN')
+        return {'message': 'Both yaw and yaw_rate cannot be NaN'}
+
+    try:
+        # check frame_id existance
+        # (for non-blocking setpoint's publishing in get_publisher_and_message)
+        stamp = rospy.get_rostime()
+        if hasattr(req, 'frame_id'):
+            tf_buffer.lookup_transform(req.frame_id or LOCAL_FRAME, LOCAL_FRAME, stamp, TRANSFORM_TIMEOUT)
+
+        with handle_lock:
+            current_req = req
+            current_pub, current_msg = get_publisher_and_message(req, stamp, False)
+            rospy.loginfo('Topic: %s, message: %s', current_pub.name, current_msg)
+
+            current_msg.header.stamp = stamp
+            current_pub.publish(current_msg)
+
+        if req.auto_arm:
+            offboard_and_arm()
+        else:
+            if state.mode != 'OFFBOARD':
+                return {'message': 'Copter is not in OFFBOARD mode, use auto_arm?'}
+            if not state.armed:
+                return {'message': 'Copter is not armed, use auto_arm?'}
+
+        return {'success': True}
+
+    except Exception as e:
+        rospy.logerr(str(e))
+        return {'success': False, 'message': str(e)}
+
+
+def land(req):
+    if not state or not state.connected:
+        rospy.logwarn('No connection to the FCU')
+        return {'message': 'No connection to the FCU'}
+
+    rospy.loginfo('Set %s mode', LAND_MODE)
+    res = set_mode(custom_mode=LAND_MODE)
+    if not res.mode_sent:
+        return {'message': 'Cannot send %s mode request' % LAND_MODE}
+
+    start = rospy.get_rostime()
+    while True:
+        if state.mode == LAND_MODE:
+            return {'success': True}
+        if rospy.get_rostime() - start > LAND_TIMEOUT:
+            return {'message': '%s mode request timed out' % LAND_MODE}
+        rospy.sleep(0.1)
+
+
+def release(req):
+    global current_pub
+    current_pub = None
+    rospy.loginfo('simple_offboard: release')
+    return {'success': True}
+
+
+rospy.Service('navigate', srv.Navigate, handle)
+rospy.Service('navigate_global', srv.NavigateGlobal, handle)
+rospy.Service('set_position', srv.SetPosition, handle)
+rospy.Service('set_position_global', srv.SetPositionGlobal, handle)
+rospy.Service('set_velocity', srv.SetVelocity, handle)
+rospy.Service('set_attitude', srv.SetAttitude, handle)
+rospy.Service('set_rates', srv.SetRates, handle)
+rospy.Service('land', Trigger, land)
+rospy.Service('release', Trigger, release)
+
+
+def get_telemetry(req):
+    res = {
+        'frame_id': req.frame_id or LOCAL_FRAME,
+        'x': float('nan'),
+        'y': float('nan'),
+        'z': float('nan'),
+        'lat': float('nan'),
+        'lon': float('nan'),
+        'alt': float('nan'),
+        'vx': float('nan'),
+        'vy': float('nan'),
+        'vz': float('nan'),
+        'pitch': float('nan'),
+        'roll': float('nan'),
+        'yaw': float('nan'),
+        'pitch_rate': float('nan'),
+        'roll_rate': float('nan'),
+        'yaw_rate': float('nan'),
+        'voltage': float('nan'),
+        'cell_voltage': float('nan')
+    }
+    frame_id = req.frame_id or LOCAL_FRAME
+    stamp = rospy.get_rostime()
+
+    transform_timeout = rospy.Duration(0.4)
+    try:
+        if pose:
+            p = tf_buffer.transform(pose, frame_id, transform_timeout)
+            res['x'] = p.pose.position.x
+            res['y'] = p.pose.position.y
+            res['z'] = p.pose.position.z
+
+            # Calculate roll pitch and yaw as Tait-Bryan angles, order z-y-x
+            res['yaw'], res['pitch'], res['roll'] = euler_from_orientation(p.pose.orientation, axes='rzyx')
+    except:
+        pass
+
+    if velocity:
+        try:
+            v = Vector3Stamped()
+            v.header.stamp = velocity.header.stamp
+            v.header.frame_id = velocity.header.frame_id
+            v.vector = velocity.twist.linear
+            linear = tf_buffer.transform(v, frame_id, transform_timeout)
+            res['vx'] = linear.vector.x
+            res['vy'] = linear.vector.y
+            res['vz'] = linear.vector.z
+        except:
+            pass
+
+        res['yaw_rate'] = velocity.twist.angular.z
+        res['pitch_rate'] = velocity.twist.angular.y
+        res['roll_rate'] = velocity.twist.angular.x
+
+    if global_position and stamp - global_position.header.stamp < rospy.Duration(5):
+        res['lat'] = global_position.latitude
+        res['lon'] = global_position.longitude
+        res['alt'] = global_position.altitude
+
+    if state:
+        res['connected'] = state.connected
+        res['armed'] = state.armed
+        res['mode'] = state.mode
+
+    if battery:
+        res['voltage'] = battery.voltage
+        try:
+            res['cell_voltage'] = battery.cell_voltage[0]
+        except:
+            pass
+
+    return res
+
+
+rospy.Service('get_telemetry', srv.GetTelemetry, get_telemetry)
+
+
+rospy.loginfo('simple_offboard inited')
+
+
+def start_loop():
+    global current_pub, current_msg, current_req
+    r = rospy.Rate(SETPOINT_RATE)
+
+    while not rospy.is_shutdown():
+        with handle_lock:
+            if current_pub is not None:
+                try:
+                    stamp = rospy.get_rostime()
+
+                    if getattr(current_req, 'update_frame', False) or \
+                            isinstance(current_req, (srv.NavigateRequest, srv.NavigateGlobalRequest)):
+                        current_pub, current_msg = get_publisher_and_message(current_req, stamp, True,
+                                                                             getattr(current_req, 'update_frame', False))
+
+                except Exception as e:
+                    rospy.logwarn_throttle(10, str(e))
+
+                current_msg.header.stamp = stamp
+                current_pub.publish(current_msg)
+
+                # For monitoring
+                if isinstance(current_msg, PositionTarget):
+                    p = PoseStamped()
+                    p.header.frame_id = LOCAL_FRAME
+                    p.header.stamp = stamp
+                    p.pose.position = current_msg.position
+                    p.pose.orientation = orientation_from_euler(0, 0, current_msg.yaw)
+                    target_pub.publish(p)
+
+        r.sleep()
+
+
+start_loop()

clever/src/utils.h

@@ -0,0 +1,36 @@
+#pragma once
+// TODO: merge with util.h
+
+#include <string>
+#include <ros/ros.h>
+#include <geometry_msgs/Vector3.h>
+#include <geometry_msgs/Point.h>
+
+// Read required param or shutdown the node
+template<typename T>
+static void param(ros::NodeHandle nh, const std::string& param_name, T& param_val)
+{
+	if (!nh.getParam(param_name, param_val)) {
+		ROS_FATAL("Required param %s is not defined", param_name);
+		ros::shutdown();
+	}
+}
+
+static inline double hypot(double x, double y, double z)
+{
+	return std::sqrt(x*x + y*y + z*z);
+}
+
+static inline void vectorToPoint(const geometry_msgs::Vector3& vector, geometry_msgs::Point& point)
+{
+	point.x = vector.x;
+	point.y = vector.y;
+	point.z = vector.z;
+}
+
+static inline void pointToVector(const geometry_msgs::Point& point, geometry_msgs::Vector3& vector)
+{
+	vector.x = point.x;
+	vector.y = point.y;
+	vector.z = point.z;
+}