docs: some info on optical flow troubleshooting

Fixed broken links

.editorconfig

@@ -0,0 +1,13 @@
+root = true
+
+[*]
+end_of_line = lf
+insert_final_newline = true
+charset = utf-8
+
+[*.{py,swift,launch}]
+indent_style = space
+indent_size = 4
+
+[*.{js,html}]
+indent_style = tab

.gitignore

@@ -1,2 +1,3 @@
-/deploy/ros_lib/
 *.pyc
+*.DS_Store
+/images

.markdownlint.json

@@ -0,0 +1,22 @@
+{
+    "MD003": false,
+    "MD013": false,
+    "MD033": false,
+    "MD034": false,
+    "MD044": {
+        "names": [
+            "MAVLink",
+            "ROS",
+            "Python",
+            "C++",
+            "PX4",
+            "WireShark",
+            "Wi-Fi",
+            "Raspberry Pi",
+            "Pixhawk",
+            "Pixracer",
+            "ArUco"
+        ],
+        "code_blocks": false
+    }
+}

.travis.yml

@@ -0,0 +1,35 @@
+sudo: required
+language: generic
+services:
+  - docker
+env:
+  global:
+    - DOCKER="smirart/img-tool:v0.1"
+    - TARGET_REPO="https://github.com/${TRAVIS_REPO_SLUG}.git"
+    - if [[ -z ${TRAVIS_TAG} ]]; then IMAGE_VERSION="${TRAVIS_COMMIT}}"; else IMAGE_VERSION="${TRAVIS_TAG}"; fi
+    - IMAGE_NAME="$(basename -s '.git' ${TARGET_REPO})_${IMAGE_VERSION}.img"
+git:
+  depth: 1
+before_script:
+  - docker pull ${DOCKER}
+script:
+  - docker run --privileged --rm -v /dev:/dev -v $(pwd):/builder/repo -e TRAVIS_TAG="${TRAVIS_TAG}" ${DOCKER}
+before_deploy:
+  # Set up git user name and tag this commit
+  - git config --local user.name "urpylka"
+  - git config --local user.email "urpylka@gmail.com"
+  - sudo chmod -R 777 *
+  - cd images && zip ${IMAGE_NAME}.zip ${IMAGE_NAME}
+deploy:
+  provider: releases
+  api_key: ${GITHUB_OAUTH_TOKEN}
+  file: ${IMAGE_NAME}.zip
+  skip_cleanup: true
+  on:
+    tags: true
+
+# More info there
+# https://github.com/travis-ci/travis-ci/issues/6893
+# https://docs.travis-ci.com/user/customizing-the-build/
+# https://docs.travis-ci.com/user/deployment/releases
+# https://docs.travis-ci.com/user/environment-variables/

LICENSE.md

@@ -1,6 +1,6 @@
 MIT License
 
-Copyright (c) 2018 Copter Express
+Copyright (c) 2018 Copter Express Technologies
 
 Permission is hereby granted, free of charge, to any person obtaining a copy
 of this software and associated documentation files (the "Software"), to deal

README.md

@@ -4,16 +4,17 @@
 
 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 [Robokross-2016](https://www.youtube.com/watch?v=dGbDaz_VmYU) and [Robokross-2017](https://youtu.be/AQnd2CRczbQ) competitions and many others.
+Copter Express has implemented a large number of different autonomous drone projects using exactly the same platform: [automated pizza delivery](https://www.youtube.com/watch?v=hmkAoZOtF58) in Samara and Kazan, coffee delivery in Skolkovo Innovation Center, [autonomous quadcopter with charging station](https://www.youtube.com/watch?v=RjX6nUqw1mI) for site monitoring and security, winning drones on [Robocross-2016](https://www.youtube.com/watch?v=dGbDaz_VmYU) and [Robocross-2017](https://youtu.be/AQnd2CRczbQ) competitions and many others.
 
-**The main documentation in Russian is available on our Gitbook:**
+**The main documentation in Russian is available [on our Gitbook](https://clever.copterexpress.com/).**
-**https://copterexpress.gitbooks.io/clever/content/**
 
 Use it to learn how to assemble, configure, pilot and program autonomous CLEVER drone.
 
 ## Preconfigured RPi 3 image
 
-Preconfigured image for Raspberry Pi 3 with installed and configured software, ready to fly, is available [here](https://copterexpress.gitbooks.io/clever/content/docs/microsd_images.html).
+**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/urpylka/clever.svg?branch=master)](https://travis-ci.org/urpylka/clever)
 
 Image includes:
 
@@ -24,7 +25,7 @@ Image includes:
 * mavros
 * CLEVER software bundle for autonomous drone control
 
-API description in Russian for autonomous flights is available [here](https://copterexpress.gitbooks.io/clever/simple_offboard.html).
+API description (in Russian) for autonomous flights is available [on GitBook](https://copterexpress.gitbooks.io/clever/simple_offboard.html).
 
 ## Manual installation
 

_config.yml

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

apps/ios/Podfile

@@ -9,6 +9,5 @@ target 'cleverrc' do
 
   # Pods for cleverrc
   pod 'SwiftSocket', '~> 2.0'
-  pod 'NotificationBannerSwift'
 
 end

apps/ios/Podfile.lock

@@ -1,21 +1,12 @@
 PODS:
-  - MarqueeLabel/Swift (3.1.4)
-  - NotificationBannerSwift (1.5.4):
-    - MarqueeLabel/Swift
-    - SnapKit (~> 4.0)
-  - SnapKit (4.0.0)
   - SwiftSocket (2.0.2)
 
 DEPENDENCIES:
-  - NotificationBannerSwift
   - SwiftSocket (~> 2.0)
 
 SPEC CHECKSUMS:
-  MarqueeLabel: bf768455fe88d427f71476ebb23f9092b660f40b
-  NotificationBannerSwift: 4f6666c8421dcf11be0812dd1093d932c15921af
-  SnapKit: a42d492c16e80209130a3379f73596c3454b7694
   SwiftSocket: 6f4c9c63fbc5c1d61188936bb3c599fd546f40ae
 
-PODFILE CHECKSUM: fd5199f69c3ee8c1fbc0dd582477d890c8b2a24f
+PODFILE CHECKSUM: 2044f57d00f536792fbc38c63ded4fa78dcc135c
 
 COCOAPODS: 1.4.0

apps/ios/cleverrc.xcodeproj/project.pbxproj

@@ -139,7 +139,7 @@
 			isa = PBXProject;
 			attributes = {
 				LastSwiftUpdateCheck = 0920;
-				LastUpgradeCheck = 0920;
+				LastUpgradeCheck = 0930;
 				ORGANIZATIONNAME = "Copter Express";
 				TargetAttributes = {
 					7C51653C20139237004D1F4D = {
@@ -226,16 +226,10 @@
 			);
 			inputPaths = (
 				"${SRCROOT}/Pods/Target Support Files/Pods-cleverrc/Pods-cleverrc-frameworks.sh",
-				"${BUILT_PRODUCTS_DIR}/MarqueeLabel/MarqueeLabel.framework",
-				"${BUILT_PRODUCTS_DIR}/NotificationBannerSwift/NotificationBannerSwift.framework",
-				"${BUILT_PRODUCTS_DIR}/SnapKit/SnapKit.framework",
 				"${BUILT_PRODUCTS_DIR}/SwiftSocket/SwiftSocket.framework",
 			);
 			name = "[CP] Embed Pods Frameworks";
 			outputPaths = (
-				"${TARGET_BUILD_DIR}/${FRAMEWORKS_FOLDER_PATH}/MarqueeLabel.framework",
-				"${TARGET_BUILD_DIR}/${FRAMEWORKS_FOLDER_PATH}/NotificationBannerSwift.framework",
-				"${TARGET_BUILD_DIR}/${FRAMEWORKS_FOLDER_PATH}/SnapKit.framework",
 				"${TARGET_BUILD_DIR}/${FRAMEWORKS_FOLDER_PATH}/SwiftSocket.framework",
 			);
 			runOnlyForDeploymentPostprocessing = 0;
@@ -294,6 +288,7 @@
 				CLANG_WARN_BOOL_CONVERSION = YES;
 				CLANG_WARN_COMMA = YES;
 				CLANG_WARN_CONSTANT_CONVERSION = YES;
+				CLANG_WARN_DEPRECATED_OBJC_IMPLEMENTATIONS = YES;
 				CLANG_WARN_DIRECT_OBJC_ISA_USAGE = YES_ERROR;
 				CLANG_WARN_DOCUMENTATION_COMMENTS = YES;
 				CLANG_WARN_EMPTY_BODY = YES;
@@ -301,6 +296,7 @@
 				CLANG_WARN_INFINITE_RECURSION = YES;
 				CLANG_WARN_INT_CONVERSION = YES;
 				CLANG_WARN_NON_LITERAL_NULL_CONVERSION = YES;
+				CLANG_WARN_OBJC_IMPLICIT_RETAIN_SELF = YES;
 				CLANG_WARN_OBJC_LITERAL_CONVERSION = YES;
 				CLANG_WARN_OBJC_ROOT_CLASS = YES_ERROR;
 				CLANG_WARN_RANGE_LOOP_ANALYSIS = YES;
@@ -351,6 +347,7 @@
 				CLANG_WARN_BOOL_CONVERSION = YES;
 				CLANG_WARN_COMMA = YES;
 				CLANG_WARN_CONSTANT_CONVERSION = YES;
+				CLANG_WARN_DEPRECATED_OBJC_IMPLEMENTATIONS = YES;
 				CLANG_WARN_DIRECT_OBJC_ISA_USAGE = YES_ERROR;
 				CLANG_WARN_DOCUMENTATION_COMMENTS = YES;
 				CLANG_WARN_EMPTY_BODY = YES;
@@ -358,6 +355,7 @@
 				CLANG_WARN_INFINITE_RECURSION = YES;
 				CLANG_WARN_INT_CONVERSION = YES;
 				CLANG_WARN_NON_LITERAL_NULL_CONVERSION = YES;
+				CLANG_WARN_OBJC_IMPLICIT_RETAIN_SELF = YES;
 				CLANG_WARN_OBJC_LITERAL_CONVERSION = YES;
 				CLANG_WARN_OBJC_ROOT_CLASS = YES_ERROR;
 				CLANG_WARN_RANGE_LOOP_ANALYSIS = YES;
@@ -393,7 +391,7 @@
 			buildSettings = {
 				ASSETCATALOG_COMPILER_APPICON_NAME = AppIcon;
 				CODE_SIGN_STYLE = Automatic;
-				DEVELOPMENT_TEAM = 7QY6KJ2672;
+				DEVELOPMENT_TEAM = M8TDN3PAH2;
 				INFOPLIST_FILE = cleverrc/Info.plist;
 				LD_RUNPATH_SEARCH_PATHS = "$(inherited) @executable_path/Frameworks";
 				PRODUCT_BUNDLE_IDENTIFIER = coex.cleverrc;
@@ -409,7 +407,7 @@
 			buildSettings = {
 				ASSETCATALOG_COMPILER_APPICON_NAME = AppIcon;
 				CODE_SIGN_STYLE = Automatic;
-				DEVELOPMENT_TEAM = 7QY6KJ2672;
+				DEVELOPMENT_TEAM = M8TDN3PAH2;
 				INFOPLIST_FILE = cleverrc/Info.plist;
 				LD_RUNPATH_SEARCH_PATHS = "$(inherited) @executable_path/Frameworks";
 				PRODUCT_BUNDLE_IDENTIFIER = coex.cleverrc;

apps/ios/cleverrc.xcworkspace/xcshareddata/IDEWorkspaceChecks.plist

@@ -0,0 +1,8 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE plist PUBLIC "-//Apple//DTD PLIST 1.0//EN" "http://www.apple.com/DTDs/PropertyList-1.0.dtd">
+<plist version="1.0">
+<dict>
+	<key>IDEDidComputeMac32BitWarning</key>
+	<true/>
+</dict>
+</plist>

apps/ios/cleverrc/AppDelegate.swift

@@ -3,7 +3,7 @@
 //  cleverrc
 //
 //  Created by Oleg Kalachev on 20.01.2018.
-//  Copyright © 2018 Copter Express. All rights reserved.
+//  Copyright © 2018 Copter Express Technologies. All rights reserved.
 //
 
 import UIKit

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

@@ -1,94 +1,169 @@
 {
   "images" : [
     {
-      "idiom" : "iphone",
       "size" : "20x20",
+      "idiom" : "iphone",
+      "filename" : "Icon-App-20x20@2x.png",
       "scale" : "2x"
     },
     {
-      "idiom" : "iphone",
       "size" : "20x20",
+      "idiom" : "iphone",
+      "filename" : "Icon-App-20x20@3x.png",
       "scale" : "3x"
     },
     {
-      "idiom" : "iphone",
       "size" : "29x29",
+      "idiom" : "iphone",
+      "filename" : "Icon-App-29x29@2x.png",
       "scale" : "2x"
     },
     {
-      "idiom" : "iphone",
       "size" : "29x29",
+      "idiom" : "iphone",
+      "filename" : "Icon-App-29x29@3x.png",
       "scale" : "3x"
     },
     {
-      "idiom" : "iphone",
       "size" : "40x40",
+      "idiom" : "iphone",
+      "filename" : "Icon-App-40x40@2x.png",
       "scale" : "2x"
     },
     {
-      "idiom" : "iphone",
       "size" : "40x40",
+      "idiom" : "iphone",
+      "filename" : "Icon-App-40x40@3x.png",
       "scale" : "3x"
     },
     {
       "size" : "60x60",
       "idiom" : "iphone",
-      "filename" : "cleverios180.png",
+      "filename" : "Icon-App-60x60@2x.png",
       "scale" : "2x"
     },
     {
       "size" : "60x60",
       "idiom" : "iphone",
-      "filename" : "cleverios180-1.png",
+      "filename" : "Icon-App-60x60@3x.png",
       "scale" : "3x"
     },
     {
-      "idiom" : "ipad",
       "size" : "20x20",
+      "idiom" : "ipad",
+      "filename" : "Icon-App-20x20@1x.png",
       "scale" : "1x"
     },
     {
-      "idiom" : "ipad",
       "size" : "20x20",
+      "idiom" : "ipad",
+      "filename" : "Icon-App-20x20@2x-1.png",
       "scale" : "2x"
     },
     {
-      "idiom" : "ipad",
       "size" : "29x29",
+      "idiom" : "ipad",
+      "filename" : "Icon-App-29x29@1x.png",
       "scale" : "1x"
     },
     {
-      "idiom" : "ipad",
       "size" : "29x29",
+      "idiom" : "ipad",
+      "filename" : "Icon-App-29x29@2x-1.png",
       "scale" : "2x"
     },
     {
-      "idiom" : "ipad",
       "size" : "40x40",
+      "idiom" : "ipad",
+      "filename" : "Icon-App-40x40@1x.png",
       "scale" : "1x"
     },
     {
-      "idiom" : "ipad",
       "size" : "40x40",
+      "idiom" : "ipad",
+      "filename" : "Icon-App-40x40@2x-1.png",
       "scale" : "2x"
     },
     {
-      "idiom" : "ipad",
       "size" : "76x76",
+      "idiom" : "ipad",
+      "filename" : "Icon-App-76x76@1x.png",
       "scale" : "1x"
     },
     {
-      "idiom" : "ipad",
       "size" : "76x76",
+      "idiom" : "ipad",
+      "filename" : "Icon-App-76x76@2x.png",
       "scale" : "2x"
     },
     {
-      "idiom" : "ipad",
       "size" : "83.5x83.5",
+      "idiom" : "ipad",
+      "filename" : "Icon-App-83.5x83.5@2x.png",
       "scale" : "2x"
     },
     {
+      "size" : "1024x1024",
       "idiom" : "ios-marketing",
+      "filename" : "ItunesArtwork@2x.png",
+      "scale" : "1x"
+    },
+    {
+      "size" : "24x24",
+      "idiom" : "watch",
+      "scale" : "2x",
+      "role" : "notificationCenter",
+      "subtype" : "38mm"
+    },
+    {
+      "size" : "27.5x27.5",
+      "idiom" : "watch",
+      "scale" : "2x",
+      "role" : "notificationCenter",
+      "subtype" : "42mm"
+    },
+    {
+      "size" : "29x29",
+      "idiom" : "watch",
+      "role" : "companionSettings",
+      "scale" : "2x"
+    },
+    {
+      "size" : "29x29",
+      "idiom" : "watch",
+      "role" : "companionSettings",
+      "scale" : "3x"
+    },
+    {
+      "size" : "40x40",
+      "idiom" : "watch",
+      "scale" : "2x",
+      "role" : "appLauncher",
+      "subtype" : "38mm"
+    },
+    {
+      "size" : "44x44",
+      "idiom" : "watch",
+      "scale" : "2x",
+      "role" : "longLook",
+      "subtype" : "42mm"
+    },
+    {
+      "size" : "86x86",
+      "idiom" : "watch",
+      "scale" : "2x",
+      "role" : "quickLook",
+      "subtype" : "38mm"
+    },
+    {
+      "size" : "98x98",
+      "idiom" : "watch",
+      "scale" : "2x",
+      "role" : "quickLook",
+      "subtype" : "42mm"
+    },
+    {
+      "idiom" : "watch-marketing",
       "size" : "1024x1024",
       "scale" : "1x"
     }
@@ -96,5 +171,8 @@
   "info" : {
     "version" : 1,
     "author" : "xcode"
+  },
+  "properties" : {
+    "pre-rendered" : true
   }
 }

apps/ios/cleverrc/Assets.xcassets/Image.imageset/Contents.json

@@ -1,20 +0,0 @@
-{
-  "images" : [
-    {
-      "idiom" : "universal",
-      "scale" : "1x"
-    },
-    {
-      "idiom" : "universal",
-      "scale" : "2x"
-    },
-    {
-      "idiom" : "universal",
-      "scale" : "3x"
-    }
-  ],
-  "info" : {
-    "version" : 1,
-    "author" : "xcode"
-  }
-}

apps/ios/cleverrc/Info.plist

@@ -5,7 +5,7 @@
 	<key>CFBundleDevelopmentRegion</key>
 	<string>$(DEVELOPMENT_LANGUAGE)</string>
 	<key>CFBundleDisplayName</key>
-	<string>Clever RC</string>
+	<string>CLEVER RC</string>
 	<key>CFBundleExecutable</key>
 	<string>$(EXECUTABLE_NAME)</string>
 	<key>CFBundleIdentifier</key>
@@ -17,9 +17,9 @@
 	<key>CFBundlePackageType</key>
 	<string>APPL</string>
 	<key>CFBundleShortVersionString</key>
-	<string>1.0</string>
+	<string>1.1</string>
 	<key>CFBundleVersion</key>
-	<string>1</string>
+	<string>6</string>
 	<key>LSRequiresIPhoneOS</key>
 	<true/>
 	<key>UILaunchStoryboardName</key>

apps/ios/cleverrc/ViewController.swift

@@ -3,19 +3,19 @@
 //  cleverrc
 //
 //  Created by Oleg Kalachev on 20.01.2018.
-//  Copyright © 2018 Copter Express. All rights reserved.
+//  Copyright © 2018 Copter Express Technologies. All rights reserved.
 //
 
 import UIKit
 import WebKit
 import SwiftSocket
-import NotificationBannerSwift
+import AudioToolbox.AudioServices
 
 class ViewController: UIViewController, WKScriptMessageHandler {
     @IBOutlet weak var webView: WKWebView!
     let impactGenerator = UIImpactFeedbackGenerator(style: .medium)
     let notificationGenerator = UINotificationFeedbackGenerator()
-    let udpSocket = UDPClient(address:"255.255.255.255", port: 35602)
+    let udpSocket = UDPClient(address: "255.255.255.255", port: 35602)
 
     override func viewDidLoad() {
         super.viewDidLoad()
@@ -24,6 +24,7 @@ class ViewController: UIViewController, WKScriptMessageHandler {
         UIApplication.shared.isIdleTimerDisabled = true
 
         // Setup webview event handlers
+        webView.scrollView.bounces = false;
         webView.configuration.userContentController.add(self, name: "control")
         webView.configuration.userContentController.add(self, name: "controlStart")
         webView.configuration.userContentController.add(self, name: "lowBattery")
@@ -56,18 +57,28 @@ class ViewController: UIViewController, WKScriptMessageHandler {
         } else if (message.name == "lowBattery") {
             // Got low battery notification
             print("Low battery notification")
-            notificationGenerator.notificationOccurred(.warning)
+            tapticNotify()
         } else if (message.name == "notification") {
             // Got notification message
             print(message)
-            let m = message.body as! NSDictionary;
+            tapticNotify()
-            let level = m["level"] as! Int
+        }
-            if level == 4 {
+    }
-                let banner = NotificationBanner(title: m["msg"] as! String, style: .warning)
+
-                banner.show()
+    func tapticNotify() {
-            } else {
+        if let feedbackSupportLevel = UIDevice.current.value(forKey: "_feedbackSupportLevel") as? Int {
-                let banner = NotificationBanner(title: m["msg"] as! String, style: .danger)
+            switch feedbackSupportLevel {
-                banner.show()
+            case 2:
+                // 2nd Generation Taptic Engine w/ Haptic Feedback (iPhone 7/7+)
+                notificationGenerator.notificationOccurred(.warning)
+            case 1:
+                // 1st Generation Taptic Engine (iPhone 6S/6S+)
+                let peek = SystemSoundID(1519)
+                AudioServicesPlaySystemSound(peek)
+            case 0:
+                // No Taptic Engine
+                break
+            default: break
             }
         }
     }

apps/ios/cleverrc/clever.svg

@@ -1,84 +1,57 @@
-<?xml version="1.0" encoding="utf-8"?>
+<?xml version="1.0" encoding="UTF-8" standalone="no"?>
-<svg version="1.1" id="Layer_1" xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" x="0px" y="0px"
+<svg
-	 viewBox="0 0 52.5 52.5" enable-background="new 0 0 52.5 52.5" xml:space="preserve">
+   xmlns:dc="http://purl.org/dc/elements/1.1/"
-<g>
+   xmlns:cc="http://creativecommons.org/ns#"
-	<path fill="white" fill-opacity="0.5" d="M35.7,17.2c0.3-0.3,0.5-0.7,0.7-1.1c0.2-0.4,0.3-0.9,0.3-1.4c0-0.5-0.1-1-0.3-1.4c-0.2-0.5-0.5-0.9-0.8-1.3
+   xmlns:rdf="http://www.w3.org/1999/02/22-rdf-syntax-ns#"
-		c-0.4-0.4-0.9-0.7-1.4-0.9c-0.6-0.2-1.2-0.3-1.8-0.2c-0.5,0.1-0.9,0.2-1.3,0.4c-0.2,0.1-0.4,0.3-0.6,0.4L30.2,12
+   xmlns:svg="http://www.w3.org/2000/svg"
-		c-0.1,0.1-0.1,0.1-0.2,0.1c-0.1,0.1-0.3,0.2-0.4,0.2c-0.1,0.1-0.3,0.1-0.4,0.2c-0.1,0-0.2,0.1-0.4,0.1c-0.1,0-0.2,0-0.3,0
+   xmlns="http://www.w3.org/2000/svg"
-		c-0.6,0.1-1.3-0.1-1.9-0.4c-0.2-0.1-0.4-0.2-0.6-0.4c0,0-0.1-0.1-0.1-0.1c0,0-0.1-0.1-0.1-0.1c0,0,0,0-0.1-0.1
+   viewBox="0 0 69.988266 69.987198"
-		c-0.1-0.1-0.1-0.2-0.1-0.3c0,0,0,0,0-0.1l0-6.5c-0.5-0.6-1.1-1.1-1.7-1.6c-0.9-0.7-1.9-1.4-3-1.8c-0.9-0.4-1.8-0.7-2.7-1
+   height="69.987198"
-		c-0.9-0.2-1.7-0.3-2.6-0.4c-0.9-0.1-1.8,0-2.7,0.1c-0.7,0.1-1.5,0.2-2.2,0.5C9.4,0.9,8.1,1.5,7,2.2C6.2,2.7,5.4,3.3,4.7,3.9
+   width="69.988266"
-		C4,4.6,3.4,5.3,2.8,6c-0.5,0.7-1,1.4-1.3,2.2C1.1,9,0.8,9.8,0.6,10.6c-0.2,0.7-0.4,1.4-0.4,2.1C0,13.7,0,14.6,0,15.6
+   xml:space="preserve"
-		c0,0.8,0.2,1.7,0.4,2.5c0.2,0.9,0.5,1.7,0.8,2.5C1.8,21.8,2.5,23,3.4,24c0.3,0.4,0.7,0.8,1,1.1c0.2,0.2,0.3,0.3,0.5,0.5H11
+   id="svg2"
-		c0.1-0.1,0.1-0.2,0.1-0.2c0-0.1,0.1-0.1,0.1-0.2c0-0.1,0.1-0.1,0.1-0.2c0.1-0.4,0.2-0.8,0.1-1.1c0-0.1,0-0.1,0-0.2
+   version="1.1"><metadata
-		c0-0.1-0.1-0.1-0.1-0.2c0-0.1-0.1-0.2-0.2-0.2c0,0-0.1-0.1-0.1-0.1l0,0c0,0,0-0.1-0.1-0.1c0,0,0,0,0,0l-0.1-0.1
+     id="metadata8"><rdf:RDF><cc:Work
-		c-0.1-0.1-0.2-0.3-0.3-0.4c-0.2-0.3-0.4-0.7-0.5-1c-0.1-0.3-0.2-0.7-0.3-1c-0.1-0.6-0.1-1.1,0-1.7c0.1-0.5,0.3-1,0.5-1.5
+         rdf:about=""><dc:format>image/svg+xml</dc:format><dc:type
-		c0.3-0.5,0.6-0.9,1-1.3c0.4-0.4,0.9-0.7,1.4-0.9c0.6-0.2,1.2-0.4,1.8-0.4c0.7,0,1.3,0.1,1.9,0.3c0.7,0.2,1.3,0.6,1.8,1.1
+           rdf:resource="http://purl.org/dc/dcmitype/StillImage" /></cc:Work></rdf:RDF></metadata><defs
-		c0.5,0.5,1,1.1,1.3,1.8c0.2,0.4,0.3,0.7,0.3,1.1c0.1,0.4,0.1,0.8,0.1,1.3c0,0.4-0.1,0.8-0.2,1.1c-0.1,0.3-0.3,0.7-0.4,1
+     id="defs6"><clipPath
-		c-0.1,0.2-0.2,0.3-0.3,0.5c0,0.1-0.1,0.1-0.1,0.2l0,0l0,0l0,0.1c0,0,0,0.1-0.1,0.1l-0.1,0.1c0,0-0.1,0.1-0.1,0.1
+       id="clipPath18"
-		c-0.1,0.1-0.1,0.1-0.1,0.2c0,0.1-0.1,0.1-0.1,0.2c0,0.1,0,0.1,0,0.2c-0.1,0.3-0.1,0.7,0.1,1.1c0,0.1,0,0.1,0.1,0.2
+       clipPathUnits="userSpaceOnUse"><path
-		c0,0.1,0.1,0.1,0.1,0.2c0,0.1,0.1,0.2,0.2,0.3h7.2l0-7.5c0,0,0,0,0-0.1c0-0.1,0-0.1,0.1-0.2c0-0.1,0.1-0.1,0.1-0.2
+         id="path16"
-		c0,0,0.1-0.1,0.1-0.1l0.1,0c0.1-0.1,0.2-0.1,0.3-0.2c0.5-0.3,1-0.5,1.6-0.6c0.2,0,0.3,0,0.5,0c0,0,0.1,0,0.1,0c0.1,0,0.1,0,0.2,0
+         d="M 0,52.49 H 52.491 V 0 H 0 Z" /></clipPath></defs><g
-		c0.1,0,0.2,0,0.4,0.1c0.3,0.1,0.7,0.2,0.9,0.4c0.1,0.1,0.2,0.1,0.3,0.2c0,0,0.1,0.1,0.1,0.1l0,0l-0.1,0.1c0,0,0,0,0,0l0.1-0.1
+     transform="matrix(1.3333333,0,0,-1.3333333,0,69.9872)"
-		l0.2,0.2c0.1,0.1,0.2,0.2,0.4,0.2c0.2,0.1,0.5,0.3,0.8,0.4c0.7,0.2,1.4,0.3,2,0.1c0.4-0.1,0.7-0.2,1.1-0.4
+     id="g10"><g
-		C35.1,17.7,35.4,17.5,35.7,17.2z"/>
+       id="g12"><g
-	<g>
+         clip-path="url(#clipPath18)"
-		<polygon points="41.6,29.7 41.6,29.7 41.6,29.6 41.6,29.6 		"/>
+         id="g14"><g
-		<path fill="white" fill-opacity="0.5" d="M28.6,49.2c0.9,0.8,2,1.4,3,1.9c0.8,0.4,1.7,0.7,2.6,0.9c0.9,0.2,1.8,0.4,2.7,0.4c0.9,0.1,1.9,0,2.8-0.1
+           transform="translate(35.6531,35.3361)"
-			c0.7-0.1,1.4-0.2,2.1-0.4c1.3-0.4,2.5-0.9,3.6-1.6c0.8-0.5,1.6-1.1,2.3-1.7c0.7-0.7,1.4-1.4,2-2.2c0.5-0.6,0.9-1.3,1.2-2
+           id="g20"><path
-			c0.4-0.8,0.7-1.7,1-2.5c0.2-0.6,0.3-1.3,0.4-1.9c0.1-1,0.2-2,0.1-3c0-0.8-0.2-1.6-0.3-2.4c-0.2-0.9-0.5-1.7-0.8-2.6
+             id="path22"
-			c-0.5-1.2-1.2-2.4-2.1-3.4c-0.3-0.4-0.6-0.8-1-1.1l-0.5-0.5h-6.1l-0.2,0.3c0,0.1-0.1,0.1-0.1,0.2c0,0.1-0.1,0.1-0.1,0.2
+             style="fill:white;fill-opacity:0.5;fill-rule:nonzero;stroke:none"
-			c-0.1,0.4-0.2,0.8-0.1,1.1c0,0.1,0,0.1,0,0.2c0,0.1,0.1,0.1,0.1,0.2c0,0.1,0.1,0.2,0.2,0.3c0,0,0.1,0.1,0.1,0.1l0,0
+             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
-			c0,0,0,0.1,0.1,0.1l0,0.1l0.1-0.1l0,0l0,0l0,0l0,0l0,0l0.1,0.1c0.2,0.2,0.3,0.4,0.5,0.7c0.3,0.5,0.5,1,0.6,1.5
+           transform="translate(41.5882,22.8337)"
-			c0.1,0.7,0.1,1.5,0,2.2c-0.2,0.6-0.4,1.2-0.8,1.8c-0.3,0.4-0.7,0.8-1.1,1.1c-0.4,0.3-0.8,0.5-1.3,0.7c-0.5,0.2-1,0.3-1.5,0.3
+           id="g24"><path
-			c-0.5,0-0.9,0-1.3-0.1c-0.5-0.1-1-0.3-1.4-0.6c-0.5-0.3-1-0.7-1.3-1.1c-0.4-0.5-0.7-1-0.9-1.6c-0.2-0.6-0.3-1.3-0.2-2
+             id="path26"
-			c0-0.4,0.1-0.7,0.2-1c0.1-0.4,0.3-0.8,0.5-1.1c0.1-0.2,0.2-0.3,0.3-0.5c0-0.1,0.1-0.1,0.1-0.2l0.1,0c0,0,0,0,0,0l-0.1-0.1l0.1-0.1
+             style="fill:white;fill-opacity:0.5;fill-rule:nonzero;stroke:none"
-			c0,0,0-0.1,0.1-0.1l0.1-0.1c0,0,0.1-0.1,0.1-0.1c0-0.1,0.1-0.1,0.1-0.2c0-0.1,0.1-0.1,0.1-0.2c0-0.1,0-0.1,0-0.2
+             d="M 0,0 V 0 L 0.053,0.044 0.013,0.018 Z" /></g><g
-			c0.1-0.3,0.1-0.7-0.1-1.1c0-0.1,0-0.1-0.1-0.2c0-0.1-0.1-0.2-0.1-0.2c0-0.1-0.1-0.2-0.1-0.2h-7.2l0,7.5c0,0,0,0,0,0.1
+           transform="translate(28.5515,3.2736)"
-			c0,0.1-0.1,0.2-0.1,0.3c0,0,0,0.1-0.1,0.1c0,0-0.1,0.1-0.1,0.1c0,0,0,0-0.1,0c-0.1,0.1-0.3,0.2-0.4,0.3c-0.6,0.4-1.3,0.6-1.9,0.6
+           id="g28"><path
-			c-0.1,0-0.2,0-0.3,0c-0.1,0-0.2,0-0.3,0c-0.4-0.1-0.7-0.2-1-0.4c-0.1-0.1-0.2-0.2-0.3-0.2c0,0-0.1-0.1-0.1-0.1h0l0,0l0,0
+             id="path30"
-			c0,0-0.1-0.1-0.1-0.1L22,34.8c-0.2-0.2-0.4-0.3-0.6-0.4c-0.4-0.2-0.9-0.3-1.3-0.4c-0.3,0-0.7,0-1,0.1c-0.3,0.1-0.5,0.1-0.8,0.2
+             style="fill:white;fill-opacity:0.5;fill-rule:nonzero;stroke:none"
-			c-0.5,0.2-1,0.5-1.3,0.9c-0.3,0.4-0.6,0.8-0.8,1.3C16,37,15.9,37.5,15.9,38c0,0.5,0.1,0.9,0.3,1.3c0.2,0.4,0.4,0.7,0.7,1
+             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
-			c0.3,0.3,0.6,0.5,1,0.7c0.3,0.2,0.7,0.3,1,0.4c0.4,0.1,0.9,0.1,1.4,0c0.2,0,0.5-0.1,0.7-0.2c0.3-0.1,0.5-0.2,0.7-0.4
+           transform="translate(22.2707,17.571)"
-			c0.1-0.1,0.2-0.2,0.3-0.3l0.1-0.1l0,0l0,0c0,0,0,0,0,0l0.1-0.1c0.1,0,0.1-0.1,0.2-0.1c0.1-0.1,0.2-0.2,0.3-0.2
+           id="g32"><path
-			c0.3-0.2,0.6-0.3,0.9-0.3c0.2,0,0.3,0,0.5,0h0.1c0.2,0,0.3,0,0.5,0c0.5,0.1,1,0.3,1.5,0.6c0.1,0.1,0.2,0.1,0.3,0.2l0.1,0
+             id="path34"
-			c0,0,0.1,0.1,0.1,0.1c0,0.1,0.1,0.1,0.1,0.2c0,0.1,0,0.1,0.1,0.2c0,0,0,0,0,0.1l0,0v6.5C27.4,48.2,28,48.7,28.6,49.2z"/>
+             style="fill:white;fill-opacity:0.5;fill-rule:nonzero;stroke:none"
-		<polygon points="22.3,34.9 22.3,34.9 22.3,34.9 22.3,34.9 		"/>
+             d="m 0,0 v -0.008 l 0.011,0.019 v 0 z" /></g><g
-	</g>
+           transform="translate(40.657,22.3049)"
-	<g>
+           id="g36"><path
-		<polygon points="40.7,30.2 40.7,30.2 40.7,30.2 		"/>
+             id="path38"
-		<path fill="white" fill-opacity="0.5" d="M49.2,24c0.7-0.9,1.3-1.8,1.8-2.9c0.4-0.7,0.7-1.5,0.9-2.3c0.2-0.8,0.4-1.7,0.5-2.6c0.2-1.7,0.1-3.4-0.3-5
+             style="fill:white;fill-opacity:0.5;fill-rule:nonzero;stroke:none"
-			c-0.3-1.3-0.8-2.6-1.5-3.8c-0.4-0.7-0.9-1.4-1.4-2.1c-0.6-0.7-1.3-1.4-2-2c-0.6-0.5-1.3-1-2-1.4c-0.8-0.4-1.5-0.8-2.3-1.1
+             d="M 0,0 Z" /></g><g
-			c-1-0.4-2-0.6-3.1-0.7C39.3,0.1,38.9,0,38.5,0c-0.2,0-0.4,0-0.6,0h-0.1c-0.5,0-0.9,0-1.4,0.1c-0.8,0.1-1.7,0.2-2.5,0.4
+           transform="translate(49.1867,28.5134)"
-			c-0.9,0.2-1.7,0.6-2.5,0.9c-0.8,0.4-1.5,0.8-2.2,1.3c-0.6,0.4-1.1,0.9-1.6,1.4c-0.2,0.2-0.5,0.5-0.7,0.7V11
+           id="g40"><path
-			c0.1,0.1,0.2,0.1,0.2,0.1c0.1,0,0.1,0.1,0.2,0.1c0.1,0,0.1,0.1,0.2,0.1c0.4,0.1,0.8,0.2,1.1,0.1c0.1,0,0.1,0,0.2,0
+             id="path42"
-			c0.1,0,0.1-0.1,0.2-0.1c0.1,0,0.2-0.1,0.3-0.2c0,0,0.1-0.1,0.1-0.1l0,0c0,0,0.1,0,0.1-0.1l0.1-0.1c0.2-0.1,0.3-0.2,0.5-0.3
+             style="fill:white;fill-opacity:0.5;fill-rule:nonzero;stroke:none"
-			c0.3-0.2,0.7-0.4,1-0.5c0.3-0.1,0.6-0.2,1-0.2c0.6-0.1,1.1-0.1,1.7,0c0.5,0.1,1,0.3,1.5,0.5c0.5,0.3,0.9,0.6,1.3,1
+             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>
-			c0.4,0.4,0.7,0.9,0.9,1.4c0.2,0.6,0.4,1.2,0.4,1.8c0,0.6-0.1,1.3-0.3,1.9c-0.2,0.7-0.6,1.3-1,1.8c-0.5,0.5-1.1,1-1.8,1.3
+          </g></g></g></svg>
-			c-0.4,0.1-0.7,0.3-1.1,0.3c-0.4,0.1-0.8,0.1-1.3,0.1c-0.4,0-0.7-0.1-1.1-0.2c-0.4-0.1-0.7-0.3-1-0.5c-0.2-0.1-0.3-0.2-0.5-0.3
-			c-0.1,0-0.1-0.1-0.2-0.1l-0.1-0.1c0,0-0.1,0-0.1-0.1c0,0-0.1-0.1-0.1-0.1c0,0-0.1-0.1-0.1-0.1c-0.1-0.1-0.1-0.1-0.2-0.1
-			c-0.1,0-0.1-0.1-0.2-0.1c-0.1,0-0.1,0-0.2,0C28.4,18,28,18,27.6,18.1c-0.1,0-0.1,0-0.2,0.1c-0.1,0-0.2,0.1-0.2,0.1
-			c-0.1,0-0.2,0.1-0.2,0.1v7.2l7.5,0c0,0,0,0,0.1,0c0.1,0,0.2,0.1,0.3,0.1c0,0,0.1,0,0.1,0.1c0,0,0.1,0.1,0.1,0.1l0.1,0.1
-			c0.1,0.1,0.2,0.3,0.3,0.4c0.4,0.6,0.6,1.3,0.6,1.9c0,0.1,0,0.2,0,0.3c0,0.1,0,0.2,0,0.3c-0.1,0.4-0.2,0.7-0.4,1
-			c-0.1,0.1-0.2,0.2-0.2,0.3c0,0-0.1,0.1-0.1,0.1l0,0l0,0l-0.2,0.2c-0.1,0.1-0.2,0.2-0.2,0.3c-0.1,0.2-0.3,0.5-0.3,0.7
-			c-0.2,0.7-0.3,1.4-0.1,2c0.1,0.4,0.2,0.7,0.4,1.1c0.2,0.4,0.5,0.7,0.8,0.9c0.3,0.3,0.7,0.5,1.1,0.7c0.4,0.2,0.9,0.3,1.4,0.3
-			c0,0,0,0,0,0c0.5,0,1-0.1,1.4-0.3c0.5-0.2,0.9-0.5,1.3-0.8c0.4-0.4,0.7-0.9,0.9-1.4c0.2-0.6,0.3-1.2,0.2-1.8
-			c-0.1-0.5-0.2-0.9-0.5-1.3c-0.1-0.2-0.3-0.4-0.4-0.6l-0.1-0.1l0,0l0,0l0,0l0,0l0-0.1l0,0l-0.1,0.1l-0.1-0.1
-			c-0.1-0.1-0.1-0.1-0.1-0.2c-0.1-0.1-0.2-0.3-0.2-0.4c-0.1-0.1-0.1-0.3-0.2-0.4c0-0.1-0.1-0.2-0.1-0.4c0-0.1,0-0.2,0-0.3
-			c-0.1-0.6,0.1-1.3,0.4-1.9c0.1-0.2,0.2-0.4,0.4-0.6c0,0,0.1-0.1,0.1-0.1c0,0,0.1-0.1,0.1-0.1c0,0,0.1,0,0.1-0.1
-			c0.1-0.1,0.2-0.1,0.3-0.1c0,0,0,0,0.1,0l0,0h6.5C48.2,25.1,48.7,24.5,49.2,24z"/>
-	</g>
-</g>
-<g id="XMLID_14_">
-</g>
-<g id="XMLID_15_">
-</g>
-<g id="XMLID_16_">
-</g>
-<g id="XMLID_17_">
-</g>
-<g id="XMLID_18_">
-</g>
-<g id="XMLID_19_">
-</g>
-</svg>

apps/ios/cleverrc/index.html

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

apps/ios/cleverrc/main.css

@@ -7,6 +7,10 @@ html, body {
 	color: rgba(255, 255, 255, 0.9);
 }
 
+* {
+	user-select: none;
+}
+
 .stick {
 	border-radius: 50%;
 	width: 5cm;
@@ -72,7 +76,8 @@ body.armed .telemetry .mode {
 
 body.low-battery .battery {
 	color: #ff554b;
-	animation: scale 0.3s 1 ease-in-out}
+	animation: scale 0.3s 1 ease-in-out
+}
 
 .logo {
 	position: absolute;
@@ -89,3 +94,32 @@ body.low-battery .battery {
 	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/ios/cleverrc/telemetry.js

@@ -1,6 +1,7 @@
 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 });
 
@@ -35,10 +36,14 @@ new ROSLIB.Topic({
 });
 
 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, 10000);
+notifyLowBatteryThrottled = throttle(notifyLowBattery, 15000);
 
 new ROSLIB.Topic({
 	ros: ros,
@@ -50,29 +55,50 @@ new ROSLIB.Topic({
 	batteryEl.innerHTML = (message.cell_voltage[0].toFixed(2) + ' V') || '';
 
 	if (message.cell_voltage[0] < LOW_BATTERY) {
-		console.log('low battery');
+		notifyLowBatteryThrottled();
-		callNativeApp('lowBattery');
-		body.classList.remove('low-battery');
-		void body.offsetWidth; // trick for repeating animation
-		body.classList.add('low-battery');
 	} 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: '/rosout_agg',
+	name: '/mavros/statustext/recv',
-	messageType: 'rosgraph_msgs/Log'
+	messageType: 'mavros_msgs/StatusText'
 }).subscribe(function(message) {
-	var BLACKLIST = ['CMD: ', 'PR: ', 'DROPPED'];
+	var BLACKLIST = ['CMD: ', 'PR: ', 'DROPPED', 'Clock skew detected', 'MANUAL CONTROL LOST'];
-	if(message.level >= 4) {
+	if (message.severity <= 4) {
 		if (BLACKLIST.some(function(e) {
-				return message.msg.indexOf(e) != -1;
+				return message.text.indexOf(e) != -1;
 			})) {
-			console.log('Filtered out message ' + message.msg);
+			console.log('Filtered out message ' + message.text);
 			return;
 		}
+		notify(message.text, message.severity);
 		callNativeApp('notification', message);
 	}
 });

aruco_pose/CMakeLists.txt

@@ -16,11 +16,15 @@ find_package(catkin REQUIRED COMPONENTS
   image_transport
   cv_bridge
   tf
-  #tf2
+  tf2
-  #tf2_ros
+  tf2_ros
-  #aruco_msgs
+  tf2_geometry_msgs
+  sensor_msgs
+  message_generation
 )
 
+find_package(OpenCV REQUIRED)
+
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
 
@@ -55,11 +59,12 @@ find_package(catkin REQUIRED COMPONENTS
 ##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
 
 ## Generate messages in the 'msg' folder
-#add_message_files(
+add_message_files(
-#  FILES
+  FILES
-#  Marker.msg
+  Point2D.msg
-#  MarkerArray.msg
+  Marker.msg
-#)
+  MarkerArray.msg
+)
 
 ## Generate services in the 'srv' folder
 # add_service_files(
@@ -76,10 +81,11 @@ find_package(catkin REQUIRED COMPONENTS
 # )
 
 ## Generate added messages and services with any dependencies listed here
-#generate_messages(
+generate_messages(
-#  DEPENDENCIES
+  DEPENDENCIES
-#  std_msgs  # Or other packages containing msgs
+  std_msgs
-#)
+  geometry_msgs
+)
 
 ################################################
 ## Declare ROS dynamic reconfigure parameters ##
@@ -111,9 +117,9 @@ find_package(catkin REQUIRED COMPONENTS
 ## CATKIN_DEPENDS: catkin_packages dependent projects also need
 ## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
-#  INCLUDE_DIRS include
+  INCLUDE_DIRS DEPENDS OpenCV
   LIBRARIES aruco_pose
-#  CATKIN_DEPENDS other_catkin_pkg
+  CATKIN_DEPENDS message_runtime
 #  DEPENDS system_lib
 )
 
@@ -126,11 +132,13 @@ catkin_package(
 include_directories(
 # include
   ${catkin_INCLUDE_DIRS}
+  ${OpenCV_INCLUDE_DIRS}
 )
 
 ## Declare a C++ library
-add_library(${PROJECT_NAME}
+add_library(aruco_pose
-  src/aruco_pose.cpp
+  src/aruco_detect.cpp
+  src/aruco_map.cpp
 )
 
 ## Add cmake target dependencies of the library
@@ -154,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(aruco_pose
-
-target_link_libraries(${PROJECT_NAME}
   ${catkin_LIBRARIES}
-  "/opt/ros/kinetic/lib/libopencv_aruco3.so" # TODO: fix launch fails with .so loading
+  ${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,61 +2,43 @@
 <package>
   <name>aruco_pose</name>
   <version>0.0.0</version>
-  <description>ArUco maps precise pose estimation nodelet</description>
+  <description>Positioning by ArUco markers</description>
 
-  <!-- One maintainer tag required, multiple allowed, one person per tag -->
-  <!-- Example:  -->
-  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
   <maintainer email="okalachev@gmail.com">Oleg Kalachev</maintainer>
+  <license>MIT</license>
 
+  <!--url type="website">http://wiki.ros.org/aruco_pose</url-->
+  <author email="okalachev@gmail.com">Oleg Kalachev</author>
+  <author email="urpylka@gmail.com">Artem Smirnov</author>
 
-  <!-- One license tag required, multiple allowed, one license per tag -->
-  <!-- Commonly used license strings: -->
-  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
-  <license>TODO</license>
-
-
-  <!-- Url tags are optional, but multiple are allowed, one per tag -->
-  <!-- Optional attribute type can be: website, bugtracker, or repository -->
-  <!-- Example: -->
-  <!-- <url type="website">http://wiki.ros.org/aruco_pose</url> -->
-
-
-  <!-- Author tags are optional, multiple are allowed, one per tag -->
-  <!-- Authors do not have to be maintainers, but could be -->
-  <!-- Example: -->
-  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
-
-
-  <!-- The *_depend tags are used to specify dependencies -->
-  <!-- Dependencies can be catkin packages or system dependencies -->
-  <!-- Examples: -->
-  <!-- Use build_depend for packages you need at compile time: -->
-  <!--   <build_depend>message_generation</build_depend> -->
-  <!-- Use buildtool_depend for build tool packages: -->
-  <!--   <buildtool_depend>catkin</buildtool_depend> -->
-  <!-- Use run_depend for packages you need at runtime: -->
-  <!--   <run_depend>message_runtime</run_depend> -->
-  <!-- Use test_depend for packages you need only for testing: -->
-  <!--   <test_depend>gtest</test_depend> -->
   <buildtool_depend>catkin</buildtool_depend>
 
+  <build_depend>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>std_msgs</build_depend>
-  <build_depend>cv_bridge</build_depend>
-  <build_depend>tf</build_depend>
-
   <run_depend>nodelet</run_depend>
   <run_depend>roscpp</run_depend>
   <run_depend>image_transport</run_depend>
   <run_depend>cv_bridge</run_depend>
-  <build_depend>tf</build_depend>
+  <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>
-        <nodelet plugin="${prefix}/nodelet_plugins.xml" />
+    <nodelet plugin="${prefix}/nodelet_plugins.xml" />
     <!-- Other tools can request additional information be placed here -->
-
   </export>
 </package>

aruco_pose/src/aruco_detect.cpp

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

builder/assets/butterfly.service

@@ -0,0 +1,6 @@
+[Unit]
+Description=Butterfly Terminal Server
+
+[Service]
+ExecStart=/usr/local/bin/butterfly.server.py --host="0.0.0.0" --unsecure
+User=pi

builder/assets/butterfly.socket

@@ -0,0 +1,5 @@
+[Socket]
+ListenStream=57575
+
+[Install]
+WantedBy=sockets.target

builder/assets/clever.service

@@ -4,7 +4,7 @@ Requires=roscore.service
 After=roscore.service
 
 [Service]
-EnvironmentFile=/home/pi/catkin_ws/src/clever/deploy/roscore.env
+EnvironmentFile=/lib/systemd/system/roscore.env
 ExecStart=/opt/ros/kinetic/bin/roslaunch clever clever.launch --wait
 Restart=on-abort
 

builder/assets/hardware_setup.sh

@@ -0,0 +1,78 @@
+#! /usr/bin/env bash
+
+#
+# Script for build the image. Used builder script of the target repo
+# For build: docker run --privileged -it --rm -v /dev:/dev -v $(pwd):/builder/repo smirart/builder
+#
+# Copyright (C) 2018 Copter Express Technologies
+#
+# Author: Artem Smirnov <urpylka@gmail.com>
+#
+
+set -e # Exit immidiately on non-zero result
+
+echo_stamp() {
+  # TEMPLATE: echo_stamp <TEXT> <TYPE>
+  # TYPE: SUCCESS, ERROR, INFO
+
+  # More info there https://www.shellhacks.com/ru/bash-colors/
+
+  TEXT="$(date '+[%Y-%m-%d %H:%M:%S]') $1"
+  TEXT="\e[1m$TEXT\e[0m" # BOLD
+
+  case "$2" in
+    SUCCESS)
+    TEXT="\e[32m${TEXT}\e[0m";; # GREEN
+    ERROR)
+    TEXT="\e[31m${TEXT}\e[0m";; # RED
+    *)
+    TEXT="\e[34m${TEXT}\e[0m";; # BLUE
+  esac
+  echo -e ${TEXT}
+}
+
+##################################################
+# Configure hardware interfaces
+##################################################
+
+# 1. Enable sshd
+echo_stamp "#1 Turn on sshd"
+touch /boot/ssh
+# /usr/bin/raspi-config nonint do_ssh 0
+
+# 2. Enable GPIO
+echo_stamp "#2 GPIO enabled by default"
+
+# 3. Enable I2C
+echo_stamp "#3 Turn on I2C"
+/usr/bin/raspi-config nonint do_i2c 0
+
+# 4. Enable SPI
+echo_stamp "#4 Turn on SPI"
+/usr/bin/raspi-config nonint do_spi 0
+
+# 5. Enable raspicam
+echo_stamp "#5 Turn on raspicam"
+/usr/bin/raspi-config nonint do_camera 0
+
+# 6. Enable hardware UART
+echo_stamp "#6 Turn on UART"
+# Temporary solution
+# https://github.com/RPi-Distro/raspi-config/pull/75
+/usr/bin/raspi-config nonint do_serial 1
+/usr/bin/raspi-config nonint set_config_var enable_uart 1 /boot/config.txt
+/usr/bin/raspi-config nonint set_config_var dtoverlay pi3-disable-bt /boot/config.txt
+systemctl disable hciuart.service
+
+# After adding to Raspbian OS
+# https://github.com/RPi-Distro/raspi-config/commit/d6d9ecc0d9cbe4aaa9744ae733b9cb239e79c116
+#/usr/bin/raspi-config nonint do_serial 2
+
+# 7. Enable V4L driver http://robocraft.ru/blog/electronics/3158.html
+#echo "bcm2835-v4l2" >> /etc/modules
+echo_stamp "#7 Turn on v4l2 driver"
+if ! grep -q "^bcm2835-v4l2" /etc/modules;
+then printf "bcm2835-v4l2\n" >> /etc/modules
+fi
+
+echo_stamp "#8 End of configure hardware interfaces"

builder/assets/index.html

@@ -0,0 +1,13 @@
+<h1>CLEVER Drone Kit Tools</h1>
+
+<ul>
+	<!-- <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="viz.html">View 3D visualization</a> (<code>ros3djs</code>)</li> -->
+</ul>
+
+<script type="text/javascript">
+	document.querySelector("#wvs").href = location.origin + ':8080';
+	document.querySelector("#butterfly").href = location.origin + ':57575';
+</script>

builder/assets/init_rpi.sh

@@ -0,0 +1,43 @@
+#! /usr/bin/env bash
+
+#
+# Script for build the image. Used builder script of the target repo
+# For build: docker run --privileged -it --rm -v /dev:/dev -v $(pwd):/builder/repo smirart/builder
+#
+# Copyright (C) 2018 Copter Express Technologies
+#
+# Author: Artem Smirnov <urpylka@gmail.com>
+#
+
+set -e # Exit immidiately on non-zero result
+
+echo_stamp() {
+  # TEMPLATE: echo_stamp <TEXT> <TYPE>
+  # TYPE: SUCCESS, ERROR, INFO
+
+  # More info there https://www.shellhacks.com/ru/bash-colors/
+
+  TEXT="$(date '+[%Y-%m-%d %H:%M:%S]') $1"
+  TEXT="\e[1m$TEXT\e[0m" # BOLD
+
+  case "$2" in
+    SUCCESS)
+    TEXT="\e[32m${TEXT}\e[0m";; # GREEN
+    ERROR)
+    TEXT="\e[31m${TEXT}\e[0m";; # RED
+    *)
+    TEXT="\e[34m${TEXT}\e[0m";; # BLUE
+  esac
+  echo -e ${TEXT}
+}
+
+echo_stamp "Rename SSID"
+sudo sed -i.OLD "s/CLEVER/CLEVER-$(head -c 100 /dev/urandom | xxd -ps -c 100 | sed -e 's/[^0-9]//g' | cut -c 1-4)/g" /etc/wpa_supplicant/wpa_supplicant.conf
+
+echo_stamp "Harware setup"
+/root/hardware_setup.sh
+
+echo_stamp "Remove init scripts"
+rm /root/init_rpi.sh /root/hardware_setup.sh
+
+echo_stamp "End of initialization of the image"

builder/assets/kinetic-ros-clever.rosinstall

@@ -0,0 +1,676 @@
+- tar:
+    local-name: catkin
+    uri: https://github.com/ros-gbp/catkin-release/archive/release/kinetic/catkin/0.7.11-0.tar.gz
+    version: catkin-release-release-kinetic-catkin-0.7.11-0
+- tar:
+    local-name: cmake_modules
+    uri: https://github.com/ros-gbp/cmake_modules-release/archive/release/kinetic/cmake_modules/0.4.1-0.tar.gz
+    version: cmake_modules-release-release-kinetic-cmake_modules-0.4.1-0
+- tar:
+    local-name: gencpp
+    uri: https://github.com/ros-gbp/gencpp-release/archive/release/kinetic/gencpp/0.6.0-0.tar.gz
+    version: gencpp-release-release-kinetic-gencpp-0.6.0-0
+- tar:
+    local-name: geneus
+    uri: https://github.com/tork-a/geneus-release/archive/release/kinetic/geneus/2.2.6-0.tar.gz
+    version: geneus-release-release-kinetic-geneus-2.2.6-0
+- tar:
+    local-name: genlisp
+    uri: https://github.com/ros-gbp/genlisp-release/archive/release/kinetic/genlisp/0.4.16-0.tar.gz
+    version: genlisp-release-release-kinetic-genlisp-0.4.16-0
+- tar:
+    local-name: genmsg
+    uri: https://github.com/ros-gbp/genmsg-release/archive/release/kinetic/genmsg/0.5.10-0.tar.gz
+    version: genmsg-release-release-kinetic-genmsg-0.5.10-0
+- tar:
+    local-name: gennodejs
+    uri: https://github.com/RethinkRobotics-release/gennodejs-release/archive/release/kinetic/gennodejs/2.0.1-0.tar.gz
+    version: gennodejs-release-release-kinetic-gennodejs-2.0.1-0
+- tar:
+    local-name: genpy
+    uri: https://github.com/ros-gbp/genpy-release/archive/release/kinetic/genpy/0.6.7-0.tar.gz
+    version: genpy-release-release-kinetic-genpy-0.6.7-0
+- tar:
+    local-name: message_generation
+    uri: https://github.com/ros-gbp/message_generation-release/archive/release/kinetic/message_generation/0.4.0-0.tar.gz
+    version: message_generation-release-release-kinetic-message_generation-0.4.0-0
+- tar:
+    local-name: message_runtime
+    uri: https://github.com/ros-gbp/message_runtime-release/archive/release/kinetic/message_runtime/0.4.12-0.tar.gz
+    version: message_runtime-release-release-kinetic-message_runtime-0.4.12-0
+- tar:
+    local-name: ros/mk
+    uri: https://github.com/ros-gbp/ros-release/archive/release/kinetic/mk/1.14.3-0.tar.gz
+    version: ros-release-release-kinetic-mk-1.14.3-0
+- tar:
+    local-name: ros/ros
+    uri: https://github.com/ros-gbp/ros-release/archive/release/kinetic/ros/1.14.3-0.tar.gz
+    version: ros-release-release-kinetic-ros-1.14.3-0
+- tar:
+    local-name: ros/rosbash
+    uri: https://github.com/ros-gbp/ros-release/archive/release/kinetic/rosbash/1.14.3-0.tar.gz
+    version: ros-release-release-kinetic-rosbash-1.14.3-0
+- tar:
+    local-name: ros/rosboost_cfg
+    uri: https://github.com/ros-gbp/ros-release/archive/release/kinetic/rosboost_cfg/1.14.3-0.tar.gz
+    version: ros-release-release-kinetic-rosboost_cfg-1.14.3-0
+- tar:
+    local-name: ros/rosbuild
+    uri: https://github.com/ros-gbp/ros-release/archive/release/kinetic/rosbuild/1.14.3-0.tar.gz
+    version: ros-release-release-kinetic-rosbuild-1.14.3-0
+- tar:
+    local-name: ros/rosclean
+    uri: https://github.com/ros-gbp/ros-release/archive/release/kinetic/rosclean/1.14.3-0.tar.gz
+    version: ros-release-release-kinetic-rosclean-1.14.3-0
+- tar:
+    local-name: ros/roscreate
+    uri: https://github.com/ros-gbp/ros-release/archive/release/kinetic/roscreate/1.14.3-0.tar.gz
+    version: ros-release-release-kinetic-roscreate-1.14.3-0
+- tar:
+    local-name: ros/roslang
+    uri: https://github.com/ros-gbp/ros-release/archive/release/kinetic/roslang/1.14.3-0.tar.gz
+    version: ros-release-release-kinetic-roslang-1.14.3-0
+- tar:
+    local-name: ros/roslib
+    uri: https://github.com/ros-gbp/ros-release/archive/release/kinetic/roslib/1.14.3-0.tar.gz
+    version: ros-release-release-kinetic-roslib-1.14.3-0
+- tar:
+    local-name: ros/rosmake
+    uri: https://github.com/ros-gbp/ros-release/archive/release/kinetic/rosmake/1.14.3-0.tar.gz
+    version: ros-release-release-kinetic-rosmake-1.14.3-0
+- tar:
+    local-name: ros/rosunit
+    uri: https://github.com/ros-gbp/ros-release/archive/release/kinetic/rosunit/1.14.3-0.tar.gz
+    version: ros-release-release-kinetic-rosunit-1.14.3-0
+- tar:
+    local-name: ros_comm/message_filters
+    uri: https://github.com/ros-gbp/ros_comm-release/archive/release/kinetic/message_filters/1.12.13-0.tar.gz
+    version: ros_comm-release-release-kinetic-message_filters-1.12.13-0
+- tar:
+    local-name: ros_comm/ros_comm
+    uri: https://github.com/ros-gbp/ros_comm-release/archive/release/kinetic/ros_comm/1.12.13-0.tar.gz
+    version: ros_comm-release-release-kinetic-ros_comm-1.12.13-0
+- tar:
+    local-name: ros_comm/rosbag
+    uri: https://github.com/ros-gbp/ros_comm-release/archive/release/kinetic/rosbag/1.12.13-0.tar.gz
+    version: ros_comm-release-release-kinetic-rosbag-1.12.13-0
+- tar:
+    local-name: ros_comm/rosbag_storage
+    uri: https://github.com/ros-gbp/ros_comm-release/archive/release/kinetic/rosbag_storage/1.12.13-0.tar.gz
+    version: ros_comm-release-release-kinetic-rosbag_storage-1.12.13-0
+- tar:
+    local-name: ros_comm/rosconsole
+    uri: https://github.com/ros-gbp/ros_comm-release/archive/release/kinetic/rosconsole/1.12.13-0.tar.gz
+    version: ros_comm-release-release-kinetic-rosconsole-1.12.13-0
+- tar:
+    local-name: ros_comm/roscpp
+    uri: https://github.com/ros-gbp/ros_comm-release/archive/release/kinetic/roscpp/1.12.13-0.tar.gz
+    version: ros_comm-release-release-kinetic-roscpp-1.12.13-0
+- tar:
+    local-name: ros_comm/rosgraph
+    uri: https://github.com/ros-gbp/ros_comm-release/archive/release/kinetic/rosgraph/1.12.13-0.tar.gz
+    version: ros_comm-release-release-kinetic-rosgraph-1.12.13-0
+- tar:
+    local-name: ros_comm/roslaunch
+    uri: https://github.com/ros-gbp/ros_comm-release/archive/release/kinetic/roslaunch/1.12.13-0.tar.gz
+    version: ros_comm-release-release-kinetic-roslaunch-1.12.13-0
+- tar:
+    local-name: ros_comm/roslz4
+    uri: https://github.com/ros-gbp/ros_comm-release/archive/release/kinetic/roslz4/1.12.13-0.tar.gz
+    version: ros_comm-release-release-kinetic-roslz4-1.12.13-0
+- tar:
+    local-name: ros_comm/rosmaster
+    uri: https://github.com/ros-gbp/ros_comm-release/archive/release/kinetic/rosmaster/1.12.13-0.tar.gz
+    version: ros_comm-release-release-kinetic-rosmaster-1.12.13-0
+- tar:
+    local-name: ros_comm/rosmsg
+    uri: https://github.com/ros-gbp/ros_comm-release/archive/release/kinetic/rosmsg/1.12.13-0.tar.gz
+    version: ros_comm-release-release-kinetic-rosmsg-1.12.13-0
+- tar:
+    local-name: ros_comm/rosnode
+    uri: https://github.com/ros-gbp/ros_comm-release/archive/release/kinetic/rosnode/1.12.13-0.tar.gz
+    version: ros_comm-release-release-kinetic-rosnode-1.12.13-0
+- tar:
+    local-name: ros_comm/rosout
+    uri: https://github.com/ros-gbp/ros_comm-release/archive/release/kinetic/rosout/1.12.13-0.tar.gz
+    version: ros_comm-release-release-kinetic-rosout-1.12.13-0
+- tar:
+    local-name: ros_comm/rosparam
+    uri: https://github.com/ros-gbp/ros_comm-release/archive/release/kinetic/rosparam/1.12.13-0.tar.gz
+    version: ros_comm-release-release-kinetic-rosparam-1.12.13-0
+- tar:
+    local-name: ros_comm/rospy
+    uri: https://github.com/ros-gbp/ros_comm-release/archive/release/kinetic/rospy/1.12.13-0.tar.gz
+    version: ros_comm-release-release-kinetic-rospy-1.12.13-0
+- tar:
+    local-name: ros_comm/rosservice
+    uri: https://github.com/ros-gbp/ros_comm-release/archive/release/kinetic/rosservice/1.12.13-0.tar.gz
+    version: ros_comm-release-release-kinetic-rosservice-1.12.13-0
+- tar:
+    local-name: ros_comm/rostest
+    uri: https://github.com/ros-gbp/ros_comm-release/archive/release/kinetic/rostest/1.12.13-0.tar.gz
+    version: ros_comm-release-release-kinetic-rostest-1.12.13-0
+- tar:
+    local-name: ros_comm/rostopic
+    uri: https://github.com/ros-gbp/ros_comm-release/archive/release/kinetic/rostopic/1.12.13-0.tar.gz
+    version: ros_comm-release-release-kinetic-rostopic-1.12.13-0
+- tar:
+    local-name: ros_comm/roswtf
+    uri: https://github.com/ros-gbp/ros_comm-release/archive/release/kinetic/roswtf/1.12.13-0.tar.gz
+    version: ros_comm-release-release-kinetic-roswtf-1.12.13-0
+- tar:
+    local-name: ros_comm/topic_tools
+    uri: https://github.com/ros-gbp/ros_comm-release/archive/release/kinetic/topic_tools/1.12.13-0.tar.gz
+    version: ros_comm-release-release-kinetic-topic_tools-1.12.13-0
+- tar:
+    local-name: ros_comm/xmlrpcpp
+    uri: https://github.com/ros-gbp/ros_comm-release/archive/release/kinetic/xmlrpcpp/1.12.13-0.tar.gz
+    version: ros_comm-release-release-kinetic-xmlrpcpp-1.12.13-0
+- tar:
+    local-name: ros_comm_msgs/rosgraph_msgs
+    uri: https://github.com/ros-gbp/ros_comm_msgs-release/archive/release/kinetic/rosgraph_msgs/1.11.2-0.tar.gz
+    version: ros_comm_msgs-release-release-kinetic-rosgraph_msgs-1.11.2-0
+- tar:
+    local-name: ros_comm_msgs/std_srvs
+    uri: https://github.com/ros-gbp/ros_comm_msgs-release/archive/release/kinetic/std_srvs/1.11.2-0.tar.gz
+    version: ros_comm_msgs-release-release-kinetic-std_srvs-1.11.2-0
+- tar:
+    local-name: ros_environment
+    uri: https://github.com/ros-gbp/ros_environment-release/archive/release/kinetic/ros_environment/1.0.0-0.tar.gz
+    version: ros_environment-release-release-kinetic-ros_environment-1.0.0-0
+- tar:
+    local-name: roscpp_core/cpp_common
+    uri: https://github.com/ros-gbp/roscpp_core-release/archive/release/kinetic/cpp_common/0.6.9-0.tar.gz
+    version: roscpp_core-release-release-kinetic-cpp_common-0.6.9-0
+- tar:
+    local-name: roscpp_core/roscpp_serialization
+    uri: https://github.com/ros-gbp/roscpp_core-release/archive/release/kinetic/roscpp_serialization/0.6.9-0.tar.gz
+    version: roscpp_core-release-release-kinetic-roscpp_serialization-0.6.9-0
+- tar:
+    local-name: roscpp_core/roscpp_traits
+    uri: https://github.com/ros-gbp/roscpp_core-release/archive/release/kinetic/roscpp_traits/0.6.9-0.tar.gz
+    version: roscpp_core-release-release-kinetic-roscpp_traits-0.6.9-0
+- tar:
+    local-name: roscpp_core/rostime
+    uri: https://github.com/ros-gbp/roscpp_core-release/archive/release/kinetic/rostime/0.6.9-0.tar.gz
+    version: roscpp_core-release-release-kinetic-rostime-0.6.9-0
+- tar:
+    local-name: roslisp
+    uri: https://github.com/ros-gbp/roslisp-release/archive/release/kinetic/roslisp/1.9.21-0.tar.gz
+    version: roslisp-release-release-kinetic-roslisp-1.9.21-0
+- tar:
+    local-name: rospack
+    uri: https://github.com/ros-gbp/rospack-release/archive/release/kinetic/rospack/2.4.4-0.tar.gz
+    version: rospack-release-release-kinetic-rospack-2.4.4-0
+- tar:
+    local-name: std_msgs
+    uri: https://github.com/ros-gbp/std_msgs-release/archive/release/kinetic/std_msgs/0.5.11-0.tar.gz
+    version: std_msgs-release-release-kinetic-std_msgs-0.5.11-0
+- tar:
+    local-name: actionlib
+    uri: https://github.com/ros-gbp/actionlib-release/archive/release/kinetic/actionlib/1.11.13-0.tar.gz
+    version: actionlib-release-release-kinetic-actionlib-1.11.13-0
+- tar:
+    local-name: angles
+    uri: https://github.com/ros-gbp/geometry_angles_utils-release/archive/release/kinetic/angles/1.9.11-0.tar.gz
+    version: geometry_angles_utils-release-release-kinetic-angles-1.9.11-0
+- tar:
+    local-name: async_web_server_cpp
+    uri: https://github.com/gt-rail-release/async_web_server_cpp-release/archive/release/kinetic/async_web_server_cpp/0.0.3-0.tar.gz
+    version: async_web_server_cpp-release-release-kinetic-async_web_server_cpp-0.0.3-0
+- tar:
+    local-name: bond_core/bond
+    uri: https://github.com/ros-gbp/bond_core-release/archive/release/kinetic/bond/1.8.1-0.tar.gz
+    version: bond_core-release-release-kinetic-bond-1.8.1-0
+- tar:
+    local-name: bond_core/bond_core
+    uri: https://github.com/ros-gbp/bond_core-release/archive/release/kinetic/bond_core/1.8.1-0.tar.gz
+    version: bond_core-release-release-kinetic-bond_core-1.8.1-0
+- tar:
+    local-name: bond_core/bondcpp
+    uri: https://github.com/ros-gbp/bond_core-release/archive/release/kinetic/bondcpp/1.8.1-0.tar.gz
+    version: bond_core-release-release-kinetic-bondcpp-1.8.1-0
+- tar:
+    local-name: bond_core/bondpy
+    uri: https://github.com/ros-gbp/bond_core-release/archive/release/kinetic/bondpy/1.8.1-0.tar.gz
+    version: bond_core-release-release-kinetic-bondpy-1.8.1-0
+- tar:
+    local-name: bond_core/smclib
+    uri: https://github.com/ros-gbp/bond_core-release/archive/release/kinetic/smclib/1.8.1-0.tar.gz
+    version: bond_core-release-release-kinetic-smclib-1.8.1-0
+- tar:
+    local-name: catkin
+    uri: https://github.com/ros-gbp/catkin-release/archive/release/kinetic/catkin/0.7.11-0.tar.gz
+    version: catkin-release-release-kinetic-catkin-0.7.11-0
+- tar:
+    local-name: class_loader
+    uri: https://github.com/ros-gbp/class_loader-release/archive/release/kinetic/class_loader/0.3.9-0.tar.gz
+    version: class_loader-release-release-kinetic-class_loader-0.3.9-0
+- tar:
+    local-name: cmake_modules
+    uri: https://github.com/ros-gbp/cmake_modules-release/archive/release/kinetic/cmake_modules/0.4.1-0.tar.gz
+    version: cmake_modules-release-release-kinetic-cmake_modules-0.4.1-0
+- tar:
+    local-name: common_msgs/actionlib_msgs
+    uri: https://github.com/ros-gbp/common_msgs-release/archive/release/kinetic/actionlib_msgs/1.12.6-0.tar.gz
+    version: common_msgs-release-release-kinetic-actionlib_msgs-1.12.6-0
+- tar:
+    local-name: common_msgs/diagnostic_msgs
+    uri: https://github.com/ros-gbp/common_msgs-release/archive/release/kinetic/diagnostic_msgs/1.12.6-0.tar.gz
+    version: common_msgs-release-release-kinetic-diagnostic_msgs-1.12.6-0
+- tar:
+    local-name: common_msgs/geometry_msgs
+    uri: https://github.com/ros-gbp/common_msgs-release/archive/release/kinetic/geometry_msgs/1.12.6-0.tar.gz
+    version: common_msgs-release-release-kinetic-geometry_msgs-1.12.6-0
+- tar:
+    local-name: common_msgs/nav_msgs
+    uri: https://github.com/ros-gbp/common_msgs-release/archive/release/kinetic/nav_msgs/1.12.6-0.tar.gz
+    version: common_msgs-release-release-kinetic-nav_msgs-1.12.6-0
+- tar:
+    local-name: common_msgs/sensor_msgs
+    uri: https://github.com/ros-gbp/common_msgs-release/archive/release/kinetic/sensor_msgs/1.12.6-0.tar.gz
+    version: common_msgs-release-release-kinetic-sensor_msgs-1.12.6-0
+- tar:
+    local-name: common_msgs/stereo_msgs
+    uri: https://github.com/ros-gbp/common_msgs-release/archive/release/kinetic/stereo_msgs/1.12.6-0.tar.gz
+    version: common_msgs-release-release-kinetic-stereo_msgs-1.12.6-0
+- tar:
+    local-name: common_msgs/trajectory_msgs
+    uri: https://github.com/ros-gbp/common_msgs-release/archive/release/kinetic/trajectory_msgs/1.12.6-0.tar.gz
+    version: common_msgs-release-release-kinetic-trajectory_msgs-1.12.6-0
+- tar:
+    local-name: common_msgs/visualization_msgs
+    uri: https://github.com/ros-gbp/common_msgs-release/archive/release/kinetic/visualization_msgs/1.12.6-0.tar.gz
+    version: common_msgs-release-release-kinetic-visualization_msgs-1.12.6-0
+- tar:
+    local-name: cv_camera
+    uri: https://github.com/OTL/cv_camera-release/archive/release/kinetic/cv_camera/0.3.0-0.tar.gz
+    version: cv_camera-release-release-kinetic-cv_camera-0.3.0-0
+- tar:
+    local-name: diagnostics/diagnostic_updater
+    uri: https://github.com/ros-gbp/diagnostics-release/archive/release/kinetic/diagnostic_updater/1.9.3-0.tar.gz
+    version: diagnostics-release-release-kinetic-diagnostic_updater-1.9.3-0
+- tar:
+    local-name: dynamic_reconfigure
+    uri: https://github.com/ros-gbp/dynamic_reconfigure-release/archive/release/kinetic/dynamic_reconfigure/1.5.49-0.tar.gz
+    version: dynamic_reconfigure-release-release-kinetic-dynamic_reconfigure-1.5.49-0
+- tar:
+    local-name: gencpp
+    uri: https://github.com/ros-gbp/gencpp-release/archive/release/kinetic/gencpp/0.6.0-0.tar.gz
+    version: gencpp-release-release-kinetic-gencpp-0.6.0-0
+- tar:
+    local-name: geneus
+    uri: https://github.com/tork-a/geneus-release/archive/release/kinetic/geneus/2.2.6-0.tar.gz
+    version: geneus-release-release-kinetic-geneus-2.2.6-0
+- tar:
+    local-name: genlisp
+    uri: https://github.com/ros-gbp/genlisp-release/archive/release/kinetic/genlisp/0.4.16-0.tar.gz
+    version: genlisp-release-release-kinetic-genlisp-0.4.16-0
+- tar:
+    local-name: genmsg
+    uri: https://github.com/ros-gbp/genmsg-release/archive/release/kinetic/genmsg/0.5.10-0.tar.gz
+    version: genmsg-release-release-kinetic-genmsg-0.5.10-0
+- tar:
+    local-name: gennodejs
+    uri: https://github.com/RethinkRobotics-release/gennodejs-release/archive/release/kinetic/gennodejs/2.0.1-0.tar.gz
+    version: gennodejs-release-release-kinetic-gennodejs-2.0.1-0
+- tar:
+    local-name: genpy
+    uri: https://github.com/ros-gbp/genpy-release/archive/release/kinetic/genpy/0.6.7-0.tar.gz
+    version: genpy-release-release-kinetic-genpy-0.6.7-0
+- tar:
+    local-name: geographic_info/geographic_msgs
+    uri: https://github.com/ros-geographic-info/geographic_info-release/archive/release/kinetic/geographic_msgs/0.5.2-0.tar.gz
+    version: geographic_info-release-release-kinetic-geographic_msgs-0.5.2-0
+- tar:
+    local-name: geometry/eigen_conversions
+    uri: https://github.com/ros-gbp/geometry-release/archive/release/kinetic/eigen_conversions/1.11.9-0.tar.gz
+    version: geometry-release-release-kinetic-eigen_conversions-1.11.9-0
+- tar:
+    local-name: geometry/tf
+    uri: https://github.com/ros-gbp/geometry-release/archive/release/kinetic/tf/1.11.9-0.tar.gz
+    version: geometry-release-release-kinetic-tf-1.11.9-0
+- tar:
+    local-name: geometry2/geometry2
+    uri: https://github.com/ros-gbp/geometry2-release/archive/release/kinetic/geometry2/0.5.17-0.tar.gz
+    version: geometry2-release-release-kinetic-geometry2-0.5.17-0
+- tar:
+    local-name: geometry2/tf2
+    uri: https://github.com/ros-gbp/geometry2-release/archive/release/kinetic/tf2/0.5.17-0.tar.gz
+    version: geometry2-release-release-kinetic-tf2-0.5.17-0
+- tar:
+    local-name: geometry2/tf2_bullet
+    uri: https://github.com/ros-gbp/geometry2-release/archive/release/kinetic/tf2_bullet/0.5.17-0.tar.gz
+    version: geometry2-release-release-kinetic-tf2_bullet-0.5.17-0
+- tar:
+    local-name: geometry2/tf2_eigen
+    uri: https://github.com/ros-gbp/geometry2-release/archive/release/kinetic/tf2_eigen/0.5.17-0.tar.gz
+    version: geometry2-release-release-kinetic-tf2_eigen-0.5.17-0
+- tar:
+    local-name: geometry2/tf2_geometry_msgs
+    uri: https://github.com/ros-gbp/geometry2-release/archive/release/kinetic/tf2_geometry_msgs/0.5.17-0.tar.gz
+    version: geometry2-release-release-kinetic-tf2_geometry_msgs-0.5.17-0
+- tar:
+    local-name: geometry2/tf2_kdl
+    uri: https://github.com/ros-gbp/geometry2-release/archive/release/kinetic/tf2_kdl/0.5.17-0.tar.gz
+    version: geometry2-release-release-kinetic-tf2_kdl-0.5.17-0
+- tar:
+    local-name: geometry2/tf2_msgs
+    uri: https://github.com/ros-gbp/geometry2-release/archive/release/kinetic/tf2_msgs/0.5.17-0.tar.gz
+    version: geometry2-release-release-kinetic-tf2_msgs-0.5.17-0
+- tar:
+    local-name: geometry2/tf2_py
+    uri: https://github.com/ros-gbp/geometry2-release/archive/release/kinetic/tf2_py/0.5.17-0.tar.gz
+    version: geometry2-release-release-kinetic-tf2_py-0.5.17-0
+- tar:
+    local-name: geometry2/tf2_ros
+    uri: https://github.com/ros-gbp/geometry2-release/archive/release/kinetic/tf2_ros/0.5.17-0.tar.gz
+    version: geometry2-release-release-kinetic-tf2_ros-0.5.17-0
+- tar:
+    local-name: geometry2/tf2_sensor_msgs
+    uri: https://github.com/ros-gbp/geometry2-release/archive/release/kinetic/tf2_sensor_msgs/0.5.17-0.tar.gz
+    version: geometry2-release-release-kinetic-tf2_sensor_msgs-0.5.17-0
+- tar:
+    local-name: geometry2/tf2_tools
+    uri: https://github.com/ros-gbp/geometry2-release/archive/release/kinetic/tf2_tools/0.5.17-0.tar.gz
+    version: geometry2-release-release-kinetic-tf2_tools-0.5.17-0
+- tar:
+    local-name: image_common/camera_calibration_parsers
+    uri: https://github.com/ros-gbp/image_common-release/archive/release/kinetic/camera_calibration_parsers/1.11.13-0.tar.gz
+    version: image_common-release-release-kinetic-camera_calibration_parsers-1.11.13-0
+- tar:
+    local-name: image_common/camera_info_manager
+    uri: https://github.com/ros-gbp/image_common-release/archive/release/kinetic/camera_info_manager/1.11.13-0.tar.gz
+    version: image_common-release-release-kinetic-camera_info_manager-1.11.13-0
+- tar:
+    local-name: image_common/image_transport
+    uri: https://github.com/ros-gbp/image_common-release/archive/release/kinetic/image_transport/1.11.13-0.tar.gz
+    version: image_common-release-release-kinetic-image_transport-1.11.13-0
+- tar:
+    local-name: mavlink
+    uri: https://github.com/mavlink/mavlink-gbp-release/archive/release/kinetic/mavlink/2018.6.6-0.tar.gz
+    version: mavlink-gbp-release-release-kinetic-mavlink-2018.6.6-0
+- tar:
+    local-name: mavros/libmavconn
+    uri: https://github.com/mavlink/mavros-release/archive/release/kinetic/libmavconn/0.26.0-0.tar.gz
+    version: mavros-release-release-kinetic-libmavconn-0.26.0-0
+- tar:
+    local-name: mavros/mavros
+    uri: https://github.com/mavlink/mavros-release/archive/release/kinetic/mavros/0.26.0-0.tar.gz
+    version: mavros-release-release-kinetic-mavros-0.26.0-0
+- tar:
+    local-name: mavros/mavros_extras
+    uri: https://github.com/mavlink/mavros-release/archive/release/kinetic/mavros_extras/0.26.0-0.tar.gz
+    version: mavros-release-release-kinetic-mavros_extras-0.26.0-0
+- tar:
+    local-name: mavros/mavros_msgs
+    uri: https://github.com/mavlink/mavros-release/archive/release/kinetic/mavros_msgs/0.26.0-0.tar.gz
+    version: mavros-release-release-kinetic-mavros_msgs-0.26.0-0
+- tar:
+    local-name: message_generation
+    uri: https://github.com/ros-gbp/message_generation-release/archive/release/kinetic/message_generation/0.4.0-0.tar.gz
+    version: message_generation-release-release-kinetic-message_generation-0.4.0-0
+- tar:
+    local-name: message_runtime
+    uri: https://github.com/ros-gbp/message_runtime-release/archive/release/kinetic/message_runtime/0.4.12-0.tar.gz
+    version: message_runtime-release-release-kinetic-message_runtime-0.4.12-0
+- tar:
+    local-name: nodelet_core/nodelet
+    uri: https://github.com/ros-gbp/nodelet_core-release/archive/release/kinetic/nodelet/1.9.14-0.tar.gz
+    version: nodelet_core-release-release-kinetic-nodelet-1.9.14-0
+- tar:
+    local-name: orocos_kinematics_dynamics/orocos_kdl
+    uri: https://github.com/smits/orocos-kdl-release/archive/release/kinetic/orocos_kdl/1.3.1-0.tar.gz
+    version: orocos-kdl-release-release-kinetic-orocos_kdl-1.3.1-0
+- tar:
+    local-name: orocos_kinematics_dynamics/python_orocos_kdl
+    uri: https://github.com/smits/orocos-kdl-release/archive/release/kinetic/python_orocos_kdl/1.3.1-0.tar.gz
+    version: orocos-kdl-release-release-kinetic-python_orocos_kdl-1.3.1-0
+- tar:
+    local-name: pluginlib
+    uri: https://github.com/ros-gbp/pluginlib-release/archive/release/kinetic/pluginlib/1.11.3-0.tar.gz
+    version: pluginlib-release-release-kinetic-pluginlib-1.11.3-0
+- tar:
+    local-name: ros/mk
+    uri: https://github.com/ros-gbp/ros-release/archive/release/kinetic/mk/1.14.3-0.tar.gz
+    version: ros-release-release-kinetic-mk-1.14.3-0
+- tar:
+    local-name: ros/ros
+    uri: https://github.com/ros-gbp/ros-release/archive/release/kinetic/ros/1.14.3-0.tar.gz
+    version: ros-release-release-kinetic-ros-1.14.3-0
+- tar:
+    local-name: ros/rosbash
+    uri: https://github.com/ros-gbp/ros-release/archive/release/kinetic/rosbash/1.14.3-0.tar.gz
+    version: ros-release-release-kinetic-rosbash-1.14.3-0
+- tar:
+    local-name: ros/rosboost_cfg
+    uri: https://github.com/ros-gbp/ros-release/archive/release/kinetic/rosboost_cfg/1.14.3-0.tar.gz
+    version: ros-release-release-kinetic-rosboost_cfg-1.14.3-0
+- tar:
+    local-name: ros/rosbuild
+    uri: https://github.com/ros-gbp/ros-release/archive/release/kinetic/rosbuild/1.14.3-0.tar.gz
+    version: ros-release-release-kinetic-rosbuild-1.14.3-0
+- tar:
+    local-name: ros/rosclean
+    uri: https://github.com/ros-gbp/ros-release/archive/release/kinetic/rosclean/1.14.3-0.tar.gz
+    version: ros-release-release-kinetic-rosclean-1.14.3-0
+- tar:
+    local-name: ros/roscreate
+    uri: https://github.com/ros-gbp/ros-release/archive/release/kinetic/roscreate/1.14.3-0.tar.gz
+    version: ros-release-release-kinetic-roscreate-1.14.3-0
+- tar:
+    local-name: ros/roslang
+    uri: https://github.com/ros-gbp/ros-release/archive/release/kinetic/roslang/1.14.3-0.tar.gz
+    version: ros-release-release-kinetic-roslang-1.14.3-0
+- tar:
+    local-name: ros/roslib
+    uri: https://github.com/ros-gbp/ros-release/archive/release/kinetic/roslib/1.14.3-0.tar.gz
+    version: ros-release-release-kinetic-roslib-1.14.3-0
+- tar:
+    local-name: ros/rosmake
+    uri: https://github.com/ros-gbp/ros-release/archive/release/kinetic/rosmake/1.14.3-0.tar.gz
+    version: ros-release-release-kinetic-rosmake-1.14.3-0
+- tar:
+    local-name: ros/rosunit
+    uri: https://github.com/ros-gbp/ros-release/archive/release/kinetic/rosunit/1.14.3-0.tar.gz
+    version: ros-release-release-kinetic-rosunit-1.14.3-0
+- tar:
+    local-name: ros_comm/message_filters
+    uri: https://github.com/ros-gbp/ros_comm-release/archive/release/kinetic/message_filters/1.12.13-0.tar.gz
+    version: ros_comm-release-release-kinetic-message_filters-1.12.13-0
+- tar:
+    local-name: ros_comm/ros_comm
+    uri: https://github.com/ros-gbp/ros_comm-release/archive/release/kinetic/ros_comm/1.12.13-0.tar.gz
+    version: ros_comm-release-release-kinetic-ros_comm-1.12.13-0
+- tar:
+    local-name: ros_comm/rosbag
+    uri: https://github.com/ros-gbp/ros_comm-release/archive/release/kinetic/rosbag/1.12.13-0.tar.gz
+    version: ros_comm-release-release-kinetic-rosbag-1.12.13-0
+- tar:
+    local-name: ros_comm/rosbag_storage
+    uri: https://github.com/ros-gbp/ros_comm-release/archive/release/kinetic/rosbag_storage/1.12.13-0.tar.gz
+    version: ros_comm-release-release-kinetic-rosbag_storage-1.12.13-0
+- tar:
+    local-name: ros_comm/rosconsole
+    uri: https://github.com/ros-gbp/ros_comm-release/archive/release/kinetic/rosconsole/1.12.13-0.tar.gz
+    version: ros_comm-release-release-kinetic-rosconsole-1.12.13-0
+- tar:
+    local-name: ros_comm/roscpp
+    uri: https://github.com/ros-gbp/ros_comm-release/archive/release/kinetic/roscpp/1.12.13-0.tar.gz
+    version: ros_comm-release-release-kinetic-roscpp-1.12.13-0
+- tar:
+    local-name: ros_comm/rosgraph
+    uri: https://github.com/ros-gbp/ros_comm-release/archive/release/kinetic/rosgraph/1.12.13-0.tar.gz
+    version: ros_comm-release-release-kinetic-rosgraph-1.12.13-0
+- tar:
+    local-name: ros_comm/roslaunch
+    uri: https://github.com/ros-gbp/ros_comm-release/archive/release/kinetic/roslaunch/1.12.13-0.tar.gz
+    version: ros_comm-release-release-kinetic-roslaunch-1.12.13-0
+- tar:
+    local-name: ros_comm/roslz4
+    uri: https://github.com/ros-gbp/ros_comm-release/archive/release/kinetic/roslz4/1.12.13-0.tar.gz
+    version: ros_comm-release-release-kinetic-roslz4-1.12.13-0
+- tar:
+    local-name: ros_comm/rosmaster
+    uri: https://github.com/ros-gbp/ros_comm-release/archive/release/kinetic/rosmaster/1.12.13-0.tar.gz
+    version: ros_comm-release-release-kinetic-rosmaster-1.12.13-0
+- tar:
+    local-name: ros_comm/rosmsg
+    uri: https://github.com/ros-gbp/ros_comm-release/archive/release/kinetic/rosmsg/1.12.13-0.tar.gz
+    version: ros_comm-release-release-kinetic-rosmsg-1.12.13-0
+- tar:
+    local-name: ros_comm/rosnode
+    uri: https://github.com/ros-gbp/ros_comm-release/archive/release/kinetic/rosnode/1.12.13-0.tar.gz
+    version: ros_comm-release-release-kinetic-rosnode-1.12.13-0
+- tar:
+    local-name: ros_comm/rosout
+    uri: https://github.com/ros-gbp/ros_comm-release/archive/release/kinetic/rosout/1.12.13-0.tar.gz
+    version: ros_comm-release-release-kinetic-rosout-1.12.13-0
+- tar:
+    local-name: ros_comm/rosparam
+    uri: https://github.com/ros-gbp/ros_comm-release/archive/release/kinetic/rosparam/1.12.13-0.tar.gz
+    version: ros_comm-release-release-kinetic-rosparam-1.12.13-0
+- tar:
+    local-name: ros_comm/rospy
+    uri: https://github.com/ros-gbp/ros_comm-release/archive/release/kinetic/rospy/1.12.13-0.tar.gz
+    version: ros_comm-release-release-kinetic-rospy-1.12.13-0
+- tar:
+    local-name: ros_comm/rosservice
+    uri: https://github.com/ros-gbp/ros_comm-release/archive/release/kinetic/rosservice/1.12.13-0.tar.gz
+    version: ros_comm-release-release-kinetic-rosservice-1.12.13-0
+- tar:
+    local-name: ros_comm/rostest
+    uri: https://github.com/ros-gbp/ros_comm-release/archive/release/kinetic/rostest/1.12.13-0.tar.gz
+    version: ros_comm-release-release-kinetic-rostest-1.12.13-0
+- tar:
+    local-name: ros_comm/rostopic
+    uri: https://github.com/ros-gbp/ros_comm-release/archive/release/kinetic/rostopic/1.12.13-0.tar.gz
+    version: ros_comm-release-release-kinetic-rostopic-1.12.13-0
+- tar:
+    local-name: ros_comm/roswtf
+    uri: https://github.com/ros-gbp/ros_comm-release/archive/release/kinetic/roswtf/1.12.13-0.tar.gz
+    version: ros_comm-release-release-kinetic-roswtf-1.12.13-0
+- tar:
+    local-name: ros_comm/topic_tools
+    uri: https://github.com/ros-gbp/ros_comm-release/archive/release/kinetic/topic_tools/1.12.13-0.tar.gz
+    version: ros_comm-release-release-kinetic-topic_tools-1.12.13-0
+- tar:
+    local-name: ros_comm/xmlrpcpp
+    uri: https://github.com/ros-gbp/ros_comm-release/archive/release/kinetic/xmlrpcpp/1.12.13-0.tar.gz
+    version: ros_comm-release-release-kinetic-xmlrpcpp-1.12.13-0
+- tar:
+    local-name: ros_comm_msgs/rosgraph_msgs
+    uri: https://github.com/ros-gbp/ros_comm_msgs-release/archive/release/kinetic/rosgraph_msgs/1.11.2-0.tar.gz
+    version: ros_comm_msgs-release-release-kinetic-rosgraph_msgs-1.11.2-0
+- tar:
+    local-name: ros_comm_msgs/std_srvs
+    uri: https://github.com/ros-gbp/ros_comm_msgs-release/archive/release/kinetic/std_srvs/1.11.2-0.tar.gz
+    version: ros_comm_msgs-release-release-kinetic-std_srvs-1.11.2-0
+- tar:
+    local-name: ros_environment
+    uri: https://github.com/ros-gbp/ros_environment-release/archive/release/kinetic/ros_environment/1.0.0-0.tar.gz
+    version: ros_environment-release-release-kinetic-ros_environment-1.0.0-0
+- tar:
+    local-name: ros_tutorials/rospy_tutorials
+    uri: https://github.com/ros-gbp/ros_tutorials-release/archive/release/kinetic/rospy_tutorials/0.7.1-0.tar.gz
+    version: ros_tutorials-release-release-kinetic-rospy_tutorials-0.7.1-0
+- tar:
+    local-name: rosauth
+    uri: https://github.com/gt-rail-release/rosauth-release/archive/release/kinetic/rosauth/0.1.7-0.tar.gz
+    version: rosauth-release-release-kinetic-rosauth-0.1.7-0
+- tar:
+    local-name: rosbag_migration_rule
+    uri: https://github.com/ros-gbp/rosbag_migration_rule-release/archive/release/kinetic/rosbag_migration_rule/1.0.0-0.tar.gz
+    version: rosbag_migration_rule-release-release-kinetic-rosbag_migration_rule-1.0.0-0
+- tar:
+    local-name: rosbridge_suite/rosapi
+    uri: https://github.com/RobotWebTools-release/rosbridge_suite-release/archive/release/kinetic/rosapi/0.9.0-0.tar.gz
+    version: rosbridge_suite-release-release-kinetic-rosapi-0.9.0-0
+- tar:
+    local-name: rosbridge_suite/rosbridge_library
+    uri: https://github.com/RobotWebTools-release/rosbridge_suite-release/archive/release/kinetic/rosbridge_library/0.9.0-0.tar.gz
+    version: rosbridge_suite-release-release-kinetic-rosbridge_library-0.9.0-0
+- tar:
+    local-name: rosbridge_suite/rosbridge_server
+    uri: https://github.com/RobotWebTools-release/rosbridge_suite-release/archive/release/kinetic/rosbridge_server/0.9.0-0.tar.gz
+    version: rosbridge_suite-release-release-kinetic-rosbridge_server-0.9.0-0
+- tar:
+    local-name: rosbridge_suite/rosbridge_suite
+    uri: https://github.com/RobotWebTools-release/rosbridge_suite-release/archive/release/kinetic/rosbridge_suite/0.9.0-0.tar.gz
+    version: rosbridge_suite-release-release-kinetic-rosbridge_suite-0.9.0-0
+- tar:
+    local-name: rosconsole_bridge
+    uri: https://github.com/ros-gbp/rosconsole_bridge-release/archive/release/kinetic/rosconsole_bridge/0.5.1-0.tar.gz
+    version: rosconsole_bridge-release-release-kinetic-rosconsole_bridge-0.5.1-0
+- tar:
+    local-name: roscpp_core/cpp_common
+    uri: https://github.com/ros-gbp/roscpp_core-release/archive/release/kinetic/cpp_common/0.6.9-0.tar.gz
+    version: roscpp_core-release-release-kinetic-cpp_common-0.6.9-0
+- tar:
+    local-name: roscpp_core/roscpp_serialization
+    uri: https://github.com/ros-gbp/roscpp_core-release/archive/release/kinetic/roscpp_serialization/0.6.9-0.tar.gz
+    version: roscpp_core-release-release-kinetic-roscpp_serialization-0.6.9-0
+- tar:
+    local-name: roscpp_core/roscpp_traits
+    uri: https://github.com/ros-gbp/roscpp_core-release/archive/release/kinetic/roscpp_traits/0.6.9-0.tar.gz
+    version: roscpp_core-release-release-kinetic-roscpp_traits-0.6.9-0
+- tar:
+    local-name: roscpp_core/rostime
+    uri: https://github.com/ros-gbp/roscpp_core-release/archive/release/kinetic/rostime/0.6.9-0.tar.gz
+    version: roscpp_core-release-release-kinetic-rostime-0.6.9-0
+- tar:
+    local-name: roslint
+    uri: https://github.com/ros-gbp/roslint-release/archive/release/kinetic/roslint/0.11.0-0.tar.gz
+    version: roslint-release-release-kinetic-roslint-0.11.0-0
+- tar:
+    local-name: roslisp
+    uri: https://github.com/ros-gbp/roslisp-release/archive/release/kinetic/roslisp/1.9.21-0.tar.gz
+    version: roslisp-release-release-kinetic-roslisp-1.9.21-0
+- tar:
+    local-name: rospack
+    uri: https://github.com/ros-gbp/rospack-release/archive/release/kinetic/rospack/2.4.4-0.tar.gz
+    version: rospack-release-release-kinetic-rospack-2.4.4-0
+- tar:
+    local-name: rosserial/rosserial
+    uri: https://github.com/ros-gbp/rosserial-release/archive/release/kinetic/rosserial/0.7.7-0.tar.gz
+    version: rosserial-release-release-kinetic-rosserial-0.7.7-0
+- tar:
+    local-name: rosserial/rosserial_client
+    uri: https://github.com/ros-gbp/rosserial-release/archive/release/kinetic/rosserial_client/0.7.7-0.tar.gz
+    version: rosserial-release-release-kinetic-rosserial_client-0.7.7-0
+- tar:
+    local-name: rosserial/rosserial_msgs
+    uri: https://github.com/ros-gbp/rosserial-release/archive/release/kinetic/rosserial_msgs/0.7.7-0.tar.gz
+    version: rosserial-release-release-kinetic-rosserial_msgs-0.7.7-0
+- tar:
+    local-name: rosserial/rosserial_python
+    uri: https://github.com/ros-gbp/rosserial-release/archive/release/kinetic/rosserial_python/0.7.7-0.tar.gz
+    version: rosserial-release-release-kinetic-rosserial_python-0.7.7-0
+- tar:
+    local-name: std_msgs
+    uri: https://github.com/ros-gbp/std_msgs-release/archive/release/kinetic/std_msgs/0.5.11-0.tar.gz
+    version: std_msgs-release-release-kinetic-std_msgs-0.5.11-0
+- tar:
+    local-name: unique_identifier/uuid_msgs
+    uri: https://github.com/ros-geographic-info/unique_identifier-release/archive/release/kinetic/uuid_msgs/1.0.5-0.tar.gz
+    version: unique_identifier-release-release-kinetic-uuid_msgs-1.0.5-0
+- tar:
+    local-name: urdf/urdf
+    uri: https://github.com/ros-gbp/urdf-release/archive/release/kinetic/urdf/1.12.12-0.tar.gz
+    version: urdf-release-release-kinetic-urdf-1.12.12-0
+- tar:
+    local-name: urdf/urdf_parser_plugin
+    uri: https://github.com/ros-gbp/urdf-release/archive/release/kinetic/urdf_parser_plugin/1.12.12-0.tar.gz
+    version: urdf-release-release-kinetic-urdf_parser_plugin-1.12.12-0
+- tar:
+    local-name: usb_cam
+    uri: https://github.com/ros-gbp/usb_cam-release/archive/release/kinetic/usb_cam/0.3.5-0.tar.gz
+    version: usb_cam-release-release-kinetic-usb_cam-0.3.5-0
+- tar:
+    local-name: vision_opencv/cv_bridge
+    uri: https://github.com/ros-gbp/vision_opencv-release/archive/release/kinetic/cv_bridge/1.12.8-0.tar.gz
+    version: vision_opencv-release-release-kinetic-cv_bridge-1.12.8-0
+- tar:
+    local-name: web_video_server
+    uri: https://github.com/RobotWebTools-release/web_video_server-release/archive/release/kinetic/web_video_server/0.1.0-0.tar.gz
+    version: web_video_server-release-release-kinetic-web_video_server-0.1.0-0

builder/assets/kinetic-rosdep-clever.yaml

@@ -0,0 +1,528 @@
+actionlib:
+  debian:
+    stretch: [ros-kinetic-actionlib]
+actionlib_msgs:
+  debian:
+    stretch: [ros-kinetic-actionlib-msgs]
+angles:
+  debian:
+    stretch: [ros-kinetic-angles]
+async_web_server_cpp:
+  debian:
+    stretch: [ros-kinetic-async-web-server-cpp]
+bond:
+  debian:
+    stretch: [ros-kinetic-bond]
+bondcpp:
+  debian:
+    stretch: [ros-kinetic-bondcpp]
+bondpy:
+  debian:
+    stretch: [ros-kinetic-bondpy]
+camera_calibration_parsers:
+  debian:
+    stretch: [ros-kinetic-camera-calibration-parsers]
+camera_info_manager:
+  debian:
+    stretch: [ros-kinetic-camera-info-manager]
+catkin:
+  debian:
+    stretch: [ros-kinetic-catkin]
+class_loader:
+  debian:
+    stretch: [ros-kinetic-class-loader]
+cmake_modules:
+  debian:
+    stretch: [ros-kinetic-cmake-modules]
+cpp_common:
+  debian:
+    stretch: [ros-kinetic-cpp-common]
+cv_bridge:
+  debian:
+    stretch: [ros-kinetic-cv-bridge]
+cv_camera:
+  debian:
+    stretch: [ros-kinetic-cv-camera=0.3.1-0stretch]
+diagnostic_msgs:
+  debian:
+    stretch: [ros-kinetic-diagnostic-msgs]
+diagnostic_updater:
+  debian:
+    stretch: [ros-kinetic-diagnostic-updater]
+eigen_conversions:
+  debian:
+    stretch: [ros-kinetic-eigen-conversions]
+gencpp:
+  debian:
+    stretch: [ros-kinetic-gencpp]
+geneus:
+  debian:
+    stretch: [ros-kinetic-geneus]
+genlisp:
+  debian:
+    stretch: [ros-kinetic-genlisp]
+genmsg:
+  debian:
+    stretch: [ros-kinetic-genmsg]
+gennodejs:
+  debian:
+    stretch: [ros-kinetic-gennodejs]
+genpy:
+  debian:
+    stretch: [ros-kinetic-genpy]
+geographic_msgs:
+  debian:
+    stretch: [ros-kinetic-geographic-msgs]
+geometry_msgs:
+  debian:
+    stretch: [ros-kinetic-geometry-msgs]
+image_transport:
+  debian:
+    stretch: [ros-kinetic-image-transport]
+libmavconn:
+  debian:
+    stretch: [ros-kinetic-libmavconn]
+lxml:
+  debian:
+    stretch: [python-lxml=3.7.1-1]
+mavlink:
+  debian:
+    stretch: [ros-kinetic-mavlink]
+mavros:
+  debian:
+    stretch: [ros-kinetic-mavros]
+mavros_msgs:
+  debian:
+    stretch: [ros-kinetic-mavros-msgs]
+mavros_extras:
+  debian:
+    stretch: [ros-kinetic-mavros-extras]
+message_filters:
+  debian:
+    stretch: [ros-kinetic-message-filters]
+message_generation:
+  debian:
+    stretch: [ros-kinetic-message-generation]
+message_runtime:
+  debian:
+    stretch: [ros-kinetic-message-runtime]
+mjpg-streamer:
+  debian:
+    stretch: [mjpg-streamer=2.0]
+mk:
+  debian:
+    stretch: [ros-kinetic-mk]
+nav_msgs:
+  debian:
+    stretch: [ros-kinetic-nav-msgs]
+nodelet:
+  debian:
+    stretch: [ros-kinetic-nodelet]
+opencv3:
+  debian:
+    stretch: [ros-kinetic-opencv3]
+orocos_kdl:
+  debian:
+    stretch: [ros-kinetic-orocos-kdl]
+pluginlib:
+  debian:
+    stretch: [ros-kinetic-pluginlib]
+python_orocos_kdl:
+  debian:
+    stretch: [ros-kinetic-python-orocos-kdl]
+ros:
+  debian:
+    stretch: [ros-kinetic-ros]
+ros_comm:
+  debian:
+    stretch: [ros-kinetic-ros-comm]
+ros_environment:
+  debian:
+    stretch: [ros-kinetic-ros-environment]
+rosapi:
+  debian:
+    stretch: [ros-kinetic-rosapi]
+rosauth:
+  debian:
+    stretch: [ros-kinetic-rosauth]
+rosbag:
+  debian:
+    stretch: [ros-kinetic-rosbag]
+rosbag_migration_rule:
+  debian:
+    stretch: [ros-kinetic-rosbag-migration-rule]
+rosbag_storage:
+  debian:
+    stretch: [ros-kinetic-rosbag-storage]
+rosbash:
+  debian:
+    stretch: [ros-kinetic-rosbash]
+rosboost_cfg:
+  debian:
+    stretch: [ros-kinetic-rosboost-cfg]
+rosbridge_library:
+  debian:
+    stretch: [ros-kinetic-rosbridge-library]
+rosbridge_server:
+  debian:
+    stretch: [ros-kinetic-rosbridge-server]
+rosbuild:
+  debian:
+    stretch: [ros-kinetic-rosbuild]
+rosclean:
+  debian:
+    stretch: [ros-kinetic-rosclean]
+rosconsole:
+  debian:
+    stretch: [ros-kinetic-rosconsole]
+rosconsole_bridge:
+  debian:
+    stretch: [ros-kinetic-rosconsole-bridge]
+roscpp:
+  debian:
+    stretch: [ros-kinetic-roscpp]
+roscpp_serialization:
+  debian:
+    stretch: [ros-kinetic-roscpp-serialization]
+roscpp_traits:
+  debian:
+    stretch: [ros-kinetic-roscpp-traits]
+roscreate:
+  debian:
+    stretch: [ros-kinetic-roscreate]
+rosgraph:
+  debian:
+    stretch: [ros-kinetic-rosgraph]
+rosgraph_msgs:
+  debian:
+    stretch: [ros-kinetic-rosgraph-msgs]
+roslang:
+  debian:
+    stretch: [ros-kinetic-roslang]
+roslaunch:
+  debian:
+    stretch: [ros-kinetic-roslaunch]
+roslib:
+  debian:
+    stretch: [ros-kinetic-roslib]
+roslint:
+  debian:
+    stretch: [ros-kinetic-roslint]
+roslisp:
+  debian:
+    stretch: [ros-kinetic-roslisp]
+roslz4:
+  debian:
+    stretch: [ros-kinetic-roslz4]
+rosmake:
+  debian:
+    stretch: [ros-kinetic-rosmake]
+rosmaster:
+  debian:
+    stretch: [ros-kinetic-rosmaster]
+rosmsg:
+  debian:
+    stretch: [ros-kinetic-rosmsg]
+rosnode:
+  debian:
+    stretch: [ros-kinetic-rosnode]
+rosout:
+  debian:
+    stretch: [ros-kinetic-rosout]
+rospack:
+  debian:
+    stretch: [ros-kinetic-rospack]
+rosparam:
+  debian:
+    stretch: [ros-kinetic-rosparam]
+rospy:
+  debian:
+    stretch: [ros-kinetic-rospy]
+rospy_tutorials:
+  debian:
+    stretch: [ros-kinetic-rospy-tutorials]
+rosserial_client:
+  debian:
+    stretch: [ros-kinetic-rosserial-client]
+rosserial_msgs:
+  debian:
+    stretch: [ros-kinetic-rosserial-msgs]
+rosserial_python:
+  debian:
+    stretch: [ros-kinetic-rosserial-python]
+rosservice:
+  debian:
+    stretch: [ros-kinetic-rosservice]
+rostest:
+  debian:
+    stretch: [ros-kinetic-rostest]
+rostime:
+  debian:
+    stretch: [ros-kinetic-rostime]
+rostopic:
+  debian:
+    stretch: [ros-kinetic-rostopic]
+rosunit:
+  debian:
+    stretch: [ros-kinetic-rosunit]
+roswtf:
+  debian:
+    stretch: [ros-kinetic-roswtf]
+sensor_msgs:
+  debian:
+    stretch: [ros-kinetic-sensor-msgs]
+smclib:
+  debian:
+    stretch: [ros-kinetic-smclib]
+std_msgs:
+  debian:
+    stretch: [ros-kinetic-std-msgs]
+std_srvs:
+  debian:
+    stretch: [ros-kinetic-std-srvs]
+stereo_msgs:
+  debian:
+    stretch: [ros-kinetic-stereo-msgs]
+tf2:
+  debian:
+    stretch: [ros-kinetic-tf2]
+tf2_bullet:
+  debian:
+    stretch: [ros-kinetic-tf2-bullet]
+tf2_eigen:
+  debian:
+    stretch: [ros-kinetic-tf2-eigen]
+tf2_geometry_msgs:
+  debian:
+    stretch: [ros-kinetic-tf2-geometry-msgs]
+tf2_kdl:
+  debian:
+    stretch: [ros-kinetic-tf2-kdl]
+tf2_msgs:
+  debian:
+    stretch: [ros-kinetic-tf2-msgs]
+tf2_py:
+  debian:
+    stretch: [ros-kinetic-tf2-py]
+tf2_ros:
+  debian:
+    stretch: [ros-kinetic-tf2-ros]
+tf2_sensor_msgs:
+  debian:
+    stretch: [ros-kinetic-tf2-sensor-msgs]
+tf2_tools:
+  debian:
+    stretch: [ros-kinetic-tf2-tools]
+tf:
+  debian:
+    stretch: [ros-kinetic-tf]
+topic_tools:
+  debian:
+    stretch: [ros-kinetic-topic-tools]
+trajectory_msgs:
+  debian:
+    stretch: [ros-kinetic-trajectory-msgs]
+urdf:
+  debian:
+    stretch: [ros-kinetic-urdf]
+urdf_parser_plugin:
+  debian:
+    stretch: [ros-kinetic-urdf-parser-plugin]
+uuid_msgs:
+  debian:
+    stretch: [ros-kinetic-uuid-msgs]
+visualization_msgs:
+  debian:
+    stretch: [ros-kinetic-visualization-msgs]
+xmlrpcpp:
+  debian:
+    stretch: [ros-kinetic-xmlrpcpp]
+boost:
+  debian:
+    stretch: [libboost-all-dev]
+bullet:
+  debian:
+    stretch: [libbullet-dev]
+bzip2:
+  debian:
+    stretch: [libbz2-dev]
+cmake:
+  debian:
+    stretch: [cmake]
+cppunit:
+  debian:
+    stretch: [libcppunit-dev]
+eigen:
+  debian:
+    stretch: [libeigen3-dev]
+geographiclib-tools:
+  debian:
+    stretch: [geographiclib-tools]
+geographiclib:
+  debian:
+    stretch: [libgeographic-dev]
+google-mock:
+  debian:
+    stretch: [google-mock]
+graphviz:
+  debian:
+    stretch: [graphviz]
+gtest:
+  debian:
+    stretch: [libgtest-dev]
+libconsole-bridge-dev:
+  debian:
+    stretch: [libconsole-bridge-dev]
+libjpeg:
+  debian:
+    stretch: [libjpeg-dev]
+libpng-dev:
+  debian:
+    stretch: [libpng-dev]
+libpoco-dev:
+  debian:
+    stretch: [libpoco-dev]
+libssl-dev:
+  debian:
+    stretch: [libssl-dev]
+libtiff-dev:
+  debian:
+    stretch: [libtiff5-dev]
+liburdfdom-dev:
+  debian:
+    stretch: [liburdfdom-dev]
+liburdfdom-headers-dev:
+  debian:
+    stretch: [liburdfdom-headers-dev]
+libv4l-dev:
+  debian:
+    stretch: [libv4l-dev]
+libvtk-qt:
+  debian:
+    stretch: [libvtk6-qt-dev]
+libwebp-dev:
+  debian:
+    stretch: [libwebp-dev]
+log4cxx:
+  debian:
+    stretch: [liblog4cxx-dev]
+lz4:
+  debian:
+    stretch: [liblz4-dev]
+pkg-config:
+  debian:
+    stretch: [pkg-config]
+protobuf:
+  debian:
+    stretch: [libprotobuf10]
+python-bson:
+  debian:
+    stretch: [python-bson]
+python-catkin-pkg:
+  debian:
+    stretch: [python-catkin-pkg]
+python-coverage:
+  debian:
+    stretch: [python-coverage]
+python-defusedxml:
+  debian:
+    stretch: [python-defusedxml]
+python-empy:
+  debian:
+    stretch: [python-empy]
+python-future:
+  debian:
+    stretch: [python-future]
+python-imaging:
+  debian:
+    stretch: [python-imaging]
+python-lxml:
+  debian:
+    stretch: [python-lxml]
+python-mock:
+  debian:
+    stretch: [python-mock]
+python-netifaces:
+  debian:
+    stretch: [python-netifaces]
+python-nose:
+  debian:
+    stretch: [python-nose]
+python-numpy:
+  debian:
+    stretch: [python-numpy]
+python-paramiko:
+  debian:
+    stretch: [python-paramiko]
+python-rosdep:
+  debian:
+    stretch: [python-rosdep]
+python-rospkg:
+  debian:
+    stretch: [python-rospkg]
+python-serial:
+  debian:
+    stretch: [python-serial]
+python-setuptools:
+  debian:
+    stretch: [python-setuptools]
+python-sip:
+  debian:
+    stretch: [python-sip-dev]
+python-tornado:
+  debian:
+    stretch: [python-tornado]
+python-twisted-core:
+  debian:
+    stretch: [python-twisted-core]
+python-websocket:
+  debian:
+    stretch: [python-websocket]
+python-wxtools:
+  debian:
+    stretch: [python-wxtools]
+python-yaml:
+  debian:
+    stretch: [python-yaml]
+python:
+  debian:
+    stretch: [python-dev]
+sbcl:
+  debian:
+    stretch: [sbcl]
+tinyxml2:
+  debian:
+    stretch: [libtinyxml2-dev]
+tinyxml:
+  debian:
+    stretch: [libtinyxml-dev]
+uuid:
+  debian:
+    stretch: [uuid-dev]
+web_video_server:
+  debian:
+    stretch: [ros-kinetic-web-video-server]
+v4l-utils:
+  debian:
+    stretch: [v4l-utils]
+yaml-cpp:
+  debian:
+    stretch: [libyaml-cpp-dev]
+zlib:
+  debian:
+    stretch: [zlib1g-dev]
+compressed_depth_image_transport:
+  debian:
+    stretch: [ros-kinetic-compressed-depth-image-transport]
+compressed_image_transport:
+  debian:
+    stretch: [ros-kinetic-compressed-image-transport]
+dynamic_reconfigure:
+  debian:
+    stretch: [ros-kinetic-dynamic-reconfigure]
+theora_image_transport:
+  debian:
+    stretch: [ros-kinetic-theora-image-transport]
+libogg:
+  debian:
+    stretch: [libtheora0=1.1.1+dfsg.1-14]

builder/assets/monkey-clever

@@ -0,0 +1,57 @@
+# Default Host - Configuration
+# ============================
+# Here the variable principals of the program are defined in respect
+# to the configuration of the different types of directives.
+
+[HOST]
+    # ServerName:
+    # -----------
+    # Allow you to set a host and domain name (e.g monkey.linuxchile.cl). If
+    # you are working in a local network just set your IP address or if you
+    # are working like localhost set your loopback address (127.0.0.1).
+
+    ServerName 0.0.0.0
+
+    # DocumentRoot:
+    # -------------
+    # This variable corresponds to the location of the main server directory
+    # of the web pages, where the files of your site are located.
+    #
+    # Example:
+    #      DocumentRoot /home/krypton/htdocs
+
+    DocumentRoot /usr/share/monkey-static
+
+    # Redirect:
+    # ---------
+    # Under specific conditions, you may want the server performs a HTTP
+    # redirect when this Virtual Host is reach. If that is the case, append
+    # to the Redirect key the value of the address where to redirect the
+    # HTTP client.
+    #
+    # Redirect http://monkey-project.com
+
+[LOGGER]
+    # AccessLog:
+    # ----------
+    # Registration file of correct request.
+
+    AccessLog /var/log/monkey-clever/access.log
+
+    # ErrorLog:
+    # ---------
+    # Registration file of incorrect request.
+
+    ErrorLog /var/log/monkey-clever/error.log
+
+[ERROR_PAGES]
+    404  404.html
+
+[HANDLERS]
+    # FastCGI
+    # =======
+    # Match /.*\.php fastcgi
+
+    # CGI
+    # ===
+    # Match /cgi-bin/.*\.cgi cgi

builder/assets/monkey.service

@@ -0,0 +1,9 @@
+[Unit]
+Description=Monkey web-server
+
+[Service]
+ExecStart=/usr/sbin/monkey --port 80 --workers 1
+Restart=on-abort
+
+[Install]
+WantedBy=multi-user.target

builder/assets/roscore.service

@@ -3,7 +3,7 @@ Description=Launcher for the ROS master, parameter server and rosout logging nod
 After=network.target
 
 [Service]
-EnvironmentFile=/home/pi/catkin_ws/src/clever/deploy/roscore.env
+EnvironmentFile=/lib/systemd/system/roscore.env
 ExecStart=/opt/ros/kinetic/bin/roscore
 Restart=on-abort
 

builder/image-build.sh

@@ -0,0 +1,106 @@
+#! /usr/bin/env bash
+
+#
+# Script for build the image. Used builder script of the target repo
+# For build: docker run --privileged -it --rm -v /dev:/dev -v $(pwd):/builder/repo smirart/builder
+#
+# Copyright (C) 2018 Copter Express Technologies
+#
+# Author: Artem Smirnov <urpylka@gmail.com>
+#
+
+set -e # Exit immidiately on non-zero result
+
+SOURCE_IMAGE="http://repo.coex.space/2018-06-27-raspbian-stretch-lite.zip"
+
+export DEBIAN_FRONTEND=${DEBIAN_FRONTEND:='noninteractive'}
+export LANG=${LANG:='C.UTF-8'}
+export LC_ALL=${LC_ALL:='C.UTF-8'}
+
+echo_stamp() {
+  # TEMPLATE: echo_stamp <TEXT> <TYPE>
+  # TYPE: SUCCESS, ERROR, INFO
+
+  # More info there https://www.shellhacks.com/ru/bash-colors/
+
+  TEXT="$(date '+[%Y-%m-%d %H:%M:%S]') $1"
+  TEXT="\e[1m$TEXT\e[0m" # BOLD
+
+  case "$2" in
+    SUCCESS)
+    TEXT="\e[32m${TEXT}\e[0m";; # GREEN
+    ERROR)
+    TEXT="\e[31m${TEXT}\e[0m";; # RED
+    *)
+    TEXT="\e[34m${TEXT}\e[0m";; # BLUE
+  esac
+  echo -e ${TEXT}
+}
+
+BUILDER_DIR="/builder"
+REPO_DIR="${BUILDER_DIR}/repo"
+SCRIPTS_DIR="${REPO_DIR}/builder"
+IMAGES_DIR="${REPO_DIR}/images"
+
+[[ ! -d ${SCRIPTS_DIR} ]] && (echo_stamp "Directory ${SCRIPTS_DIR} doesn't exist" "ERROR"; exit 1)
+[[ ! -d ${IMAGES_DIR} ]] && mkdir ${IMAGES_DIR} && echo_stamp "Directory ${IMAGES_DIR} was created successful" "SUCCESS"
+
+if [[ -z ${TRAVIS_TAG} ]]; then IMAGE_VERSION="$(cd ${REPO_DIR}; git log --format=%h -1)"; else IMAGE_VERSION="${TRAVIS_TAG}"; fi
+# IMAGE_VERSION="${TRAVIS_TAG:=$(cd ${REPO_DIR}; git log --format=%h -1)}"
+REPO_URL="$(cd ${REPO_DIR}; git remote --verbose | grep origin | grep fetch | cut -f2 | cut -d' ' -f1 | sed 's/git@github\.com\:/https\:\/\/github.com\//')"
+REPO_NAME="$(basename -s '.git' ${REPO_URL})"
+IMAGE_NAME="${REPO_NAME}_${IMAGE_VERSION}.img"
+IMAGE_PATH="${IMAGES_DIR}/${IMAGE_NAME}"
+
+get_image() {
+  # TEMPLATE: get_image <IMAGE_PATH> <RPI_DONWLOAD_URL> 
+  local BUILD_DIR=$(dirname $1)
+  local RPI_ZIP_NAME=$(basename $2)
+  local RPI_IMAGE_NAME=$(echo ${RPI_ZIP_NAME} | sed 's/zip/img/')
+
+  if [ ! -e "${BUILD_DIR}/${RPI_ZIP_NAME}" ]; then
+    echo_stamp "Downloading original Linux distribution" \
+    && wget -nv -O ${BUILD_DIR}/${RPI_ZIP_NAME} $2 \
+    && echo_stamp "Downloading complete" "SUCCESS" \
+    || (echo_stamp "Downloading was failed!" "ERROR"; exit 1)
+  else echo_stamp "Linux distribution already donwloaded"; fi
+
+  echo_stamp "Unzipping Linux distribution image" \
+  && unzip -p ${BUILD_DIR}/${RPI_ZIP_NAME} ${RPI_IMAGE_NAME} > $1 \
+  && echo_stamp "Unzipping complete" "SUCCESS" \
+  || (echo_stamp "Unzipping was failed!" "ERROR"; exit 1)
+}
+
+get_image ${IMAGE_PATH} ${SOURCE_IMAGE}
+
+# Make free space
+${BUILDER_DIR}/image-resize.sh ${IMAGE_PATH} max '7G'
+
+${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/init_rpi.sh' '/root/'
+${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/hardware_setup.sh' '/root/'
+${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} exec ${SCRIPTS_DIR}'/image-init.sh' ${IMAGE_VERSION} ${SOURCE_IMAGE}
+
+# Monkey
+${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/'
+
+# 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_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/monkey.service' '/lib/systemd/system/'
+# software install
+${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} exec ${SCRIPTS_DIR}'/image-software.sh'
+# network setup
+${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} exec ${SCRIPTS_DIR}'/image-network.sh'
+
+# If RPi then use a one thread to build a ROS package on RPi, else use all
+[[ $(arch) == 'armv7l' ]] && NUMBER_THREADS=1 || NUMBER_THREADS=$(nproc --all)
+# Clever
+${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/clever.service' '/lib/systemd/system/'
+${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/roscore.env' '/lib/systemd/system/'
+${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/roscore.service' '/lib/systemd/system/'
+${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/kinetic-rosdep-clever.yaml' '/etc/ros/rosdep/'
+# ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/kinetic-ros-clever.rosinstall' '/home/pi/ros_catkin_ws/'
+${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} exec ${SCRIPTS_DIR}'/image-ros.sh' ${REPO_URL} ${IMAGE_VERSION} false false ${NUMBER_THREADS} 
+
+${BUILDER_DIR}/image-resize.sh ${IMAGE_PATH}

builder/image-init.sh

@@ -0,0 +1,54 @@
+#! /usr/bin/env bash
+
+#
+# Script for initialisation image
+#
+# Copyright (C) 2018 Copter Express Technologies
+#
+# Author: Artem Smirnov <urpylka@gmail.com>
+#
+
+set -e # Exit immidiately on non-zero result
+
+echo_stamp() {
+  # TEMPLATE: echo_stamp <TEXT> <TYPE>
+  # TYPE: SUCCESS, ERROR, INFO
+
+  # More info there https://www.shellhacks.com/ru/bash-colors/
+
+  TEXT="$(date '+[%Y-%m-%d %H:%M:%S]') $1"
+  TEXT="\e[1m$TEXT\e[0m" # BOLD
+
+  case "$2" in
+    SUCCESS)
+    TEXT="\e[32m${TEXT}\e[0m";; # GREEN
+    ERROR)
+    TEXT="\e[31m${TEXT}\e[0m";; # RED
+    *)
+    TEXT="\e[34m${TEXT}\e[0m";; # BLUE
+  esac
+  echo -e ${TEXT}
+}
+
+echo_stamp "Write CLEVER information"
+
+# Clever image version
+echo "$1" >> /etc/clever_version
+# Origin image file name
+echo "${2%.*}" >> /etc/clever_origin
+
+echo_stamp "Write magic script to /etc/rc.local"
+MAGIC_SCRIPT="sudo /root/init_rpi.sh; sudo sed -i '/sudo \\\/root\\\/init_rpi.sh/d' /etc/rc.local && sudo reboot"
+sed -i "19a${MAGIC_SCRIPT}" /etc/rc.local
+
+# It needs for autosizer.sh & maybe that is correct
+echo_stamp "Change boot partition"
+sed -i 's/root=[^ ]*/root=\/dev\/mmcblk0p2/' /boot/cmdline.txt
+sed -i 's/.*  \/boot           vfat    defaults          0       2$/\/dev\/mmcblk0p1  \/boot           vfat    defaults          0       2/' /etc/fstab
+sed -i 's/.*  \/               ext4    defaults,noatime  0       1$/\/dev\/mmcblk0p2  \/               ext4    defaults,noatime  0       1/' /etc/fstab
+
+echo_stamp "Set max space for syslogs"
+# https://unix.stackexchange.com/questions/139513/how-to-clear-journalctl
+sed -i 's/#SystemMaxUse=/SystemMaxUse=200M/' /etc/systemd/journald.conf
+
+echo_stamp "End of init image"

builder/image-network.sh

@@ -0,0 +1,73 @@
+#! /usr/bin/env bash
+
+#
+# Script for network configure
+#
+# Copyright (C) 2018 Copter Express Technologies
+#
+# Author: Artem Smirnov <urpylka@gmail.com>
+#
+
+set -e # Exit immidiately on non-zero result
+
+echo_stamp() {
+  # TEMPLATE: echo_stamp <TEXT> <TYPE>
+  # TYPE: SUCCESS, ERROR, INFO
+
+  # More info there https://www.shellhacks.com/ru/bash-colors/
+
+  TEXT="$(date '+[%Y-%m-%d %H:%M:%S]') $1"
+  TEXT="\e[1m$TEXT\e[0m" # BOLD
+
+  case "$2" in
+    SUCCESS)
+    TEXT="\e[32m${TEXT}\e[0m";; # GREEN
+    ERROR)
+    TEXT="\e[31m${TEXT}\e[0m";; # RED
+    *)
+    TEXT="\e[34m${TEXT}\e[0m";; # BLUE
+  esac
+  echo -e ${TEXT}
+}
+
+echo_stamp "#1 Write to /etc/wpa_supplicant/wpa_supplicant.conf"
+
+# TODO: Use wpa_cli insted direct file edit
+cat << EOF >> /etc/wpa_supplicant/wpa_supplicant.conf
+network={
+    ssid="CLEVER"
+    psk="cleverwifi"
+    mode=2
+    proto=RSN
+    key_mgmt=WPA-PSK
+    pairwise=CCMP
+    group=CCMP
+    auth_alg=OPEN
+}
+EOF
+
+echo_stamp "#2 Write STATIC to /etc/dhcpcd.conf"
+
+cat << EOF >> /etc/dhcpcd.conf
+interface wlan0
+static ip_address=192.168.11.1/24
+EOF
+
+echo_stamp "#3 Write dhcp-config to /etc/dnsmasq.conf"
+
+cat << EOF >> /etc/dnsmasq.conf
+interface=wlan0
+address=/clever/coex/192.168.11.1
+dhcp-range=192.168.11.100,192.168.11.200,12h
+no-hosts
+filterwin2k
+bogus-priv
+domain-needed
+quiet-dhcp6
+EOF
+
+#echo_stamp "#4 Write magic script for rename SSID to /etc/rc.local"
+#RENAME_SSID="sudo sed -i.OLD \"s/CLEVER/CLEVER-\$(head -c 100 /dev/urandom | xxd -ps -c 100 | sed -e 's/[^0-9]//g' | cut -c 1-4)/g\" /etc/wpa_supplicant/wpa_supplicant.conf && sudo sed -i '/sudo sed/d' /etc/rc.local && sudo reboot"
+#sed -i "19a$RENAME_SSID" /etc/rc.local
+
+echo_stamp "#5 End of network installation"

builder/image-ros.sh

@@ -0,0 +1,176 @@
+#! /usr/bin/env bash
+
+#
+# Script for build the image. Used builder script of the target repo
+# For build: docker run --privileged -it --rm -v /dev:/dev -v $(pwd):/builder/repo smirart/builder
+#
+# Copyright (C) 2018 Copter Express Technologies
+#
+# Author: Artem Smirnov <urpylka@gmail.com>
+#
+
+set -e # Exit immidiately on non-zero result
+
+REPO=$1
+REF=$2
+INSTALL_ROS_PACK_SOURCES=$3
+DISCOVER_ROS_PACK=$4
+NUMBER_THREADS=$5
+
+echo_stamp() {
+  # TEMPLATE: echo_stamp <TEXT> <TYPE>
+  # TYPE: SUCCESS, ERROR, INFO
+
+  # More info there https://www.shellhacks.com/ru/bash-colors/
+
+  TEXT="$(date '+[%Y-%m-%d %H:%M:%S]') $1"
+  TEXT="\e[1m$TEXT\e[0m" # BOLD
+
+  case "$2" in
+    SUCCESS)
+    TEXT="\e[32m${TEXT}\e[0m";; # GREEN
+    ERROR)
+    TEXT="\e[31m${TEXT}\e[0m";; # RED
+    *)
+    TEXT="\e[34m${TEXT}\e[0m";; # BLUE
+  esac
+  echo -e ${TEXT}
+}
+
+# https://gist.github.com/letmaik/caa0f6cc4375cbfcc1ff26bd4530c2a3
+# https://github.com/travis-ci/travis-build/blob/master/lib/travis/build/templates/header.sh
+my_travis_retry() {
+  local result=0
+  local count=1
+  while [ $count -le 3 ]; do
+    [ $result -ne 0 ] && {
+      echo -e "\n${ANSI_RED}The command \"$@\" failed. Retrying, $count of 3.${ANSI_RESET}\n" >&2
+    }
+    # ! { } ignores set -e, see https://stackoverflow.com/a/4073372
+    ! { "$@"; result=$?; }
+    [ $result -eq 0 ] && break
+    count=$(($count + 1))
+    sleep 1
+  done
+
+  [ $count -gt 3 ] && {
+    echo -e "\n${ANSI_RED}The command \"$@\" failed 3 times.${ANSI_RESET}\n" >&2
+  }
+
+  return $result
+}
+
+# TODO: 'kinetic-rosdep-clever.yaml' should add only if we use our repo?
+echo_stamp "Init rosdep" \
+&& rosdep init \
+&& echo "yaml file:///etc/ros/rosdep/kinetic-rosdep-clever.yaml" >> /etc/ros/rosdep/sources.list.d/20-default.list \
+&& rosdep update
+
+resolve_rosdep() {
+  # TEMPLATE: resolve_rosdep <CATKIN_PATH> <ROS_DISTRO> <OS_DISTRO> <OS_VERSION>
+  CATKIN_PATH=$1
+  ROS_DISTRO='kinetic'
+  OS_DISTRO='debian'
+  OS_VERSION='stretch'
+
+  echo_stamp "Installing dependencies apps with rosdep in ${CATKIN_PATH}"
+  cd ${CATKIN_PATH}
+  my_travis_retry rosdep install -y --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} -r --os=${OS_DISTRO}:${OS_VERSION}
+}
+
+INSTALL_ROS_PACK_SOURCES=${INSTALL_ROS_PACK_SOURCES:='false'}
+if [ "${INSTALL_ROS_PACK_SOURCES}" = "true" ]; then
+  DISCOVER_ROS_PACK=${DISCOVER_ROS_PACK:='true'}
+  if [ "${DISCOVER_ROS_PACK}" = "false" ]; then
+    echo_stamp "Preparing ros_comm packages to kinetic-ros_comm-wet.rosinstall" \
+    && mkdir -p /home/pi/ros_catkin_ws && cd /home/pi/ros_catkin_ws \
+    && rosinstall_generator ros_comm --rosdistro kinetic --deps --wet-only --tar > kinetic-ros_comm-wet.rosinstall \
+    && wstool init -j${NUMBER_THREADS} src kinetic-ros_comm-wet.rosinstall \
+    && echo_stamp "All roscomm sources was installed!" "SUCCESS" \
+    || (echo_stamp "Some roscomm sources installation was failed!" "ERROR"; exit 1)
+
+    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 \
+    --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 \
+    && echo_stamp "All custom sources was installed!" "SUCCESS" \
+    || (echo_stamp "Some custom sources installation was failed!" "ERROR"; exit 1)
+  else
+    echo_stamp "Creating ros_catkin_ws & getting all sources using wstool" \
+    && mkdir -p /home/pi/ros_catkin_ws && cd /home/pi/ros_catkin_ws \
+    && wstool init -j${NUMBER_THREADS} src kinetic-ros-clever.rosinstall \
+    > /dev/null \
+    && echo_stamp "All CLEVER sources was installed!" "SUCCESS" \
+    || (echo_stamp "Some CLEVER sources installation was failed!" "ERROR"; exit 1)
+  fi
+
+  resolve_rosdep '/home/pi/ros_catkin_ws'
+
+  # TODO: Add refactor to origin repo
+  #echo_stamp "Refactoring usb_cam in SRC"
+  #sed -i '/#define __STDC_CONSTANT_MACROS/a\#define PIX_FMT_RGB24 AV_PIX_FMT_RGB24\n#define PIX_FMT_YUV422P AV_PIX_FMT_YUV422P' /home/pi/ros_catkin_ws/src/usb_cam/src/usb_cam.cpp
+
+  echo_stamp "Building ros_catkin_ws packages"
+  cd /home/pi/ros_catkin_ws && ./src/catkin/bin/catkin_make_isolated --install -j${NUMBER_THREADS} -DCMAKE_BUILD_TYPE=Release --install-space /opt/ros/kinetic
+
+  #echo_stamp "#11 Building light packages on 2 threads"
+  #cd /home/pi/ros_catkin_ws && ./src/catkin/bin/catkin_make_isolated --install -DCMAKE_BUILD_TYPE=Release -j2 --install-space /opt/ros/kinetic --pkg 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 diagnostic_msgs diagnostic_updater dynamic_reconfigure eigen_conversions gencpp geneus genlisp genmsg gennodejs genpy geographic_msgs geometry_msgs geometry2 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 xmlrpcpp
+
+  #echo_stamp "#12 Building heavy packages"
+  # This command uses less threads to avoid Raspberry Pi freeze
+  #cd /home/pi/ros_catkin_ws && ./src/catkin/bin/catkin_make_isolated --install -DCMAKE_BUILD_TYPE=Release -j1 --install-space /opt/ros/kinetic --pkg mavros opencv3 cv_bridge cv_camera mavros_extras web_video_server
+
+  # Install builded packages
+  # WARNING: A major bug was found when using --pkg option (catkin_make_isolated doesn't install environment files)
+  # TODO: Can we increase threads number with HDD swap?
+
+  echo_stamp "Remove build_isolated & devel_isolated from ros_catkin_ws"
+  rm -rf /home/pi/ros_catkin_ws/build_isolated /home/pi/ros_catkin_ws/devel_isolated
+  chown -Rf pi:pi /home/pi/ros_catkin_ws
+fi
+
+echo_stamp "Installing CLEVER" \
+&& git clone ${REPO} /home/pi/catkin_ws/src/clever \
+&& cd /home/pi/catkin_ws/src/clever \
+&& echo "REF: ${REF}" \
+&& git checkout ${REF} \
+&& cd /home/pi/catkin_ws \
+&& resolve_rosdep $(pwd) \
+&& my_travis_retry pip install wheel \
+&& my_travis_retry pip install -r /home/pi/catkin_ws/src/clever/clever/requirements.txt \
+&& source /opt/ros/kinetic/setup.bash \
+&& catkin_make -j${NUMBER_THREADS} -DCMAKE_BUILD_TYPE=Release \
+&& systemctl enable roscore \
+&& systemctl enable clever \
+&& echo_stamp "All CLEVER was installed!" "SUCCESS" \
+|| (echo_stamp "CLEVER installation was failed!" "ERROR"; exit 1)
+
+# 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
+
+echo_stamp "Change permissions for catkin_ws"
+chown -Rf pi:pi /home/pi/catkin_ws
+
+echo_stamp "Setup ROS environment"
+cat << EOF >> /home/pi/.bashrc
+LANG='C.UTF-8'
+LC_ALL='C.UTF-8'
+ROS_DISTRO='kinetic'
+export ROS_IP='192.168.11.1'
+source /opt/ros/kinetic/setup.bash
+source /home/pi/catkin_ws/devel/setup.bash
+EOF
+
+#echo_stamp "Removing local apt mirror"
+# Restore original sources.list
+#mv /var/sources.list.bak /etc/apt/sources.list
+# Clean apt cache
+apt-get clean -qq > /dev/null
+# Remove local mirror repository key
+#apt-key del COEX-MIRROR
+
+echo_stamp "END of ROS INSTALLATION"

builder/image-software.sh

@@ -0,0 +1,130 @@
+#! /usr/bin/env bash
+
+#
+# Script for install software to the image.
+#
+# Copyright (C) 2018 Copter Express Technologies
+#
+# Author: Artem Smirnov <urpylka@gmail.com>
+#
+
+set -e # Exit immidiately on non-zero result
+
+echo_stamp() {
+  # TEMPLATE: echo_stamp <TEXT> <TYPE>
+  # TYPE: SUCCESS, ERROR, INFO
+
+  # More info there https://www.shellhacks.com/ru/bash-colors/
+
+  TEXT="$(date '+[%Y-%m-%d %H:%M:%S]') $1"
+  TEXT="\e[1m${TEXT}\e[0m" # BOLD
+
+  case "$2" in
+    SUCCESS)
+    TEXT="\e[32m${TEXT}\e[0m";; # GREEN
+    ERROR)
+    TEXT="\e[31m${TEXT}\e[0m";; # RED
+    *)
+    TEXT="\e[34m${TEXT}\e[0m";; # BLUE
+  esac
+  echo -e ${TEXT}
+}
+
+# https://gist.github.com/letmaik/caa0f6cc4375cbfcc1ff26bd4530c2a3
+# https://github.com/travis-ci/travis-build/blob/master/lib/travis/build/templates/header.sh
+my_travis_retry() {
+  local result=0
+  local count=1
+  while [ $count -le 3 ]; do
+    [ $result -ne 0 ] && {
+      echo -e "\n${ANSI_RED}The command \"$@\" failed. Retrying, $count of 3.${ANSI_RESET}\n" >&2
+    }
+    # ! { } ignores set -e, see https://stackoverflow.com/a/4073372
+    ! { "$@"; result=$?; }
+    [ $result -eq 0 ] && break
+    count=$(($count + 1))
+    sleep 1
+  done
+
+  [ $count -gt 3 ] && {
+    echo -e "\n${ANSI_RED}The command \"$@\" failed 3 times.${ANSI_RESET}\n" >&2
+  }
+
+  return $result
+}
+
+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~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
+echo "deb http://repo.coex.space/rpi-ros-kinetic stretch main" > /etc/apt/sources.list.d/rpi-ros-kinetic.list
+echo "deb http://repo.coex.space/clever stretch main" > /etc/apt/sources.list.d/clever.list
+
+echo_stamp "Update apt cache"
+
+# TODO: FIX ERROR: /usr/bin/apt-key: 596: /usr/bin/apt-key: cannot create /dev/null: Permission denied
+apt-get update -qq
+# && apt upgrade -y
+
+echo_stamp "Software installing"
+apt-get install --no-install-recommends -y \
+unzip=6.0-21 \
+zip=3.0-11 \
+ipython=5.1.0-3 \
+ipython3=5.1.0-3 \
+screen=4.5.0-6 \
+byobu=5.112-1  \
+nmap=7.40-1 \
+lsof=4.89+dfsg-0.1 \
+git \
+dnsmasq=2.76-5+rpt1+deb9u1  \
+tmux=2.3-4 \
+vim=2:8.0.0197-4+deb9u1 \
+cmake=3.7.2-1 \
+python-pip=9.0.1-2+rpt2 \
+python3-pip=9.0.1-2+rpt2 \
+libjpeg8-dev=8d1-2 \
+tcpdump \
+ltrace \
+libpoco-dev=1.7.6+dfsg1-5+deb9u1 \
+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 \
+&& echo_stamp "Everything was installed!" "SUCCESS" \
+|| (echo_stamp "Some packages wasn't installed!" "ERROR"; exit 1)
+
+# Deny byobu to check available updates
+sed -i "s/updates_available//" /usr/share/byobu/status/status
+# sed -i "s/updates_available//" /home/pi/.byobu/status
+
+echo_stamp "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 "Setup Monkey"
+mv /etc/monkey/sites/default /etc/monkey/sites/default.orig
+mv /root/monkey-clever /etc/monkey/sites/default
+systemctl enable monkey.service
+
+echo_stamp "Add .vimrc"
+cat << EOF > /home/pi/.vimrc
+set mouse-=a
+syntax on
+autocmd BufNewFile,BufRead *.launch set syntax=xml
+EOF
+
+echo_stamp "End of software installation"

clever/CMakeLists.txt

@@ -21,6 +21,9 @@ find_package(catkin REQUIRED COMPONENTS
   tf
   tf2
   tf2_geometry_msgs
+  tf2_ros
+  image_transport
+  cv_bridge
 )
 
 
@@ -121,7 +124,7 @@ generate_messages(
 ## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
 #  INCLUDE_DIRS include
-#  LIBRARIES clever
+  LIBRARIES clever
 #  CATKIN_DEPENDS other_catkin_pkg
 #  DEPENDS system_lib
 )
@@ -137,7 +140,11 @@ include_directories(
   ${catkin_INCLUDE_DIRS}
 )
 
-## Declare a C++ library
+# Declare a C++ library
+add_library(clever
+  src/optical_flow.cpp
+)
+
 add_library(fcu_horiz
   src/fcu_horiz.cpp
 )
@@ -156,8 +163,20 @@ add_library(aruco_vpe
 ## The recommended prefix ensures that target names across packages don't collide
 add_executable(rc src/rc.cpp)
 
+add_executable(camera_markers src/camera_markers.cpp)
+
+add_executable(frames src/frames.cpp)
+
+add_executable(vpe_publisher src/vpe_publisher.cpp)
+
 target_link_libraries(rc ${catkin_LIBRARIES})
 
+target_link_libraries(camera_markers ${catkin_LIBRARIES})
+
+target_link_libraries(frames ${catkin_LIBRARIES})
+
+target_link_libraries(vpe_publisher ${catkin_LIBRARIES})
+
 ## Rename C++ executable without prefix
 ## The above recommended prefix causes long target names, the following renames the
 ## target back to the shorter version for ease of user use
@@ -169,6 +188,10 @@ target_link_libraries(rc ${catkin_LIBRARIES})
 # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
 ## Specify libraries to link a library or executable target against
+target_link_libraries(clever
+  ${catkin_LIBRARIES}
+)
+
 target_link_libraries(fcu_horiz
   ${catkin_LIBRARIES}
   "/opt/ros/kinetic/lib/libtf2_ros.so"

clever/launch/aruco.launch

@@ -1,24 +1,35 @@
 <launch>
-    <remap from="image" to="main_camera/image_raw"/>
+    <arg name="aruco_detect" default="true"/>
-    <remap from="camera_info" to="main_camera/camera_info"/>
+    <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">
+    <!-- detect aruco markers, estimate poses -->
-        <param name="frame_id" value="aruco_map_raw"/>
+    <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">
-        <param name="type" value="gridboard"/>
+        <remap from="image_raw" to="main_camera/image_raw"/>
-        <param name="markers_x" value="1"/>
+        <remap from="camera_info" to="main_camera/camera_info"/>
-        <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"/>
 
-        <!-- Custom gridboard: -->
+        <param name="length" value="0.32"/>
-        <!--<rosparam param="marker_ids">[6, 5, 4, 3, 2, 1]</rosparam>-->
+        <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">
+    <!-- estimate aruco map pose -->
-        <param name="aruco_orientation" value="local_origin"/>
+    <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">
-        <!--<param name="aruco_orientation" value="local_origin_upside_down"/>-->
+        <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

@@ -2,46 +2,50 @@
     <arg name="fcu_conn" default="usb"/>
     <arg name="fcu_ip" default="127.0.0.1"/>
     <arg name="gcs_bridge" default="tcp"/>
-    <arg name="viz" default="true"/>
-    <arg name="web_server" default="false"/>
     <arg name="web_video_server" default="true"/>
     <arg name="rosbridge" default="true"/>
     <arg name="main_camera" default="true"/>
+    <arg name="optical_flow" default="false"/>
     <arg name="aruco" default="false"/>
-    <arg name="rc" value="true"/>
+    <arg name="rc" default="true"/>
-    <arg name="fpv_camera" default="false"/>
-    <arg name="fpv_camera_device" default="/dev/v4l/by-id/usb-HD_Camera_Manufacturer_USB_2.0_Camera-video-index0"/>
     <arg name="arduino" default="false"/>
+    <arg name="vl53l1x" default="false"/>
 
     <!-- mavros -->
     <include file="$(find clever)/launch/mavros.launch">
         <arg name="fcu_conn" value="$(arg fcu_conn)"/>
         <arg name="fcu_ip" value="$(arg fcu_ip)"/>
         <arg name="gcs_bridge" value="$(arg gcs_bridge)"/>
-        <arg name="viz" value="$(arg viz)"/>
+        <arg name="viz" value="true"/>
     </include>
 
-    <!-- web server -->
-    <include file="$(find clever)/launch/web_server.launch" if="$(arg web_server)"/>
-
     <!-- 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 -->
+    <node pkg="nodelet" type="nodelet" name="optical_flow" args="load clever/optical_flow nodelet_manager" if="$(arg optical_flow)" clear_params="true">
+        <remap from="image" to="main_camera/image_raw"/>
+        <remap from="camera_info" to="main_camera/camera_info"/>
+    </node>
+
     <!-- main nodelet manager -->
     <node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager" output="screen" clear_params="true">
-        <param name="num_worker_threads" value="2"/>
+        <param name="num_worker_threads" value="4"/>
     </node>
 
     <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"/>
 
-    <!-- fcu_horiz frame -->
-    <node pkg="nodelet" type="nodelet" name="fcu_horiz" args="standalone clever/fcu_horiz" output="screen" clear_params="true"/>
 
     <!-- simplified offboard control -->
     <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="fcu_horiz"/>
+    </node>
+
 
     <!-- main camera -->
     <include file="$(find clever)/launch/main_camera.launch" if="$(arg main_camera)"/>
@@ -49,14 +53,15 @@
     <!-- rosbridge -->
     <include file="$(find rosbridge_server)/launch/rosbridge_websocket.launch" if="$(eval rosbridge or rc)"/>
 
+    <!-- vl53l1x ToF rangefinder -->
+    <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 -->
     <node name="rc" pkg="clever" type="rc" output="screen" if="$(arg rc)"/>
 
-    <!-- FPV video streaming -->
-    <include file="$(find clever)/launch/fpv_camera.launch" if="$(arg fpv_camera)">
-        <arg name="device" value="$(arg fpv_camera_device)"/>
-    </include>
-
     <!-- Arduino bridge -->
     <include file="$(find clever)/launch/arduino.launch" if="$(arg arduino)"/>
 </launch>

clever/launch/copter_visualization.launch

@@ -1,12 +0,0 @@
-<launch>
-    <remap to="mavros/local_position/pose" from="local_position"/>
-    <remap to="mavros/setpoint_position/local" from="local_setpoint"/>
-
-    <node name="copter_visualization" pkg="mavros_extras" type="copter_visualization"/>
-
-    <param name="copter_visualization/fixed_frame_id" value="local_origin"/>
-    <param name="copter_visualization/child_frame_id" value="fcu"/>
-    <param name="copter_visualization/marker_scale" value="1"/>
-    <param name="copter_visualization/max_track_size" value="500"/>
-    <param name="copter_visualization/num_rotors" value="4"/>
-</launch>

clever/launch/main_camera.launch

@@ -1,4 +1,7 @@
 <launch>
+    <!-- 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 -->
+
     <!-- 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"/>-->
 
@@ -16,8 +19,14 @@
         <!-- setting camera FPS -->
         <param name="rate" value="100"/>
         <param name="cv_cap_prop_fps" value="40"/>
+        <param name="capture_delay" value="0.02"/>
 
         <param name="image_width" value="320"/>
         <param name="image_height" value="240"/>
     </node>
+
+    <!-- camera visualization markers -->
+    <node pkg="clever" type="camera_markers" ns="main_camera" name="main_camera_markers">
+        <param name="scale" value="3.0"/>
+    </node>
 </launch>

clever/launch/mavros.launch

@@ -1,55 +1,96 @@
 <launch>
-    <arg name="fcu_conn" default="uart"/>
+    <arg name="fcu_conn" default="usb"/>
     <arg name="fcu_ip" default="127.0.0.1"/>
     <arg name="gcs_bridge" default="tcp"/>
     <arg name="viz" default="true"/>
-    <arg name="respawn" default="false"/>
+    <arg name="respawn" default="true"/>
 
-    <node pkg="mavros" type="mavros_node" name="mavros" required="false" clear_params="true" respawn="$(arg respawn)" respawn_delay="3" 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 -->
+        <!-- UART connection -->
-            <param name="fcu_url" value="/dev/ttyAMA0:921600" if="$(eval fcu_conn is None or fcu_conn == 'uart')"/>
+        <param name="fcu_url" value="/dev/ttyAMA0:921600" if="$(eval fcu_conn is None or fcu_conn == 'uart')"/>
 
-            <!-- USB connection -->
+        <!-- USB connection -->
-            <param name="fcu_url" value="/dev/ttyACM0" if="$(eval fcu_conn == 'usb')"/>
+        <param name="fcu_url" value="/dev/ttyACM0" if="$(eval fcu_conn == 'usb')"/>
 
-            <!-- sitl -->
+        <!-- sitl -->
-            <param name="fcu_url" value="udp://@$(arg fcu_ip):14557" if="$(eval fcu_conn == 'udp')"/>
+        <param name="fcu_url" value="udp://@$(arg fcu_ip):14557" if="$(eval fcu_conn == 'udp')"/>
 
-            <!-- gcs bridge -->
+        <!-- gcs bridge -->
-            <param name="gcs_url" value="tcp-l://0.0.0.0:5760" if="$(eval gcs_bridge == 'tcp')"/>
+        <param name="gcs_url" value="tcp-l://0.0.0.0:5760" if="$(eval gcs_bridge == 'tcp')"/>
-            <param name="gcs_url" value="udp://@192.168.11.14:14550" if="$(eval gcs_bridge == 'udp')"/> <!-- TODO: fix -->
+        <param name="gcs_url" value="udp://0.0.0.0:14550@14550" if="$(eval gcs_bridge == 'udp')"/>
-            <param name="gcs_url" value="udp-b://192.168.11.1:14550@" if="$(eval gcs_bridge == 'udp-b')"/>
+        <param name="gcs_url" value="udp-b://$(env ROS_IP):14550@14550" if="$(eval gcs_bridge == 'udp-b')"/>
-            <param name="gcs_url" value="" if="$(eval not gcs_bridge)"/>
+        <param name="gcs_url" value="udp-pb://$(env ROS_IP):14550@14550" if="$(eval gcs_bridge == 'udp-pb')"/>
+        <param name="gcs_url" value="" if="$(eval not gcs_bridge)"/>
+        <param name="gcs_quiet_mode" value="true"/>
+        <param name="conn/timeout" value="8"/>
 
-            <!-- default px4 params -->
+        <!-- default px4 params -->
-            <rosparam command="load" file="$(find mavros)/launch/px4_config.yaml"/>
+        <rosparam command="load" file="$(find mavros)/launch/px4_config.yaml"/>
 
-            <!-- additional params -->
+        <!-- rangefinders -->
-            <param name="local_position/frame_id" value="local_origin"/>
+        <rosparam>
-            <param name="local_position/tf/send" value="true"/>
+            distance_sensor:
-            <param name="local_position/tf/frame_id" value="local_origin"/>
+                rangefinder_0:
-            <param name="local_position/tf/child_frame_id" value="fcu"/>
+                    id: 0
-            <param name="global_position/tf/send" value="false"/>
+                    frame_id: "rangefinder"
-            <param name="imu/frame_id" value="fcu"/>
+                    orientation: PITCH_270
-            <rosparam param="plugin_blacklist">
+                    field_of_view: 0.5
-                - safety_area
+                rangefinder_1:
-                - image_pub
+                    id: 1
-                - vibration
+                    frame_id: "rangefinder"
-                - distance_sensor
+                    orientation: PITCH_270
-                - rangefinder
+                    field_of_view: 0.5
-                - 3dr_radio
+                rangefinder_2_sub:
-                - actuator_control
+                    subscriber: true
-                - hil_controls
+                    id: 2
-                - vfr_hud
+                    orientation: PITCH_270
-                - px4flow
+                rangefinder_3_sub:
-                - vision_speed_estimate
+                    subscriber: true
-                - fake_gps
+                    id: 3
-                - cam_imu_sync
+                    orientation: PITCH_270
-                - hil
+        </rosparam>
-                - adsb
+
-            </rosparam>
+        <!-- additional params -->
+        <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="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="fcu"/>
+        <rosparam param="plugin_blacklist">
+            - safety_area
+            - image_pub
+            - vibration
+            - rangefinder
+            - 3dr_radio
+            - actuator_control
+            - hil_controls
+            - vfr_hud
+            - vision_speed_estimate
+            - fake_gps
+            - cam_imu_sync
+            - hil
+            - adsb
+            - waypoint
+            - obstacle_distance
+            - setpoint_accel
+            - trajectory
+            - wind_estimation
+            - home_position
+        </rosparam>
     </node>
 
+    <!-- Rangefinders frame -->
+    <node pkg="tf2_ros" type="static_transform_publisher" name="rangefinder_frame" args="0 0 -0.05 0 1.5707963268 0 fcu rangefinder"/>
+
     <!-- Copter visualization -->
-    <include file="$(find clever)/launch/copter_visualization.launch" if="$(arg viz)"/>
+    <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="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"/>
+    </node>
 </launch>

clever/launch/sitl.launch

@@ -1,17 +1,19 @@
 <launch>
     <!-- Clever configuration for testing in sitl -->
     <arg name="ip" default="127.0.0.1"/>
-    <arg name="rosbridge" default="true"/>
+    <arg name="rosbridge" default="false"/>
 
     <include file="$(find clever)/launch/clever.launch">
         <arg name="fcu_conn" value="udp"/>
         <arg name="fcu_ip" value="$(arg ip)"/>
         <arg name="gcs_bridge" value="false"/>
+        <arg name="optical_flow" value="false"/>
+        <arg name="web_server" default="false"/>
         <arg name="web_video_server" default="false"/>
         <arg name="main_camera" default="false"/>
-        <arg name="fpv_camera" default="false"/>
         <arg name="rosbridge" value="$(arg rosbridge)"/>
-        <arg name="web_server" default="false"/>
         <arg name="aruco" default="false"/>
+        <arg name="vl53l1x" default="false"/>
+        <arg name="rc" default="false"/>
     </include>
 </launch>

clever/launch/web_server.launch

@@ -1,5 +0,0 @@
-<launch>
-    <node name="web_server" pkg="clever" type="web_server.py" output="screen">
-        <param name="path" value="$(find clever)/static"/>
-    </node>
-</launch>

clever/nodelet_plugins.xml

@@ -1,3 +1,8 @@
+<library path="lib/libclever">
+   <class name="clever/optical_flow" type="OpticalFlow" base_class_type="nodelet::Nodelet">
+      <description/>
+   </class>
+</library>
 <library path="lib/libfcu_horiz">
    <class name="clever/fcu_horiz" type="FcuHoriz" base_class_type="nodelet::Nodelet">
       <description/>

clever/package.xml

@@ -2,55 +2,40 @@
 <package>
   <name>clever</name>
   <version>0.0.1</version>
-  <description>The clever package</description>
+  <description>The CLEVER package</description>
 
-  <!-- One maintainer tag required, multiple allowed, one person per tag -->
-  <!-- Example:  -->
-  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
   <maintainer email="okalachev@gmail.com">Oleg Kalachev</maintainer>
+  <license>MIT</license>
 
+  <!--url type="website">http://wiki.ros.org/clever</url-->
+  <author email="okalachev@gmail.com">Oleg Kalachev</author>
+  <author email="urpylka@gmail.com">Artem Smirnov</author>
 
-  <!-- One license tag required, multiple allowed, one license per tag -->
-  <!-- Commonly used license strings: -->
-  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
-  <license>TODO</license>
-
-
-  <!-- Url tags are optional, but multiple are allowed, one per tag -->
-  <!-- Optional attribute type can be: website, bugtracker, or repository -->
-  <!-- Example: -->
-  <!-- <url type="website">http://wiki.ros.org/clever</url> -->
-
-
-  <!-- Author tags are optional, multiple are allowed, one per tag -->
-  <!-- Authors do not have to be maintainers, but could be -->
-  <!-- Example: -->
-  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
-
-
-  <!-- The *_depend tags are used to specify dependencies -->
-  <!-- Dependencies can be catkin packages or system dependencies -->
-  <!-- Examples: -->
   <!-- Use build_depend for packages you need at compile time: -->
-  <!--   <build_depend>message_generation</build_depend> -->
-  <!-- Use buildtool_depend for build tool packages: -->
-  <!--   <buildtool_depend>catkin</buildtool_depend> -->
-  <!-- Use run_depend for packages you need at runtime: -->
-  <!--   <run_depend>message_runtime</run_depend> -->
-  <!-- Use test_depend for packages you need only for testing: -->
-  <!--   <test_depend>gtest</test_depend> -->
-  <buildtool_depend>catkin</buildtool_depend>
-
   <build_depend>nodelet</build_depend>
   <build_depend>roscpp</build_depend>
-
+  <build_depend>visualization_msgs</build_depend>
-  <run_depend>nodelet</run_depend>
+  <build_depend>tf2_geometry_msgs</build_depend>
+  <!-- Use buildtool_depend for build tool packages: -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <!-- Use run_depend for packages you need at runtime: -->
+  <run_depend>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> -->
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>
-      <nodelet plugin="${prefix}/nodelet_plugins.xml" />
+    <nodelet plugin="${prefix}/nodelet_plugins.xml" />
     <!-- Other tools can request additional information be placed here -->
-
   </export>
 </package>

clever/requirements.txt

@@ -1,2 +1,5 @@
-flask==0.12.2
+flask==0.12.3
 geopy==1.11.0
+pymavlink==2.2.10
+smbus2==0.2.1
+VL53L1X==0.0.2

clever/src/camera_markers.cpp

@@ -0,0 +1,101 @@
+/*
+ * Visualization marker for camera alignment
+ * 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.
+ */
+
+#include <string>
+#include <ros/ros.h>
+#include <sensor_msgs/CameraInfo.h>
+#include <visualization_msgs/Marker.h>
+#include <visualization_msgs/MarkerArray.h>
+
+using namespace visualization_msgs;
+
+double markers_scale;
+std::string camera_frame;
+
+MarkerArray createMarkers() {
+	MarkerArray markers;
+
+	Marker lens;
+	lens.header.frame_id = camera_frame;
+	lens.ns = "camera_markers";
+	lens.id = 0;
+	lens.action = Marker::ADD;
+	lens.type = visualization_msgs::Marker::CYLINDER;
+	lens.frame_locked = true;
+	lens.scale.x = 0.013 * markers_scale;
+	lens.scale.y = 0.013 * markers_scale;
+	lens.scale.z = 0.015 * markers_scale;
+	lens.color.r = 0.3;
+	lens.color.g = 0.3;
+	lens.color.b = 0.3;
+	lens.color.a = 0.9;
+	lens.pose.position.z = 0.0075 * markers_scale;
+	lens.pose.orientation.w = 1;
+
+	Marker board;
+	board.header.frame_id = camera_frame;
+	board.ns = "camera_markers";
+	board.id = 1;
+	board.action = Marker::ADD;
+	board.type = Marker::CUBE;
+	board.frame_locked = true;
+	board.scale.x = 0.024 * markers_scale;
+	board.scale.y = 0.024 * markers_scale;
+	board.scale.z = 0.001 * markers_scale;
+	board.color.r = 0.0;
+	board.color.g = 0.8;
+	board.color.b = 0.0;
+	board.color.a = 0.9;
+	board.pose.orientation.w = 1;
+
+	Marker wire;
+	wire.header.frame_id = camera_frame;
+	wire.ns = "camera_markers";
+	wire.id = 2;
+	wire.action = Marker::ADD;
+	wire.type = Marker::CUBE;
+	wire.frame_locked = true;
+	wire.scale.x = 0.014 * markers_scale;
+	wire.scale.y = 0.04 * markers_scale;
+	wire.scale.z = 0.001 * markers_scale;
+	wire.color.r = 0.9;
+	wire.color.g = 0.9;
+	wire.color.b = 1.0;
+	wire.color.a = 0.8;
+	wire.pose.position.x = 0;
+	wire.pose.position.y = (0.01 + 0.02) * markers_scale;
+	wire.pose.position.z = 0.002 * markers_scale;
+	wire.pose.orientation.w = 1;
+
+	markers.markers.push_back(lens);
+	markers.markers.push_back(board);
+	markers.markers.push_back(wire);
+
+	return markers;
+}
+
+int main(int argc, char **argv)
+{
+	ros::init(argc, argv, "camera_markers", ros::init_options::AnonymousName);
+	ros::NodeHandle nh, nh_priv("~");
+
+	nh_priv.param("scale", markers_scale, 1.0);
+
+	// wait for camera info
+	auto camera_info = ros::topic::waitForMessage<sensor_msgs::CameraInfo>("camera_info", nh);
+	camera_frame = camera_info->header.frame_id;
+
+	ros::Publisher markers_pub = nh.advertise<visualization_msgs::MarkerArray>("camera_markers", 1, true);
+	markers_pub.publish(createMarkers());
+
+	ROS_INFO("Camera markers initialized");
+	ros::spin();
+}

clever/src/fpv_camera

@@ -4,5 +4,4 @@
 # fpv_camera <video_device> <http port>
 
 echo "Starting FPV camera $1 on :$2"
-cd /home/pi/mjpg-streamer/mjpg-streamer-experimental
+mjpg_streamer -i "/usr/lib/input_uvc.so -d $1 -r 320x240 -f 30" -o "/usr/lib/output_http.so -w /usr/share/mjpg_streamer/www -p $2"
-./mjpg_streamer -i "./input_uvc.so -d $1 -r 320x240 -f 30" -o "./output_http.so -w ./www -p $2"

clever/src/frames.cpp

@@ -0,0 +1,63 @@
+/*
+ * Auxiliary TF frames for CLEVER drone kit:
+ * - Body frame (drone body with zero pitch and roll).
+ * - TODO: REP-0105 `odom` frame emulation: continuous frame without discrete jumps.
+ * - TODO: Terrain frame (base on ALTITUDE message).
+ * - TODO: map_upside_down frame
+ * - TODO: home frame?
+ *
+ * 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.
+ */
+
+// TODO: consider implementing as a mavros plugin
+
+#include <string>
+#include <memory>
+#include <ros/ros.h>
+#include <tf/transform_datatypes.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include <geometry_msgs/TransformStamped.h>
+#include <geometry_msgs/PoseStamped.h>
+
+using std::string;
+
+static std::shared_ptr<tf2_ros::TransformBroadcaster> br;
+static geometry_msgs::TransformStamped body;
+
+inline void publishBody(const geometry_msgs::PoseStamped& pose)
+{
+	// Get only yaw from pose
+	tf::Quaternion q;
+	q.setRPY(0, 0, tf::getYaw(pose.pose.orientation));
+	tf::quaternionTFToMsg(q, body.transform.rotation);
+
+	body.transform.translation.x = pose.pose.position.x;
+	body.transform.translation.y = pose.pose.position.y;
+	body.transform.translation.z = pose.pose.position.z;
+	body.header.frame_id = pose.header.frame_id;
+	body.header.stamp = pose.header.stamp;
+	br->sendTransform(body);
+}
+
+void poseCallback(const geometry_msgs::PoseStamped& pose)
+{
+	publishBody(pose);
+}
+
+int main(int argc, char **argv) {
+	ros::init(argc, argv, "frames");
+	ros::NodeHandle nh, nh_priv("~");
+
+	nh_priv.param<string>("body/frame_id", body.child_frame_id, "body");
+
+	br = std::make_shared<tf2_ros::TransformBroadcaster>();
+	ros::Subscriber pose_sub = nh.subscribe("mavros/local_position/pose", 1, &poseCallback);
+	ROS_INFO("frames: ready");
+	ros::spin();
+}

clever/src/interactive.py

@@ -0,0 +1,85 @@
+#!/usr/bin/env python
+import copy
+
+import rospy
+import tf.transformations as t
+from interactive_markers.interactive_marker_server import InteractiveMarkerServer
+from visualization_msgs.msg import Marker, InteractiveMarker, InteractiveMarkerControl, InteractiveMarkerFeedback
+from clever import srv
+
+
+def make_box(msg):
+    marker = Marker()
+
+    marker.type = Marker.CUBE
+    marker.scale.x = msg.scale * 0.3
+    marker.scale.y = msg.scale * 0.3
+    marker.scale.z = msg.scale * 0.3
+    marker.color.r = 0.5
+    marker.color.g = 0.5
+    marker.color.b = 0.5
+    marker.color.a = 1.0
+    marker.pose.orientation.w = 1
+
+    return marker
+
+
+def make_box_control(msg):
+    control = InteractiveMarkerControl()
+    control.always_visible = True
+    control.orientation.w = 1
+    control.markers.append(make_box(msg))
+    msg.controls.append(control)
+    return control
+
+
+def make_quadcopter_marker():
+    marker = InteractiveMarker()
+    marker.header.frame_id = 'fcu'
+    marker.header.stamp = rospy.get_rostime()
+    marker.scale = 1
+    marker.pose.orientation.w = 1
+
+    marker.name = 'quadcopter'
+    marker.description = 'Quadcopter'
+
+    make_box_control(marker)
+
+    control = InteractiveMarkerControl()
+    control.orientation.w = 1
+    control.orientation.x = 0
+    control.orientation.y = 1
+    control.orientation.z = 0
+    control.interaction_mode = InteractiveMarkerControl.MOVE_ROTATE
+    marker.controls.append(copy.deepcopy(control))
+    control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
+    marker.controls.append(control)
+
+    return marker
+
+
+navigate = rospy.ServiceProxy('navigate', srv.Navigate)
+
+
+def process_feedback(feedback):
+    if feedback.event_type != InteractiveMarkerFeedback.MOUSE_UP:
+        return
+
+    p = feedback.pose.position
+    o = feedback.pose.orientation
+    yaw = t.euler_from_quaternion((o.x, o.y, o.z, o.w), axes='rzyx')[0]
+    rospy.loginfo('Navigate to %s', p)
+    rospy.loginfo(navigate(x=p.x, y=p.y, z=p.z, yaw=yaw, speed=2,
+                           frame_id=feedback.header.frame_id, auto_arm=True))
+
+
+rospy.init_node('quadcopter_im')
+
+server = InteractiveMarkerServer('quadcopter_im')
+
+int_marker = make_quadcopter_marker()
+server.insert(int_marker, process_feedback)
+server.applyChanges()
+
+rospy.loginfo('Interactive quadcopter marker initialized')
+rospy.spin()

clever/src/optical_flow.cpp

@@ -0,0 +1,200 @@
+/*
+ * Optical Flow node for PX4
+ * 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.
+ */
+
+#include <vector>
+#include <cmath>
+#include <nodelet/nodelet.h>
+#include <pluginlib/class_list_macros.h>
+#include <image_transport/image_transport.h>
+#include <cv_bridge/cv_bridge.h>
+#include <opencv2/opencv.hpp>
+#include <tf/transform_datatypes.h>
+#include <tf2/exceptions.h>
+#include <tf2/convert.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <mavros_msgs/OpticalFlowRad.h>
+#include <sensor_msgs/Imu.h>
+#include <geometry_msgs/Vector3Stamped.h>
+#include <geometry_msgs/PointStamped.h>
+#include <geometry_msgs/TwistStamped.h>
+
+using cv::Mat;
+
+class OpticalFlow : public nodelet::Nodelet
+{
+public:
+	OpticalFlow():
+		camera_matrix_(3, 3, CV_64F),
+		dist_coeffs_(8, 1, CV_64F),
+		tf_listener_(tf_buffer_)
+	{}
+
+private:
+	ros::Publisher flow_pub_, velo_pub_, shift_pub_;
+	ros::Time prev_stamp_;
+	std::string fcu_frame_id_;
+	image_transport::CameraSubscriber img_sub_;
+	image_transport::Publisher img_pub_;
+	mavros_msgs::OpticalFlowRad flow_;
+	int roi_, roi_2_;
+	Mat hann_;
+	Mat prev_, curr_;
+	Mat camera_matrix_, dist_coeffs_;
+	tf2_ros::Buffer tf_buffer_;
+	tf2_ros::TransformListener tf_listener_;
+
+	void onInit()
+	{
+		ros::NodeHandle& nh = getNodeHandle();
+		ros::NodeHandle& nh_priv = getPrivateNodeHandle();
+		image_transport::ImageTransport it(nh);
+		image_transport::ImageTransport it_priv(nh_priv);
+
+		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;
+
+		img_sub_ = it.subscribeCamera("image", 1, &OpticalFlow::flow, this);
+		img_pub_ = it_priv.advertise("debug", 1);
+		flow_pub_ = nh.advertise<mavros_msgs::OpticalFlowRad>("mavros/px4flow/raw/send", 1);
+		velo_pub_ = nh_priv.advertise<geometry_msgs::TwistStamped>("angular_velocity", 1);
+		shift_pub_ = nh_priv.advertise<geometry_msgs::Vector3Stamped>("shift", 1);
+
+		flow_.integrated_xgyro = NAN; // no IMU available
+		flow_.integrated_ygyro = NAN;
+		flow_.integrated_zgyro = NAN;
+		flow_.time_delta_distance_us = 0;
+		flow_.distance = -1; // no distance sensor available
+		flow_.temperature = 0;
+
+		ROS_INFO("Optical Flow initialized");
+	}
+
+	void parseCameraInfo(const sensor_msgs::CameraInfoConstPtr &cinfo) {
+		for (int i = 0; i < 3; ++i) {
+			for (int j = 0; j < 3; ++j) {
+				camera_matrix_.at<double>(i, j) = cinfo->K[3 * i + j];
+			}
+		}
+		for (int k = 0; k < cinfo->D.size(); k++) {
+			dist_coeffs_.at<double>(k) = cinfo->D[k];
+		}
+	}
+
+	void drawFlow(Mat& frame, double x, double y, double quality) const
+	{
+		double brightness = (1 - quality) * 25;;
+		cv::Scalar color(brightness, brightness, brightness);
+		double radius = std::sqrt(x * x + y * y);
+
+		// draw a circle and line indicating the shift direction...
+		cv::Point center(frame.cols >> 1, frame.rows >> 1);
+		cv::circle(frame, center, (int)(radius*5), color, 3, cv::LINE_AA);
+		cv::line(frame, center, cv::Point(center.x + (int)(x*5), center.y + (int)(y*5)), color, 3, cv::LINE_AA);
+	}
+
+	void flow(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cinfo)
+	{
+		parseCameraInfo(cinfo);
+
+		auto img = cv_bridge::toCvShare(msg, "mono8")->image;
+
+		// Apply ROI
+		if (roi_ != 0) {
+			img = img(cv::Rect((msg->width / 2 - roi_2_), (msg->height / 2 - roi_2_), roi_, roi_));
+		}
+
+		img.convertTo(curr_, CV_64F);
+
+		if (prev_.empty()) {
+			prev_ = curr_.clone();
+			prev_stamp_ = msg->header.stamp;
+			cv::createHanningWindow(hann_, curr_.size(), CV_64F);
+
+		} else {
+			double response;
+			cv::Point2d shift = cv::phaseCorrelate(prev_, curr_, hann_, &response);
+
+			// Publish raw shift in pixels
+			static geometry_msgs::Vector3Stamped shift_vec;
+			shift_vec.header.stamp = msg->header.stamp;
+			shift_vec.header.frame_id = msg->header.frame_id;
+			shift_vec.vector.x = shift.x;
+			shift_vec.vector.y = shift.y;
+			shift_pub_.publish(shift_vec);
+
+			// Undistort flow in pixels
+			uint32_t flow_center_x = msg->width / 2;
+			uint32_t flow_center_y = msg->height / 2;
+			shift.x += flow_center_x;
+			shift.y += flow_center_y;
+
+			std::vector<cv::Point2d> points_dist = { shift };
+			std::vector<cv::Point2d> points_undist(1);
+
+			cv::undistortPoints(points_dist, points_undist, camera_matrix_, dist_coeffs_, cv::noArray(), camera_matrix_);
+			points_undist[0].x -= flow_center_x;
+			points_undist[0].y -= flow_center_y;
+
+			// Calculate flow in radians
+			double focal_length_x = camera_matrix_.at<double>(0, 0);
+			double focal_length_y = camera_matrix_.at<double>(1, 1);
+			double flow_x = atan2(points_undist[0].x, focal_length_x);
+			double flow_y = atan2(points_undist[0].y, focal_length_y);
+
+			// // Convert to FCU frame
+			static geometry_msgs::Vector3Stamped flow_camera, flow_fcu;
+			flow_camera.header.frame_id = msg->header.frame_id;
+			flow_camera.header.stamp = msg->header.stamp;
+			flow_camera.vector.x = flow_y; // +y means counter-clockwise rotation around Y axis
+			flow_camera.vector.y = -flow_x; // +x means clockwise rotation around X axis
+			tf_buffer_.transform(flow_camera, flow_fcu, fcu_frame_id_);
+
+			// Calculate integration time
+			ros::Duration integration_time = msg->header.stamp - prev_stamp_;
+			uint32_t integration_time_us = integration_time.toSec() * 1.0e6;
+
+			// Publish flow in fcu frame
+			flow_.header.stamp = /*prev_stamp_*/ msg->header.stamp;
+			flow_.integration_time_us = integration_time_us;
+			flow_.integrated_x = flow_fcu.vector.x;
+			flow_.integrated_y = flow_fcu.vector.y;
+			flow_.quality = (uint8_t)(response * 255);
+			flow_pub_.publish(flow_);
+
+			// Publish debug image
+			if (img_pub_.getNumSubscribers() > 0) {
+				// publish debug image
+				drawFlow(img, shift_vec.vector.x, shift_vec.vector.y, response);
+				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::MONO8;
+				out_msg.image = img;
+				img_pub_.publish(out_msg.toImageMsg());
+			}
+
+			// Publish estimated angular velocity
+			static geometry_msgs::TwistStamped velo;
+			velo.header.stamp = msg->header.stamp;
+			velo.header.frame_id = fcu_frame_id_;
+			velo.twist.angular.x = flow_.integrated_x / integration_time.toSec();
+			velo.twist.angular.y = flow_.integrated_y / integration_time.toSec();
+			velo_pub_.publish(velo);
+
+			prev_ = curr_.clone();
+			prev_stamp_ = msg->header.stamp;
+		}
+	}
+};
+
+PLUGINLIB_EXPORT_CLASS(OpticalFlow, nodelet::Nodelet)

clever/src/rc.cpp

@@ -10,6 +10,7 @@
 #include "std_msgs/String.h"
 #include "mavros_msgs/State.h"
 #include "mavros_msgs/ManualControl.h"
+#include "mavros_msgs/Mavlink.h"
 
 struct ControlMessage
 {
@@ -27,6 +28,9 @@ public:
         std::thread t(&RC::socketThread, this);
         t.detach();
 
+        std::thread gcst(&RC::fakeGCSThread, this);
+        gcst.detach();
+
         initLatchedState();
     }
 
@@ -35,6 +39,7 @@ private:
     ros::Subscriber state_sub;
     ros::Publisher state_pub;
     ros::Timer state_timeout_timer;
+    ros::Time last_manual_control{0};
     mavros_msgs::StateConstPtr state_msg;
 
     void handleState(const mavros_msgs::StateConstPtr& state)
@@ -70,6 +75,37 @@ private:
         state_pub.publish(unknown_state);
     }
 
+    void fakeGCSThread()
+    {
+        // Awful workaround for fixing PX4 not sending STATUSTEXTs
+        // if there is no GCS hearbeats.
+        // TODO: use timer
+        // TODO: remove, when PX4 get this fixed.
+        ros::Publisher mavlink_pub = nh.advertise<mavros_msgs::Mavlink>("mavlink/to", 1);
+
+        // HEARTBEAT from GCS message
+        mavros_msgs::Mavlink hb;
+        hb.framing_status = mavros_msgs::Mavlink::FRAMING_OK;
+        hb.magic = mavros_msgs::Mavlink::MAVLINK_V20;
+        hb.len = 9;
+        hb.incompat_flags = 0;
+        hb.compat_flags = 0;
+        hb.seq = 0;
+        hb.sysid = 255;
+        hb.compid = 0;
+        hb.checksum = 26460;
+        hb.payload64.push_back(342282393542983680);
+        hb.payload64.push_back(3);
+
+        ros::Rate rate(1);
+        while (ros::ok()) {
+            if (ros::Time::now() - last_manual_control < ros::Duration(8)) {
+                mavlink_pub.publish(hb);
+            }
+            rate.sleep();
+        }
+    }
+
     int createSocket(int port)
     {
         int sockfd = socket(AF_INET, SOCK_DGRAM, 0);
@@ -123,6 +159,8 @@ private:
             manual_control_msg.z = msg->z;
             manual_control_msg.r = msg->r;
             manual_control_pub.publish(manual_control_msg);
+
+            last_manual_control = ros::Time::now();
         }
     }
 };

clever/src/selfcheck.py

@@ -0,0 +1,231 @@
+#!/usr/bin/env python
+
+import math
+from subprocess import Popen, PIPE
+import re
+import traceback
+import rospy
+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
+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: local_origin, fcu, fcu_horiz
+# TODO: rc service
+# TODO: perform commander check, ekf2 status on PX4
+# TODO: check if FCU params setter succeed
+# TODO: selfcheck ROS service (with blacklists for checks)
+
+
+rospy.init_node('selfcheck')
+
+
+failures = []
+
+
+def failure(text, *args):
+    failures.append(text % args)
+
+
+def check(name):
+    def inner(fn):
+        def wrapper(*args, **kwargs):
+            failures[:] = []
+            try:
+                fn(*args, **kwargs)
+                for f in failures:
+                    rospy.logwarn('%s: %s', name, f)
+            except Exception as e:
+                traceback.print_exc()
+                rospy.logwarn('%s: exception occured', name)
+                return
+            if not failures:
+                rospy.loginfo('%s: OK', name)
+        return wrapper
+    return inner
+
+
+@check('FCU')
+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)')
+    except rospy.ROSException:
+        failure('No MAVROS state (check wiring)')
+
+
+@check('Camera')
+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)
+        return
+    try:
+        info = rospy.wait_for_message(name + '/camera_info', CameraInfo, timeout=1)
+    except rospy.ROSException:
+        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)
+    if img.height != info.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_detect/markers', MarkerArray, timeout=0.5)
+    except rospy.ROSException:
+        failure('No aruco markers detection')
+
+
+@check('Visual position estimate')
+def check_vpe():
+    try:
+        rospy.wait_for_message('mavros/vision_pose/pose', PoseStamped, timeout=1)
+    except rospy.ROSException:
+        try:
+            rospy.wait_for_message('mavros/mocap/pose', PoseStamped, timeout=1)
+        except rospy.ROSException:
+            failure('No VPE or MoCap messages')
+
+
+@check('Simple offboard node')
+def check_simpleoffboard():
+    try:
+        rospy.wait_for_service('navigate', timeout=3)
+        rospy.wait_for_service('get_telemetry', timeout=3)
+        rospy.wait_for_service('land', timeout=3)
+    except rospy.ROSException:
+        failure('No simple_offboard services')
+
+
+@check('IMU')
+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)')
+
+
+@check('Local position')
+def check_local_position():
+    try:
+        rospy.wait_for_message('mavros/local_position/pose', PoseStamped, timeout=1)
+    except rospy.ROSException:
+        failure('No local position')
+
+
+@check('Velocity estimation')
+def check_velocity():
+    try:
+        velocity = rospy.wait_for_message('mavros/local_position/velocity', TwistStamped, timeout=1)
+        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)
+        if abs(vert) > 0.1:
+            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?',
+                    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?',
+                    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?',
+                    angular.z, math.degrees(angular.z))
+    except rospy.ROSException:
+        failure('No velocity estimation')
+
+
+@check('Global position (GPS)')
+def check_global_position():
+    try:
+        rospy.wait_for_message('mavros/global_position/global', NavSatFix, timeout=1)
+    except rospy.ROSException:
+        failure('No global position')
+
+
+@check('Optical flow')
+def check_optical_flow():
+    # TODO:check FPS!
+    try:
+        rospy.wait_for_message('mavros/px4flow/raw/send', OpticalFlowRad, timeout=0.5)
+    except rospy.ROSException:
+        failure('No optical flow data (from Raspberry)')
+
+
+@check('Rangefinder')
+def check_rangefinder():
+    # TODO: check FPS!
+    try:
+        rospy.wait_for_message('mavros/distance_sensor/rangefinder_3_sub', Range, timeout=0.5)
+    except rospy.ROSException:
+        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')
+
+
+@check('Boot duration')
+def check_boot_duration():
+    proc = Popen('systemd-analyze', stdout=PIPE)
+    proc.wait()
+    output = proc.communicate()[0]
+    r = re.compile(r'([\d\.]+)s$')
+    duration = float(r.search(output).groups()[0])
+    if duration > 15:
+        failure('long Raspbian boot duration: %ss (systemd-analyze for analyzing)', duration)
+
+
+@check('CPU usage')
+def check_cpu_usage():
+    WHITELIST = 'nodelet',
+    CMD = "top -n 1 -b -i | tail -n +8 | awk '{ printf(\"%-8s\\t%-8s\\t%-8s\\n\", $1, $9, $12); }'"
+    proc = Popen(CMD, stdout=PIPE, shell=True)
+    proc.wait()
+    output = proc.communicate()[0]
+    processes = output.split('\n')
+    for process in processes:
+        if not process:
+            continue
+        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)',
+                    cpu.strip(), cmd.strip(), pid.strip())
+
+
+def selfcheck():
+    check_fcu()
+    check_imu()
+    check_local_position()
+    check_velocity()
+    check_global_position()
+    check_camera('main_camera')
+    check_aruco()
+    check_simpleoffboard()
+    check_optical_flow()
+    check_vpe()
+    check_rangefinder()
+    check_cpu_usage()
+    check_boot_duration()
+
+
+if __name__ == '__main__':
+    rospy.loginfo('Performing selfcheck...')
+    selfcheck()

clever/src/simple_offboard.py

@@ -83,12 +83,13 @@ 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', False))
+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('~local_frame', 'local_origin')
+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():
@@ -120,6 +121,8 @@ def offboard_and_arm():
 
 ps = PoseStamped()
 vs = Vector3Stamped()
+pt = PositionTarget()
+at = AttitudeTarget()
 
 
 BRAKE_TIME = rospy.Duration(0)
@@ -128,7 +131,10 @@ 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)
-    k = (stamp - start_stamp) / time
+    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:
@@ -158,14 +164,17 @@ 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)
+            ps.pose.orientation = orientation_from_euler(0, 0, req.yaw, axes='sxyz')
-            current_nav_finish = tf_buffer.transform(ps, LOCAL_FRAME, TRANSFORM_TIMEOUT)
+            current_nav_finish = tf_buffer.transform(ps, LOCAL_FRAME, transform_timeout)
 
             if isinstance(req, srv.NavigateGlobalRequest):
                 # Recalculate x and y from lat and lon
@@ -183,32 +192,34 @@ def get_publisher_and_message(req, stamp, continued=True, update_frame=True):
                                          current_nav_start_stamp, req.speed)
 
         yaw_rate_flag = math.isnan(req.yaw)
-        msg = PositionTarget(coordinate_frame=PT.FRAME_LOCAL_NED,
+        msg = pt
-                             type_mask=PT.IGNORE_VX + PT.IGNORE_VY + PT.IGNORE_VZ +
+        msg.coordinate_frame = PT.FRAME_LOCAL_NED
-                                       PT.IGNORE_AFX + PT.IGNORE_AFY + PT.IGNORE_AFZ +
+        msg.type_mask = PT.IGNORE_VX + PT.IGNORE_VY + PT.IGNORE_VZ + \
-                                       (PT.IGNORE_YAW if yaw_rate_flag else PT.IGNORE_YAW_RATE),
+                        PT.IGNORE_AFX + PT.IGNORE_AFY + PT.IGNORE_AFZ + \
-                             position=setpoint,
+                        (PT.IGNORE_YAW if yaw_rate_flag else PT.IGNORE_YAW_RATE)
-                             yaw=euler_from_orientation(current_nav_finish.pose.orientation)[2] - math.pi / 2,
+        msg.position = setpoint
-                             yaw_rate=req.yaw_rate)
+        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)
+        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 = PositionTarget(coordinate_frame=PT.FRAME_LOCAL_NED,
+        msg = pt
-                             type_mask=PT.IGNORE_VX + PT.IGNORE_VY + PT.IGNORE_VZ +
+        msg.coordinate_frame = PT.FRAME_LOCAL_NED
-                                       PT.IGNORE_AFX + PT.IGNORE_AFY + PT.IGNORE_AFZ +
+        msg.type_mask = PT.IGNORE_VX + PT.IGNORE_VY + PT.IGNORE_VZ + \
-                                       (PT.IGNORE_YAW if yaw_rate_flag else PT.IGNORE_YAW_RATE),
+                        PT.IGNORE_AFX + PT.IGNORE_AFY + PT.IGNORE_AFZ + \
-                             position=pose_local.pose.position,
+                        (PT.IGNORE_YAW if yaw_rate_flag else PT.IGNORE_YAW_RATE)
-                             yaw=euler_from_orientation(pose_local.pose.orientation)[2] - math.pi / 2,
+        msg.position = pose_local.pose.position
-                             yaw_rate=req.yaw_rate)
+        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):
@@ -216,32 +227,37 @@ def get_publisher_and_message(req, stamp, continued=True, update_frame=True):
         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)
+        pose_local = tf_buffer.transform(ps, LOCAL_FRAME, transform_timeout)
-        vector_local = tf_buffer.transform(vs, LOCAL_FRAME, TRANSFORM_TIMEOUT)
+        vector_local = tf_buffer.transform(vs, LOCAL_FRAME, transform_timeout)
 
         yaw_rate_flag = math.isnan(req.yaw)
-        msg = PositionTarget(coordinate_frame=PT.FRAME_LOCAL_NED,
+        msg = pt
-                             type_mask=PT.IGNORE_PX + PT.IGNORE_PY + PT.IGNORE_PZ +
+        msg.coordinate_frame = PT.FRAME_LOCAL_NED
-                                       PT.IGNORE_AFX + PT.IGNORE_AFY + PT.IGNORE_AFZ +
+        msg.type_mask = PT.IGNORE_PX + PT.IGNORE_PY + PT.IGNORE_PZ + \
-                                       (PT.IGNORE_YAW if yaw_rate_flag else PT.IGNORE_YAW_RATE),
+                                       PT.IGNORE_AFX + PT.IGNORE_AFY + PT.IGNORE_AFZ + \
-                             velocity=vector_local.vector,
+                                       (PT.IGNORE_YAW if yaw_rate_flag else PT.IGNORE_YAW_RATE)
-                             yaw=euler_from_orientation(pose_local.pose.orientation)[2] - math.pi / 2,
+        msg.velocity = vector_local.vector
-                             yaw_rate=req.yaw_rate)
+        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)
+        pose_local = tf_buffer.transform(ps, LOCAL_FRAME, transform_timeout)
-        msg = AttitudeTarget(orientation=pose_local.pose.orientation,
+        msg = at
-                             thrust=req.thrust,
+        msg.orientation = pose_local.pose.orientation
-                             type_mask=AT.IGNORE_YAW_RATE + AT.IGNORE_PITCH_RATE + AT.IGNORE_ROLL_RATE)
+        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 = AttitudeTarget(thrust=req.thrust,
+        msg = at
-                             type_mask=AttitudeTarget.IGNORE_ATTITUDE,
+        msg.thrust = req.thrust
-                             body_rate=Vector3(req.roll_rate, req.pitch_rate, req.yaw_rate))
+        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
 
 
@@ -261,9 +277,12 @@ def handle(req):
         rospy.logwarn('No connection to the FCU')
         return {'message': 'No connection to the FCU'}
 
-    if isinstance(req, (srv.NavigateRequest, srv.NavigateGlobalRequest)) and req.speed <= 0:
+    if isinstance(req, (srv.NavigateRequest, srv.NavigateGlobalRequest)):
-        rospy.logwarn('Navigate speed must be greater than zero, %s passed')
+        if req.speed < 0:
-        return {'message': 'Navigate speed must be greater than zero, %s passed' % req.speed}
+            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):
@@ -279,14 +298,19 @@ def handle(req):
         return {'message': 'Both yaw and yaw_rate cannot be NaN'}
 
     try:
-        with handle_lock:
+        # check frame_id existance
-                stamp = rospy.get_rostime()
+        # (for non-blocking setpoint's publishing in get_publisher_and_message)
-                current_req = req
+        stamp = rospy.get_rostime()
-                current_pub, current_msg = get_publisher_and_message(req, stamp, False)
+        if hasattr(req, 'frame_id'):
-                rospy.loginfo('Topic: %s, message: %s', current_pub.name, current_msg)
+            tf_buffer.lookup_transform(req.frame_id or LOCAL_FRAME, LOCAL_FRAME, stamp, TRANSFORM_TIMEOUT)
 
-                current_msg.header.stamp = stamp
+        with handle_lock:
-                current_pub.publish(current_msg)
+            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()
@@ -348,6 +372,7 @@ def get_telemetry(req):
         'z': float('nan'),
         'lat': float('nan'),
         'lon': float('nan'),
+        'alt': float('nan'),
         'vx': float('nan'),
         'vy': float('nan'),
         'vz': float('nan'),
@@ -363,31 +388,40 @@ def get_telemetry(req):
     frame_id = req.frame_id or LOCAL_FRAME
     stamp = rospy.get_rostime()
 
-    if pose:
+    transform_timeout = rospy.Duration(0.4)
-        p = tf_buffer.transform(pose, frame_id, TRANSFORM_TIMEOUT)
+    try:
-        res['x'] = p.pose.position.x
+        if pose:
-        res['y'] = p.pose.position.y
+            p = tf_buffer.transform(pose, frame_id, transform_timeout)
-        res['z'] = p.pose.position.z
+            res['x'] = p.pose.position.x
-        # Get yaw in the request's frame_in
+            res['y'] = p.pose.position.y
-        _, _, res['yaw'] = euler_from_orientation(p.pose.orientation)
+            res['z'] = p.pose.position.z
-        # Calculate pitch and roll as angles between the pose and fcu_horiz
+
-        attitude_pose = tf_buffer.transform(pose, 'fcu_horiz', TRANSFORM_TIMEOUT)
+            # Calculate roll pitch and yaw as Tait-Bryan angles, order z-y-x
-        res['roll'], res['pitch'], _ = euler_from_orientation(attitude_pose.pose.orientation)
+            res['yaw'], res['pitch'], res['roll'] = euler_from_orientation(p.pose.orientation, axes='rzyx')
+    except:
+        pass
 
     if velocity:
-        v = Vector3Stamped()
+        try:
-        v.header.stamp = velocity.header.stamp
+            v = Vector3Stamped()
-        v.header.frame_id = velocity.header.frame_id
+            v.header.stamp = velocity.header.stamp
-        v.vector = velocity.twist.linear
+            v.header.frame_id = velocity.header.frame_id
-        linear = tf_buffer.transform(v, frame_id, TRANSFORM_TIMEOUT)
+            v.vector = velocity.twist.linear
-        res['vx'] = linear.vector.x
+            linear = tf_buffer.transform(v, frame_id, transform_timeout)
-        res['vy'] = linear.vector.y
+            res['vx'] = linear.vector.x
-        res['vz'] = linear.vector.z
+            res['vy'] = linear.vector.y
-        # TODO pitch_rate, roll_rate, yaw_rate
+            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
@@ -425,21 +459,21 @@ def start_loop():
                         current_pub, current_msg = get_publisher_and_message(current_req, stamp, True,
                                                                              getattr(current_req, 'update_frame', False))
 
-                    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 + math.pi / 2)
-                        target_pub.publish(p)
-
                 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()
 
 

clever/src/util.py

@@ -6,8 +6,8 @@ def orientation_from_quaternion(q):
     return Quaternion(*q)
 
 
-def orientation_from_euler(roll, pitch, yaw):
+def orientation_from_euler(roll, pitch, yaw, axes='rzyx'):
-    q = t.quaternion_from_euler(roll, pitch, yaw)
+    q = t.quaternion_from_euler(roll, pitch, yaw, axes)
     return orientation_from_quaternion(q)
 
 
@@ -15,9 +15,9 @@ def quaternion_from_orientation(o):
     return o.x, o.y, o.z, o.w
 
 
-def euler_from_orientation(o):
+def euler_from_orientation(o, axes='rzyx'):
     q = quaternion_from_orientation(o)
-    return t.euler_from_quaternion(q)
+    return t.euler_from_quaternion(q, axes)
 
 
 def vector3_from_point(p):

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;
+}

clever/src/vl53l1x.py

@@ -0,0 +1,38 @@
+#!/usr/bin/env python
+# TODO: rewrite, as Python version eats 20% CPU
+
+from __future__ import division
+
+import rospy
+import VL53L1X
+from sensor_msgs.msg import Range
+
+rospy.init_node('vl53l1x')
+
+
+# range_pub = rospy.Publisher('~range', Range, queue_size=5)
+# TODO: why remmaping is not working?
+range_pub = rospy.Publisher('mavros/distance_sensor/rangefinder_3_sub', Range, queue_size=10)
+z_shift = rospy.get_param("z_shift", 0)  # TODO: move to mavros (use frame)
+
+msg = Range()
+msg.radiation_type = Range.INFRARED
+msg.field_of_view = 0.471239
+msg.min_range = 0
+msg.max_range = 4
+msg.header.frame_id = 'rangefinder'
+
+tof = VL53L1X.VL53L1X(i2c_bus=1, i2c_address=0x29)
+tof.open() # Initialise the i2c bus and configure the sensor
+tof.start_ranging(3) # Start ranging, 1 = Short Range, 2 = Medium Range, 3 = Long Range
+
+rospy.loginfo('vl53l1x: start ranging')
+
+r = rospy.Rate(14)
+while not rospy.is_shutdown():
+    msg.header.stamp = rospy.get_rostime()
+    msg.range = tof.get_distance() / 1000 + z_shift
+    range_pub.publish(msg)
+    r.sleep()
+
+tof.stop_ranging() # Stop ranging

clever/src/vpe_publisher.cpp

@@ -0,0 +1,99 @@
+/*
+ * Universal VPE publisher node
+ * 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.
+ */
+
+#include <string>
+#include <ros/ros.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include <geometry_msgs/TransformStamped.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
+#include <aruco_pose/MarkerArray.h>
+
+#include "utils.h"
+
+using std::string;
+
+static string frame_id, child_frame_id;
+static tf2_ros::Buffer tf_buffer;
+static ros::Publisher vpe_pub;
+static ros::Subscriber local_position_sub;
+static ros::Timer zero_timer;
+static geometry_msgs::PoseStamped vpe, pose;
+static ros::Time local_pose_stamp(0);
+static ros::Duration publish_zero_timout;
+tf2_ros::TransformBroadcaster br;
+
+void publishZero(const ros::TimerEvent&)
+{
+	if ((ros::Time::now() - pose.header.stamp < publish_zero_timout) ||
+	    (ros::Time::now() - vpe.header.stamp < publish_zero_timout))
+		return;
+
+	ROS_INFO_THROTTLE(10, "vpe_publisher: publish zero");
+	vpe.header.stamp = ros::Time::now();
+	vpe.pose.orientation.w = 1;
+	vpe_pub.publish(vpe);
+}
+
+void localPositionCallback(const geometry_msgs::PoseStamped& msg)
+{
+	pose = msg;
+}
+
+template <typename T>
+void callback(const T& msg)
+{
+	try {
+		auto transform = tf_buffer.lookupTransform(frame_id, child_frame_id,
+		                                            msg->header.stamp, ros::Duration(0.02));
+		                                            vpe.header.stamp = msg->header.stamp;
+		vpe.pose.position.x = transform.transform.translation.x;
+		vpe.pose.position.y = transform.transform.translation.y;
+		vpe.pose.position.z = transform.transform.translation.z;
+		vpe.pose.orientation = transform.transform.rotation;
+		vpe.header.stamp = msg->header.stamp;
+		vpe_pub.publish(vpe);
+	} catch (const tf2::TransformException& e) {
+		ROS_WARN_THROTTLE(10, "vpe_publisher: %s", e.what());
+	}
+}
+
+int main(int argc, char **argv) {
+	ros::init(argc, argv, "vpe_publisher");
+	ros::NodeHandle nh, nh_priv("~");
+
+	tf2_ros::TransformListener tf_listener(tf_buffer);
+
+	param(nh_priv, "frame_id", frame_id);
+	param(nh_priv, "child_frame_id", child_frame_id);
+	nh_priv.param<std::string>("mavros/local_position/frame_id", vpe.header.frame_id, "map");
+
+	auto pose_sub = nh_priv.subscribe<geometry_msgs::PoseStamped>("pose", 1, &callback);
+	auto pose_cov_sub = nh_priv.subscribe<geometry_msgs::PoseWithCovarianceStamped>("pose_cov", 1, &callback);
+	auto markers_sub = nh_priv.subscribe<aruco_pose::MarkerArray>("markers", 1, &callback);
+	//auto pose_cov_sub = nh_priv.subscribe<geometry_msgs::PoseWithCovarianceStamped>("pose_cov", 1, &callback);
+
+	vpe_pub = nh_priv.advertise<geometry_msgs::PoseStamped>("pose_pub", 1);
+	//vpe_cov_pub = nh_priv_.advertise<geometry_msgs::PoseStamped>("pose_cov_pub", 1);
+
+	if (nh_priv.param("publish_zero", false)) {
+		// publish zero to initialize the local position
+		vpe.header.stamp = ros::Time(0);
+		zero_timer = nh.createTimer(ros::Duration(0.1), &publishZero);
+		publish_zero_timout = ros::Duration(nh_priv.param("publish_zero_timout", 5.0));
+		local_position_sub = nh.subscribe("mavros/local_position/pose", 1, &localPositionCallback);
+	}
+
+	ROS_INFO("vpe_publisher: ready");
+	ros::spin();
+}

clever/src/web_server.py

@@ -1,60 +0,0 @@
-#!/usr/bin/env python
-
-import rospy
-import subprocess
-import re
-from flask import Flask, send_from_directory, send_file, request, jsonify
-
-rospy.init_node('web_server', disable_signals=True)
-
-port = rospy.get_param('~port', 7070)
-host = rospy.get_param('~host', '0.0.0.0')
-serve_path = rospy.get_param('~path')
-app = Flask(__name__)
-
-
-@app.route('/')
-def serve_index():
-    return send_from_directory(serve_path, 'index.html')
-
-
-@app.route('/<path:path>')
-def serve_static(path):
-    print serve_path, path
-    return send_from_directory(serve_path, path)
-
-
-@app.route('/wifi_data/')
-def get_wifi_data():
-    cur_ip = request.remote_addr
-    ip_signal = get_ip_signal()
-    return jsonify({'ip': cur_ip, 'signal': ip_signal[cur_ip]}), 200
-
-
-def get_ip_signal():
-    wlan_interface = 'wlan0'
-    # Getting info about wifi client connected to access point. From here we know MAC and signal level
-    iwl = subprocess.check_output(['sudo', 'iw', 'dev', 'wlan0', 'station', 'dump']).splitlines()
-    mac_signal = {}
-    cur_client = ''
-    for line in iwl:
-        if line.find('Station') != -1:
-            cur_client = re.search(r'([0-9A-F]{2}[:-]){5}([0-9A-F]{2})', line, re.I).group()
-        if line.find('signal') != -1:
-            sg = re.search(r'(\[-?\d*\])', line, re.I).group()
-            mac_signal[cur_client] = re.sub(r'[\[\]]', '', sg)
-    ip_signal = {}
-    # Getting ip-mac mapping
-    ip_mac = subprocess.check_output(['arp', '-i', wlan_interface]).splitlines()
-    for line in ip_mac:
-        mac = re.search(r'([0-9A-F]{2}[:-]){5}([0-9A-F]{2})', line, re.I)
-        if mac is not None:
-            mac = mac.group()
-            if mac in mac_signal:
-                ips = re.search(r'((2[0-5]|1[0-9]|[0-9])?[0-9]\.){3}((2[0-5]|1[0-9]|[0-9])?[0-9])', line, re.I).group()
-                ip_signal[ips] = mac_signal[mac]
-    return ip_signal
-
-
-rospy.loginfo('Serving on %s:%s', host, port)
-app.run(host=host, port=port, threaded=True)

clever/srv/GetTelemetry.srv

@@ -7,8 +7,9 @@ string mode
 float32 x
 float32 y
 float32 z
-float32 lat
+float64 lat
-float32 lon
+float64 lon
+float32 alt
 float32 vx
 float32 vy
 float32 vz

deploy/generate_ros_lib

@@ -1,9 +0,0 @@
-#!/usr/bin/env bash
-
-# This script generates ros_lib library for Arduino for using with rosseial_arduino:
-# http://wiki.ros.org/rosserial_arduino/Tutorials
-# https://copterexpress.gitbooks.io/clever/content/docs/arduino.html
-
-rm -rf ros_lib
-rosrun rosserial_arduino make_libraries.py .
-tar czf clever_arudino.tar.gz ros_lib

docs/3g.md

@@ -1,4 +1,6 @@
 Использование внешнего 3G-модема
 ===
 
+Использование внешнего 3G-модема на Raspberry возможно с помощью пакета `sakis3g`.
+
 TODO