diff --git a/.gitattributes b/.gitattributes index 7a394671..b27c18f8 100644 --- a/.gitattributes +++ b/.gitattributes @@ -1,2 +1,5 @@ -apps/ios/cleverrc/roslib.js linguist-vendored apps/ios/cleverrc/BinUtils.swift linguist-vendored +roslib.js linguist-vendored +eventemitter2.js linguist-vendored +ros3d.js linguist-vendored +three.min.js linguist-vendored diff --git a/.travis.yml b/.travis.yml index 705df0f6..3b97c718 100644 --- a/.travis.yml +++ b/.travis.yml @@ -4,29 +4,60 @@ services: - docker env: global: - - DOCKER="goldarte/img-tool:builder-mod" + - DOCKER="sfalexrog/img-tool:builder-mod" - 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 "goldarte" - - git config --local user.email "goldartt@gmail.com" - - sudo chmod -R 777 * - - cd images && zip ${IMAGE_NAME}.zip ${IMAGE_NAME} -deploy: - provider: releases - api_key: ${GITHUB_OAUTH_TOKEN} - file: ${IMAGE_NAME}.zip - skip_cleanup: true - on: - tags: true +matrix: + fast_finish: true + include: + - name: "Raspberry Pi Image Build" + 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 "goldarte" + - git config --local user.email "goldartt@gmail.com" + - sudo chmod -R 777 * + - cd images && zip ${IMAGE_NAME}.zip ${IMAGE_NAME} + deploy: + provider: releases + api_key: ${GITHUB_OAUTH_TOKEN} + file: ${IMAGE_NAME}.zip + skip_cleanup: true + on: + tags: true + draft: true + - name: "Documentation" + language: node_js + node_js: + - "10" + before_script: + - npm install gitbook-cli -g + - npm install markdownlint-cli -g + - gitbook -V + - markdownlint -V + script: + - markdownlint docs --ignore **/lesson*.md --ignore **/metodmaterials.md + - gitbook install + - gitbook build +# *** +# Disable deployments for now, revisit this later +# --sfalexrog, 06.02.2019 +# *** +# deploy: +# provider: pages +# local-dir: _book +# skip-cleanup: true +# github-token: ${GITHUB_OAUTH_TOKEN} +# keep-history: true +# target-branch: gh-pages +# on: +# branch: WIP/gitbook-autobuild # More info there # https://github.com/travis-ci/travis-ci/issues/6893 diff --git a/README.md b/README.md index 9bf4ca28..061ebb03 100644 --- a/README.md +++ b/README.md @@ -2,7 +2,7 @@ CLEVER drone -CLEVER is an educational programmable drone kit consisting of an unassembled quadcopter, open source software and documentation. The kit includes Pixhawk/Pixracer autopilot running PX4 firmware, Raspberry Pi 3 as companion computer, a camera for computer vision navigation as well as additional sensors and peripheral devices. +CLEVER (Russian: *"Клевер"*, meaning *"Clover"*) is an educational programmable drone kit consisting of an unassembled quadcopter, open source software and documentation. The kit includes Pixhawk/Pixracer autopilot running PX4 firmware, Raspberry Pi 3 as companion computer, a camera for computer vision navigation as well as additional sensors and peripheral devices. Copter Express has implemented a large number of different autonomous drone projects using exactly the same platform: [automated pizza delivery](https://www.youtube.com/watch?v=hmkAoZOtF58) in Samara and Kazan, coffee delivery in Skolkovo Innovation Center, [autonomous quadcopter with charging station](https://www.youtube.com/watch?v=RjX6nUqw1mI) for site monitoring and security, winning drones on [Robocross-2016](https://www.youtube.com/watch?v=dGbDaz_VmYU) and [Robocross-2017](https://youtu.be/AQnd2CRczbQ) competitions and many others. diff --git a/apps/android/.gitignore b/apps/android/.gitignore new file mode 100644 index 00000000..fd45b12f --- /dev/null +++ b/apps/android/.gitignore @@ -0,0 +1,11 @@ +*.iml +.gradle +/local.properties +/.idea/caches/build_file_checksums.ser +/.idea/libraries +/.idea/modules.xml +/.idea/workspace.xml +.DS_Store +/build +/captures +.externalNativeBuild diff --git a/apps/android/.idea/assetWizardSettings.xml b/apps/android/.idea/assetWizardSettings.xml new file mode 100644 index 00000000..63b73e65 --- /dev/null +++ b/apps/android/.idea/assetWizardSettings.xml @@ -0,0 +1,57 @@ + + + + + + \ No newline at end of file diff --git a/apps/android/.idea/codeStyles/Project.xml b/apps/android/.idea/codeStyles/Project.xml new file mode 100644 index 00000000..34dc27cb --- /dev/null +++ b/apps/android/.idea/codeStyles/Project.xml @@ -0,0 +1,35 @@ + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/apps/android/.idea/codeStyles/codeStyleConfig.xml b/apps/android/.idea/codeStyles/codeStyleConfig.xml new file mode 100644 index 00000000..79ee123c --- /dev/null +++ b/apps/android/.idea/codeStyles/codeStyleConfig.xml @@ -0,0 +1,5 @@ + + + + \ No newline at end of file diff --git a/apps/android/.idea/gradle.xml b/apps/android/.idea/gradle.xml new file mode 100644 index 00000000..7ac24c77 --- /dev/null +++ b/apps/android/.idea/gradle.xml @@ -0,0 +1,18 @@ + + + + + + \ No newline at end of file diff --git a/apps/android/.idea/misc.xml b/apps/android/.idea/misc.xml new file mode 100644 index 00000000..e0d5b93f --- /dev/null +++ b/apps/android/.idea/misc.xml @@ -0,0 +1,38 @@ + + + + + + + + + + + + \ No newline at end of file diff --git a/apps/android/.idea/runConfigurations.xml b/apps/android/.idea/runConfigurations.xml new file mode 100644 index 00000000..7f68460d --- /dev/null +++ b/apps/android/.idea/runConfigurations.xml @@ -0,0 +1,12 @@ + + + + + + \ No newline at end of file diff --git a/apps/android/.idea/vcs.xml b/apps/android/.idea/vcs.xml new file mode 100644 index 00000000..94a25f7f --- /dev/null +++ b/apps/android/.idea/vcs.xml @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/apps/android/app/.gitignore b/apps/android/app/.gitignore new file mode 100644 index 00000000..796b96d1 --- /dev/null +++ b/apps/android/app/.gitignore @@ -0,0 +1 @@ +/build diff --git a/apps/android/app/build.gradle b/apps/android/app/build.gradle new file mode 100644 index 00000000..1b564e8b --- /dev/null +++ b/apps/android/app/build.gradle @@ -0,0 +1,33 @@ +apply plugin: 'com.android.application' + +apply plugin: 'kotlin-android' + +apply plugin: 'kotlin-android-extensions' + +android { + compileSdkVersion 28 + defaultConfig { + applicationId "express.copter.cleverrc" + minSdkVersion 19 + targetSdkVersion 28 + versionCode 1 + versionName "1.0" + testInstrumentationRunner "android.support.test.runner.AndroidJUnitRunner" + } + buildTypes { + release { + minifyEnabled false + proguardFiles getDefaultProguardFile('proguard-android.txt'), 'proguard-rules.pro' + } + } +} + +dependencies { + implementation fileTree(dir: 'libs', include: ['*.jar']) + implementation"org.jetbrains.kotlin:kotlin-stdlib-jdk7:$kotlin_version" + implementation 'com.android.support:appcompat-v7:28.0.0' + implementation 'com.android.support.constraint:constraint-layout:1.1.3' + testImplementation 'junit:junit:4.12' + androidTestImplementation 'com.android.support.test:runner:1.0.2' + androidTestImplementation 'com.android.support.test.espresso:espresso-core:3.0.2' +} diff --git a/apps/android/app/proguard-rules.pro b/apps/android/app/proguard-rules.pro new file mode 100644 index 00000000..f1b42451 --- /dev/null +++ b/apps/android/app/proguard-rules.pro @@ -0,0 +1,21 @@ +# Add project specific ProGuard rules here. +# You can control the set of applied configuration files using the +# proguardFiles setting in build.gradle. +# +# For more details, see +# http://developer.android.com/guide/developing/tools/proguard.html + +# If your project uses WebView with JS, uncomment the following +# and specify the fully qualified class name to the JavaScript interface +# class: +#-keepclassmembers class fqcn.of.javascript.interface.for.webview { +# public *; +#} + +# Uncomment this to preserve the line number information for +# debugging stack traces. +#-keepattributes SourceFile,LineNumberTable + +# If you keep the line number information, uncomment this to +# hide the original source file name. +#-renamesourcefileattribute SourceFile diff --git a/apps/android/app/release/release/app1.aab b/apps/android/app/release/release/app1.aab new file mode 100644 index 00000000..94c706dc Binary files /dev/null and b/apps/android/app/release/release/app1.aab differ diff --git a/apps/android/app/src/androidTest/java/express/copter/cleverrc/ExampleInstrumentedTest.kt b/apps/android/app/src/androidTest/java/express/copter/cleverrc/ExampleInstrumentedTest.kt new file mode 100644 index 00000000..0e24cc4b --- /dev/null +++ b/apps/android/app/src/androidTest/java/express/copter/cleverrc/ExampleInstrumentedTest.kt @@ -0,0 +1,24 @@ +package express.copter.cleverrc + +import android.support.test.InstrumentationRegistry +import android.support.test.runner.AndroidJUnit4 + +import org.junit.Test +import org.junit.runner.RunWith + +import org.junit.Assert.* + +/** + * Instrumented test, which will execute on an Android device. + * + * See [testing documentation](http://d.android.com/tools/testing). + */ +@RunWith(AndroidJUnit4::class) +class ExampleInstrumentedTest { + @Test + fun useAppContext() { + // Context of the app under test. + val appContext = InstrumentationRegistry.getTargetContext() + assertEquals("express.copter.cleverrc", appContext.packageName) + } +} diff --git a/apps/android/app/src/main/AndroidManifest.xml b/apps/android/app/src/main/AndroidManifest.xml new file mode 100644 index 00000000..385e0e4a --- /dev/null +++ b/apps/android/app/src/main/AndroidManifest.xml @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/apps/android/app/src/main/assets/clever.svg b/apps/android/app/src/main/assets/clever.svg new file mode 100644 index 00000000..a72392cd --- /dev/null +++ b/apps/android/app/src/main/assets/clever.svg @@ -0,0 +1,57 @@ + +image/svg+xml + diff --git a/apps/android/app/src/main/assets/index.html b/apps/android/app/src/main/assets/index.html new file mode 100644 index 00000000..b6cca6db --- /dev/null +++ b/apps/android/app/src/main/assets/index.html @@ -0,0 +1,24 @@ + + + + + + + + +
DISCONNECTED
+
+ +
+
+
+
+
+
+
+
+
+ + + + diff --git a/apps/android/app/src/main/assets/main.css b/apps/android/app/src/main/assets/main.css new file mode 100644 index 00000000..be91c227 --- /dev/null +++ b/apps/android/app/src/main/assets/main.css @@ -0,0 +1,125 @@ +html, body { + margin: 0; + padding: 0; + user-select: none; + font-family: sans-serif; + background: #212121; + color: rgba(255, 255, 255, 0.9); +} + +* { + user-select: none; +} + +.stick { + border-radius: 50%; + width: 5cm; + height: 5cm; + position: relative; + transform: translateZ(0); + border: 4px solid rgba(255,255,255,.4); + box-shadow: 0 0 0 1px rgba(0,0,0,.2), inset 0 0 0 1px rgba(0,0,0,.2); +} + +.stick-pointer { + position: absolute; + border-radius: 50%; + background-color: rgba(255,255,255,.25); + box-shadow: 0 0 10px rgba(0,0,0,.3); + width: 3cm; + height: 3cm; + margin-left: -1.5cm; + margin-top: -1.5cm; + top: 2.5cm; + left: 2.5cm; + pointer-events: none; + transform: translateZ(0); +} + +.container { + display: flex; + justify-content: space-around; + align-items: center; + width: 100%; + height: 100%; +} + +.telemetry { + position: absolute; + text-align: center; + width: 100%; + top: 30px; + font-size: 20px; + user-select: none; + pointer-events: none; +} + +body.armed .telemetry .mode { + font-weight: bold; +} + +@keyframes scale { + 0% { transform: scale(1.0); } + 50% { transform: scale(1.2); } + 100% { transform: scale(1.0); } +} + +.battery { + position: absolute; + text-align: center; + width: 100%; + bottom: 30px; + font-size: 20px; + user-select: none; + pointer-events: none; +} + +body.low-battery .battery { + color: #ff554b; + animation: scale 0.3s 1 ease-in-out +} + +.logo { + position: absolute; + background: url(clever.svg); + -webkit-background-size: 50px; + background-size: 50px; + width: 50px; + height: 50px; + top: 50%; + left: 50%; + margin-top: -25px; + margin-left: -25px; + font-size: 20px; + user-select: none; + pointer-events: none; +} + +.notifications { + pointer-events: none; + position: absolute; + top: 0; + left: 0; + right: 0; + color: white; +} + +.notifications.hidden { + transform: translateY(-100%); +} + +.notifications.anim { + transition: transform 0.2s ease; +} + +.notifications .item { + font-size: 4mm; + -webkit-text-size-adjust: none; + background: #fca83a; + padding: 3mm; + padding-bottom: 1.5mm; +} + +.notifications .item:last-child { + padding-bottom: 3mm; +} diff --git a/apps/android/app/src/main/assets/main.js b/apps/android/app/src/main/assets/main.js new file mode 100644 index 00000000..979c6172 --- /dev/null +++ b/apps/android/app/src/main/assets/main.js @@ -0,0 +1,139 @@ +function throttle(func, ms) { + var isThrottled = false, + savedArgs, + savedThis; + + function wrapper() { + if (isThrottled) { + savedArgs = arguments; + savedThis = this; + return; + } + func.apply(this, arguments); + isThrottled = true; + setTimeout(function() { + isThrottled = false; + if (savedArgs) { + wrapper.apply(savedThis, savedArgs); + savedArgs = savedThis = null; + } + }, ms); + } + return wrapper; +} + +function postAppMessage(msg) { + if (window.webkit != undefined) { + if (window.webkit.messageHandlers.appInterface != undefined) { + window.webkit.messageHandlers.appInterface.postMessage(JSON.stringify(msg)); + } + } + else if (window.appInterface != undefined) { + window.appInterface.postMessage(JSON.stringify(msg)); + } +} + +function callNativeApp(name, msg) { + try { + postAppMessage(msg); + return true; + } catch(err) { + console.warn('The native context does not exist yet'); + return false; + } +} +var rcLastPublish = null; + +function rcPublish() { + callNativeApp('control', controlMessage); + rcLastPublish = new Date(); +} + +rcPublishThrottled = throttle(rcPublish, 30); + +setInterval(function() { + if (rcLastPublish !== null && new Date() - rcLastPublish > 800) { + rcPublishThrottled(); + } +}, 50); + +var body = document.querySelector('body'); +var stickLeft = document.querySelector('.stick-left'); +var stickRight = document.querySelector('.stick-right'); + +var controlMessage = { x: 0, y: 0, z: 0, r: 0 }; + +function onStickTouchMove(touch) { + var target = touch.target; + var targetRect = target.getBoundingClientRect(); + var stickPointer = target.querySelector('.stick-pointer'); + + var offsetX = touch.clientX - targetRect.left; + var offsetY = touch.clientY - targetRect.top; + + var x = 2 * offsetX / targetRect.width; + var y = 2 * offsetY / targetRect.height; + + x = Math.max(0, x); + x = Math.min(2, x); + y = Math.max(0, y); + y = Math.min(2, y); + + stickPointer.style.left = (x * 50) + '%'; + stickPointer.style.top = (y * 50) + '%'; + + x -= 1; + y = 1 - y; + + if (target.matches('.stick-left')) { + controlMessage.z = Math.round((y + 1) * 500); + controlMessage.r = Math.round(x * 1000); + } else if (target.matches('.stick-right')) { + controlMessage.x = Math.round(y * 1000); + controlMessage.y = Math.round(x * 1000); + } +} + +body.addEventListener('touchmove', function (e) { + e.preventDefault(); +}); + +function stickTouchStart(e) { + setControlMode(); + callNativeApp('controlStart'); + onStickTouchMove(e.changedTouches[0]); + rcPublishThrottled(); + e.stopPropagation(); + e.preventDefault(); +} + +function stickTouchMove(e) { + for (touch of e.changedTouches) { + onStickTouchMove(touch); + } + //onStickTouchMove(e.changedTouches[0]); + rcPublishThrottled(); + e.stopPropagation(); + e.preventDefault(); +} + +function stickTouchEnd(e) { + var pointer = e.target.querySelector('.stick-pointer'); + if (e.target.matches('.stick-left')) { + controlMessage.r = 0; + pointer.style.left = '50%'; + } else if (e.target.matches('.stick-right')) { + controlMessage.x = 0; + controlMessage.y = 0; + pointer.style.left = '50%'; + pointer.style.top = '50%'; + } + rcPublishThrottled(); +} + +stickLeft.addEventListener('touchmove', stickTouchMove); +stickRight.addEventListener('touchmove', stickTouchMove); +stickLeft.addEventListener('touchstart', stickTouchStart); +stickRight.addEventListener('touchstart', stickTouchStart); +stickLeft.addEventListener('touchend', stickTouchEnd); +stickRight.addEventListener('touchend', stickTouchEnd); \ No newline at end of file diff --git a/apps/android/app/src/main/assets/old.js b/apps/android/app/src/main/assets/old.js new file mode 100644 index 00000000..6d940927 --- /dev/null +++ b/apps/android/app/src/main/assets/old.js @@ -0,0 +1,142 @@ +function throttle(func, ms) { + var isThrottled = false, + savedArgs, + savedThis; + + function wrapper() { + if (isThrottled) { + savedArgs = arguments; + savedThis = this; + return; + } + func.apply(this, arguments); + isThrottled = true; + setTimeout(function() { + isThrottled = false; + if (savedArgs) { + wrapper.apply(savedThis, savedArgs); + savedArgs = savedThis = null; + } + }, ms); + } + return wrapper; +} + +function postAppMessage(msg) { + if (window.webkit != undefined) { + if (window.webkit.messageHandlers.appInterface != undefined) { + window.webkit.messageHandlers.appInterface.postMessage(JSON.stringify(msg)); + } + } + else if (window.appInterface != undefined) { + window.appInterface.postMessage(JSON.stringify(msg)); + } +} + +function callNativeApp(name, msg) { + try { + postAppMessage(msg); + return true; + } catch(err) { + console.warn('The native context does not exist yet'); + return false; + } +} + + + +var rcLastPublish = null; + +function rcPublish() { + callNativeApp('control', controlMessage); + rcLastPublish = new Date(); +} + +rcPublishThrottled = throttle(rcPublish, 30); + +setInterval(function() { + if (rcLastPublish !== null && new Date() - rcLastPublish > 800) { + rcPublishThrottled(); + } +}, 50); + +var body = document.querySelector('body'); +var stickLeft = document.querySelector('.stick-left'); +var stickRight = document.querySelector('.stick-right'); + +var controlMessage = { x: 0, y: 0, z: 0, r: 0 }; + +function onStickTouchMove(touch) { + var target = touch.target; + var targetRect = target.getBoundingClientRect(); + var stickPointer = target.querySelector('.stick-pointer'); + + var offsetX = touch.clientX - targetRect.left; + var offsetY = touch.clientY - targetRect.top; + + var x = 2 * offsetX / targetRect.width; + var y = 2 * offsetY / targetRect.height; + + x = Math.max(0, x); + x = Math.min(2, x); + y = Math.max(0, y); + y = Math.min(2, y); + + stickPointer.style.left = (x * 50) + '%'; + stickPointer.style.top = (y * 50) + '%'; + + x -= 1; + y = 1 - y; + + if (target.matches('.stick-left')) { + controlMessage.z = Math.round((y + 1) * 500); + controlMessage.r = Math.round(x * 1000); + } else if (target.matches('.stick-right')) { + controlMessage.x = Math.round(y * 1000); + controlMessage.y = Math.round(x * 1000); + } +} + +body.addEventListener('touchmove', function (e) { + e.preventDefault(); +}); + +function stickTouchStart(e) { + setControlMode(); + callNativeApp('controlStart'); + onStickTouchMove(e.changedTouches[0]); + rcPublishThrottled(); + e.stopPropagation(); + e.preventDefault(); +} + +function stickTouchMove(e) { + for (touch of e.changedTouches) { + onStickTouchMove(touch); + } + //onStickTouchMove(e.changedTouches[0]); + rcPublishThrottled(); + e.stopPropagation(); + e.preventDefault(); +} + +function stickTouchEnd(e) { + var pointer = e.target.querySelector('.stick-pointer'); + if (e.target.matches('.stick-left')) { + controlMessage.r = 0; + pointer.style.left = '50%'; + } else if (e.target.matches('.stick-right')) { + controlMessage.x = 0; + controlMessage.y = 0; + pointer.style.left = '50%'; + pointer.style.top = '50%'; + } + rcPublishThrottled(); +} + +stickLeft.addEventListener('touchmove', stickTouchMove); +stickRight.addEventListener('touchmove', stickTouchMove); +stickLeft.addEventListener('touchstart', stickTouchStart); +stickRight.addEventListener('touchstart', stickTouchStart); +stickLeft.addEventListener('touchend', stickTouchEnd); +stickRight.addEventListener('touchend', stickTouchEnd); \ No newline at end of file diff --git a/apps/android/app/src/main/assets/roslib.js b/apps/android/app/src/main/assets/roslib.js new file mode 100644 index 00000000..7d44585b --- /dev/null +++ b/apps/android/app/src/main/assets/roslib.js @@ -0,0 +1,3693 @@ +(function e(t,n,r){function s(o,u){if(!n[o]){if(!t[o]){var a=typeof require=="function"&&require;if(!u&&a)return a(o,!0);if(i)return i(o,!0);var f=new Error("Cannot find module '"+o+"'");throw f.code="MODULE_NOT_FOUND",f}var l=n[o]={exports:{}};t[o][0].call(l.exports,function(e){var n=t[o][1][e];return s(n?n:e)},l,l.exports,e,t,n,r)}return n[o].exports}var i=typeof require=="function"&&require;for(var o=0;o 0 && + tree._listeners.length > this._events.maxListeners + ) { + tree._listeners.warned = true; + logPossibleMemoryLeak.call(this, tree._listeners.length, name); + } + } + return true; + } + name = type.shift(); + } + return true; + } + + // By default EventEmitters will print a warning if more than + // 10 listeners are added to it. This is a useful default which + // helps finding memory leaks. + // + // Obviously not all Emitters should be limited to 10. This function allows + // that to be increased. Set to zero for unlimited. + + EventEmitter.prototype.delimiter = '.'; + + EventEmitter.prototype.setMaxListeners = function(n) { + if (n !== undefined) { + this._events || init.call(this); + this._events.maxListeners = n; + if (!this._conf) this._conf = {}; + this._conf.maxListeners = n; + } + }; + + EventEmitter.prototype.event = ''; + + EventEmitter.prototype.once = function(event, fn) { + this.many(event, 1, fn); + return this; + }; + + EventEmitter.prototype.many = function(event, ttl, fn) { + var self = this; + + if (typeof fn !== 'function') { + throw new Error('many only accepts instances of Function'); + } + + function listener() { + if (--ttl === 0) { + self.off(event, listener); + } + fn.apply(this, arguments); + } + + listener._origin = fn; + + this.on(event, listener); + + return self; + }; + + EventEmitter.prototype.emit = function() { + + this._events || init.call(this); + + var type = arguments[0]; + + if (type === 'newListener' && !this.newListener) { + if (!this._events.newListener) { + return false; + } + } + + var al = arguments.length; + var args,l,i,j; + var handler; + + if (this._all && this._all.length) { + handler = this._all.slice(); + if (al > 3) { + args = new Array(al); + for (j = 0; j < al; j++) args[j] = arguments[j]; + } + + for (i = 0, l = handler.length; i < l; i++) { + this.event = type; + switch (al) { + case 1: + handler[i].call(this, type); + break; + case 2: + handler[i].call(this, type, arguments[1]); + break; + case 3: + handler[i].call(this, type, arguments[1], arguments[2]); + break; + default: + handler[i].apply(this, args); + } + } + } + + if (this.wildcard) { + handler = []; + var ns = typeof type === 'string' ? type.split(this.delimiter) : type.slice(); + searchListenerTree.call(this, handler, ns, this.listenerTree, 0); + } else { + handler = this._events[type]; + if (typeof handler === 'function') { + this.event = type; + switch (al) { + case 1: + handler.call(this); + break; + case 2: + handler.call(this, arguments[1]); + break; + case 3: + handler.call(this, arguments[1], arguments[2]); + break; + default: + args = new Array(al - 1); + for (j = 1; j < al; j++) args[j - 1] = arguments[j]; + handler.apply(this, args); + } + return true; + } else if (handler) { + // need to make copy of handlers because list can change in the middle + // of emit call + handler = handler.slice(); + } + } + + if (handler && handler.length) { + if (al > 3) { + args = new Array(al - 1); + for (j = 1; j < al; j++) args[j - 1] = arguments[j]; + } + for (i = 0, l = handler.length; i < l; i++) { + this.event = type; + switch (al) { + case 1: + handler[i].call(this); + break; + case 2: + handler[i].call(this, arguments[1]); + break; + case 3: + handler[i].call(this, arguments[1], arguments[2]); + break; + default: + handler[i].apply(this, args); + } + } + return true; + } else if (!this._all && type === 'error') { + if (arguments[1] instanceof Error) { + throw arguments[1]; // Unhandled 'error' event + } else { + throw new Error("Uncaught, unspecified 'error' event."); + } + return false; + } + + return !!this._all; + }; + + EventEmitter.prototype.emitAsync = function() { + + this._events || init.call(this); + + var type = arguments[0]; + + if (type === 'newListener' && !this.newListener) { + if (!this._events.newListener) { return Promise.resolve([false]); } + } + + var promises= []; + + var al = arguments.length; + var args,l,i,j; + var handler; + + if (this._all) { + if (al > 3) { + args = new Array(al); + for (j = 1; j < al; j++) args[j] = arguments[j]; + } + for (i = 0, l = this._all.length; i < l; i++) { + this.event = type; + switch (al) { + case 1: + promises.push(this._all[i].call(this, type)); + break; + case 2: + promises.push(this._all[i].call(this, type, arguments[1])); + break; + case 3: + promises.push(this._all[i].call(this, type, arguments[1], arguments[2])); + break; + default: + promises.push(this._all[i].apply(this, args)); + } + } + } + + if (this.wildcard) { + handler = []; + var ns = typeof type === 'string' ? type.split(this.delimiter) : type.slice(); + searchListenerTree.call(this, handler, ns, this.listenerTree, 0); + } else { + handler = this._events[type]; + } + + if (typeof handler === 'function') { + this.event = type; + switch (al) { + case 1: + promises.push(handler.call(this)); + break; + case 2: + promises.push(handler.call(this, arguments[1])); + break; + case 3: + promises.push(handler.call(this, arguments[1], arguments[2])); + break; + default: + args = new Array(al - 1); + for (j = 1; j < al; j++) args[j - 1] = arguments[j]; + promises.push(handler.apply(this, args)); + } + } else if (handler && handler.length) { + if (al > 3) { + args = new Array(al - 1); + for (j = 1; j < al; j++) args[j - 1] = arguments[j]; + } + for (i = 0, l = handler.length; i < l; i++) { + this.event = type; + switch (al) { + case 1: + promises.push(handler[i].call(this)); + break; + case 2: + promises.push(handler[i].call(this, arguments[1])); + break; + case 3: + promises.push(handler[i].call(this, arguments[1], arguments[2])); + break; + default: + promises.push(handler[i].apply(this, args)); + } + } + } else if (!this._all && type === 'error') { + if (arguments[1] instanceof Error) { + return Promise.reject(arguments[1]); // Unhandled 'error' event + } else { + return Promise.reject("Uncaught, unspecified 'error' event."); + } + } + + return Promise.all(promises); + }; + + EventEmitter.prototype.on = function(type, listener) { + if (typeof type === 'function') { + this.onAny(type); + return this; + } + + if (typeof listener !== 'function') { + throw new Error('on only accepts instances of Function'); + } + this._events || init.call(this); + + // To avoid recursion in the case that type == "newListeners"! Before + // adding it to the listeners, first emit "newListeners". + this.emit('newListener', type, listener); + + if (this.wildcard) { + growListenerTree.call(this, type, listener); + return this; + } + + if (!this._events[type]) { + // Optimize the case of one listener. Don't need the extra array object. + this._events[type] = listener; + } + else { + if (typeof this._events[type] === 'function') { + // Change to array. + this._events[type] = [this._events[type]]; + } + + // If we've already got an array, just append. + this._events[type].push(listener); + + // Check for listener leak + if ( + !this._events[type].warned && + this._events.maxListeners > 0 && + this._events[type].length > this._events.maxListeners + ) { + this._events[type].warned = true; + logPossibleMemoryLeak.call(this, this._events[type].length, type); + } + } + + return this; + }; + + EventEmitter.prototype.onAny = function(fn) { + if (typeof fn !== 'function') { + throw new Error('onAny only accepts instances of Function'); + } + + if (!this._all) { + this._all = []; + } + + // Add the function to the event listener collection. + this._all.push(fn); + return this; + }; + + EventEmitter.prototype.addListener = EventEmitter.prototype.on; + + EventEmitter.prototype.off = function(type, listener) { + if (typeof listener !== 'function') { + throw new Error('removeListener only takes instances of Function'); + } + + var handlers,leafs=[]; + + if(this.wildcard) { + var ns = typeof type === 'string' ? type.split(this.delimiter) : type.slice(); + leafs = searchListenerTree.call(this, null, ns, this.listenerTree, 0); + } + else { + // does not use listeners(), so no side effect of creating _events[type] + if (!this._events[type]) return this; + handlers = this._events[type]; + leafs.push({_listeners:handlers}); + } + + for (var iLeaf=0; iLeaf 0) { + recursivelyGarbageCollect(root[key]); + } + if (Object.keys(obj).length === 0) { + delete root[key]; + } + } + } + recursivelyGarbageCollect(this.listenerTree); + + return this; + }; + + EventEmitter.prototype.offAny = function(fn) { + var i = 0, l = 0, fns; + if (fn && this._all && this._all.length > 0) { + fns = this._all; + for(i = 0, l = fns.length; i < l; i++) { + if(fn === fns[i]) { + fns.splice(i, 1); + this.emit("removeListenerAny", fn); + return this; + } + } + } else { + fns = this._all; + for(i = 0, l = fns.length; i < l; i++) + this.emit("removeListenerAny", fns[i]); + this._all = []; + } + return this; + }; + + EventEmitter.prototype.removeListener = EventEmitter.prototype.off; + + EventEmitter.prototype.removeAllListeners = function(type) { + if (arguments.length === 0) { + !this._events || init.call(this); + return this; + } + + if (this.wildcard) { + var ns = typeof type === 'string' ? type.split(this.delimiter) : type.slice(); + var leafs = searchListenerTree.call(this, null, ns, this.listenerTree, 0); + + for (var iLeaf=0; iLeaf t2.secs) { + return false; + } else if(t1.secs < t2.secs) { + return true; + } else if(t1.nsecs < t2.nsecs) { + return true; + } else { + return false; + } + }; + + // TODO: this may be more complicated than necessary, since I'm + // not sure if the callbacks can ever wind up with a scenario + // where we've been preempted by a next goal, it hasn't finished + // processing, and then we get a cancel message + cancelListener.subscribe(function(cancelMessage) { + + // cancel ALL goals if both empty + if(cancelMessage.stamp.secs === 0 && cancelMessage.stamp.secs === 0 && cancelMessage.id === '') { + that.nextGoal = null; + if(that.currentGoal) { + that.emit('cancel'); + } + } else { // treat id and stamp independently + if(that.currentGoal && cancelMessage.id === that.currentGoal.goal_id.id) { + that.emit('cancel'); + } else if(that.nextGoal && cancelMessage.id === that.nextGoal.goal_id.id) { + that.nextGoal = null; + } + + if(that.nextGoal && isEarlier(that.nextGoal.goal_id.stamp, + cancelMessage.stamp)) { + that.nextGoal = null; + } + if(that.currentGoal && isEarlier(that.currentGoal.goal_id.stamp, + cancelMessage.stamp)) { + + that.emit('cancel'); + } + } + }); + + // publish status at pseudo-fixed rate; required for clients to know they've connected + var statusInterval = setInterval( function() { + var currentTime = new Date(); + var secs = Math.floor(currentTime.getTime()/1000); + var nsecs = Math.round(1000000000*(currentTime.getTime()/1000-secs)); + that.statusMessage.header.stamp.secs = secs; + that.statusMessage.header.stamp.nsecs = nsecs; + statusPublisher.publish(that.statusMessage); + }, 500); // publish every 500ms + +} + +SimpleActionServer.prototype.__proto__ = EventEmitter2.prototype; + +/** +* Set action state to succeeded and return to client +*/ + +SimpleActionServer.prototype.setSucceeded = function(result2) { + + + var resultMessage = new Message({ + status : {goal_id : this.currentGoal.goal_id, status : 3}, + result : result2 + }); + this.resultPublisher.publish(resultMessage); + + this.statusMessage.status_list = []; + if(this.nextGoal) { + this.currentGoal = this.nextGoal; + this.nextGoal = null; + this.emit('goal', this.currentGoal.goal); + } else { + this.currentGoal = null; + } +}; + +/** +* Function to send feedback +*/ + +SimpleActionServer.prototype.sendFeedback = function(feedback2) { + + var feedbackMessage = new Message({ + status : {goal_id : this.currentGoal.goal_id, status : 1}, + feedback : feedback2 + }); + this.feedbackPublisher.publish(feedbackMessage); +}; + +/** +* Handle case where client requests preemption +*/ + +SimpleActionServer.prototype.setPreempted = function() { + + this.statusMessage.status_list = []; + var resultMessage = new Message({ + status : {goal_id : this.currentGoal.goal_id, status : 2}, + }); + this.resultPublisher.publish(resultMessage); + + if(this.nextGoal) { + this.currentGoal = this.nextGoal; + this.nextGoal = null; + this.emit('goal', this.currentGoal.goal); + } else { + this.currentGoal = null; + } +}; + +module.exports = SimpleActionServer; +},{"../core/Message":10,"../core/Topic":17,"eventemitter2":1}],9:[function(require,module,exports){ +var Ros = require('../core/Ros'); +var mixin = require('../mixin'); + +var action = module.exports = { + ActionClient: require('./ActionClient'), + ActionListener: require('./ActionListener'), + Goal: require('./Goal'), + SimpleActionServer: require('./SimpleActionServer') +}; + +mixin(Ros, ['ActionClient', 'SimpleActionServer'], action); + +},{"../core/Ros":12,"../mixin":24,"./ActionClient":5,"./ActionListener":6,"./Goal":7,"./SimpleActionServer":8}],10:[function(require,module,exports){ +/** + * @fileoverview + * @author Brandon Alexander - baalexander@gmail.com + */ + +var assign = require('object-assign'); + +/** + * Message objects are used for publishing and subscribing to and from topics. + * + * @constructor + * @param values - object matching the fields defined in the .msg definition file + */ +function Message(values) { + assign(this, values); +} + +module.exports = Message; +},{"object-assign":2}],11:[function(require,module,exports){ +/** + * @fileoverview + * @author Brandon Alexander - baalexander@gmail.com + */ + +var Service = require('./Service'); +var ServiceRequest = require('./ServiceRequest'); + +/** + * A ROS parameter. + * + * @constructor + * @param options - possible keys include: + * * ros - the ROSLIB.Ros connection handle + * * name - the param name, like max_vel_x + */ +function Param(options) { + options = options || {}; + this.ros = options.ros; + this.name = options.name; +} + +/** + * Fetches the value of the param. + * + * @param callback - function with the following params: + * * value - the value of the param from ROS. + */ +Param.prototype.get = function(callback) { + var paramClient = new Service({ + ros : this.ros, + name : '/rosapi/get_param', + serviceType : 'rosapi/GetParam' + }); + + var request = new ServiceRequest({ + name : this.name + }); + + paramClient.callService(request, function(result) { + var value = JSON.parse(result.value); + callback(value); + }); +}; + +/** + * Sets the value of the param in ROS. + * + * @param value - value to set param to. + */ +Param.prototype.set = function(value, callback) { + var paramClient = new Service({ + ros : this.ros, + name : '/rosapi/set_param', + serviceType : 'rosapi/SetParam' + }); + + var request = new ServiceRequest({ + name : this.name, + value : JSON.stringify(value) + }); + + paramClient.callService(request, callback); +}; + +/** + * Delete this parameter on the ROS server. + */ +Param.prototype.delete = function(callback) { + var paramClient = new Service({ + ros : this.ros, + name : '/rosapi/delete_param', + serviceType : 'rosapi/DeleteParam' + }); + + var request = new ServiceRequest({ + name : this.name + }); + + paramClient.callService(request, callback); +}; + +module.exports = Param; +},{"./Service":13,"./ServiceRequest":14}],12:[function(require,module,exports){ +/** + * @fileoverview + * @author Brandon Alexander - baalexander@gmail.com + */ + +var WebSocket = require('ws'); +var socketAdapter = require('./SocketAdapter.js'); + +var Service = require('./Service'); +var ServiceRequest = require('./ServiceRequest'); + +var assign = require('object-assign'); +var EventEmitter2 = require('eventemitter2').EventEmitter2; + +/** + * Manages connection to the server and all interactions with ROS. + * + * Emits the following events: + * * 'error' - there was an error with ROS + * * 'connection' - connected to the WebSocket server + * * 'close' - disconnected to the WebSocket server + * * - a message came from rosbridge with the given topic name + * * - a service response came from rosbridge with the given ID + * + * @constructor + * @param options - possible keys include:
+ * * url (optional) - (can be specified later with `connect`) the WebSocket URL for rosbridge or the node server url to connect using socket.io (if socket.io exists in the page)
+ * * groovyCompatibility - don't use interfaces that changed after the last groovy release or rosbridge_suite and related tools (defaults to true) + * * transportLibrary (optional) - one of 'websocket' (default), 'socket.io' or RTCPeerConnection instance controlling how the connection is created in `connect`. + * * transportOptions (optional) - the options to use use when creating a connection. Currently only used if `transportLibrary` is RTCPeerConnection. + */ +function Ros(options) { + options = options || {}; + this.socket = null; + this.idCounter = 0; + this.isConnected = false; + this.transportLibrary = options.transportLibrary || 'websocket'; + this.transportOptions = options.transportOptions || {}; + + if (typeof options.groovyCompatibility === 'undefined') { + this.groovyCompatibility = true; + } + else { + this.groovyCompatibility = options.groovyCompatibility; + } + + // Sets unlimited event listeners. + this.setMaxListeners(0); + + // begin by checking if a URL was given + if (options.url) { + this.connect(options.url); + } +} + +Ros.prototype.__proto__ = EventEmitter2.prototype; + +/** + * Connect to the specified WebSocket. + * + * @param url - WebSocket URL or RTCDataChannel label for Rosbridge + */ +Ros.prototype.connect = function(url) { + if (this.transportLibrary === 'socket.io') { + this.socket = assign(io(url, {'force new connection': true}), socketAdapter(this)); + this.socket.on('connect', this.socket.onopen); + this.socket.on('data', this.socket.onmessage); + this.socket.on('close', this.socket.onclose); + this.socket.on('error', this.socket.onerror); + } else if (this.transportLibrary.constructor.name === 'RTCPeerConnection') { + this.socket = assign(this.transportLibrary.createDataChannel(url, this.transportOptions), socketAdapter(this)); + }else { + this.socket = assign(new WebSocket(url), socketAdapter(this)); + } + +}; + +/** + * Disconnect from the WebSocket server. + */ +Ros.prototype.close = function() { + if (this.socket) { + this.socket.close(); + } +}; + +/** + * Sends an authorization request to the server. + * + * @param mac - MAC (hash) string given by the trusted source. + * @param client - IP of the client. + * @param dest - IP of the destination. + * @param rand - Random string given by the trusted source. + * @param t - Time of the authorization request. + * @param level - User level as a string given by the client. + * @param end - End time of the client's session. + */ +Ros.prototype.authenticate = function(mac, client, dest, rand, t, level, end) { + // create the request + var auth = { + op : 'auth', + mac : mac, + client : client, + dest : dest, + rand : rand, + t : t, + level : level, + end : end + }; + // send the request + this.callOnConnection(auth); +}; + +/** + * Sends the message over the WebSocket, but queues the message up if not yet + * connected. + */ +Ros.prototype.callOnConnection = function(message) { + var that = this; + var messageJson = JSON.stringify(message); + var emitter = null; + if (this.transportLibrary === 'socket.io') { + emitter = function(msg){that.socket.emit('operation', msg);}; + } else { + emitter = function(msg){that.socket.send(msg);}; + } + + if (!this.isConnected) { + that.once('connection', function() { + emitter(messageJson); + }); + } else { + emitter(messageJson); + } +}; + +/** + * Sends a set_level request to the server + * + * @param level - Status level (none, error, warning, info) + * @param id - Optional: Operation ID to change status level on + */ +Ros.prototype.setStatusLevel = function(level, id){ + var levelMsg = { + op: 'set_level', + level: level, + id: id + }; + + this.callOnConnection(levelMsg); +}; + +/** + * Retrieves Action Servers in ROS as an array of string + * + * * actionservers - Array of action server names + */ +Ros.prototype.getActionServers = function(callback, failedCallback) { + var getActionServers = new Service({ + ros : this, + name : '/rosapi/action_servers', + serviceType : 'rosapi/GetActionServers' + }); + + var request = new ServiceRequest({}); + if (typeof failedCallback === 'function'){ + getActionServers.callService(request, + function(result) { + callback(result.action_servers); + }, + function(message){ + failedCallback(message); + } + ); + }else{ + getActionServers.callService(request, function(result) { + callback(result.action_servers); + }); + } +}; + +/** + * Retrieves list of topics in ROS as an array. + * + * @param callback function with params: + * * topics - Array of topic names + */ +Ros.prototype.getTopics = function(callback, failedCallback) { + var topicsClient = new Service({ + ros : this, + name : '/rosapi/topics', + serviceType : 'rosapi/Topics' + }); + + var request = new ServiceRequest(); + if (typeof failedCallback === 'function'){ + topicsClient.callService(request, + function(result) { + callback(result); + }, + function(message){ + failedCallback(message); + } + ); + }else{ + topicsClient.callService(request, function(result) { + callback(result); + }); + } +}; + +/** + * Retrieves Topics in ROS as an array as specific type + * + * @param topicType topic type to find: + * @param callback function with params: + * * topics - Array of topic names + */ +Ros.prototype.getTopicsForType = function(topicType, callback, failedCallback) { + var topicsForTypeClient = new Service({ + ros : this, + name : '/rosapi/topics_for_type', + serviceType : 'rosapi/TopicsForType' + }); + + var request = new ServiceRequest({ + type: topicType + }); + if (typeof failedCallback === 'function'){ + topicsForTypeClient.callService(request, + function(result) { + callback(result.topics); + }, + function(message){ + failedCallback(message); + } + ); + }else{ + topicsForTypeClient.callService(request, function(result) { + callback(result.topics); + }); + } +}; + +/** + * Retrieves list of active service names in ROS. + * + * @param callback - function with the following params: + * * services - array of service names + */ +Ros.prototype.getServices = function(callback, failedCallback) { + var servicesClient = new Service({ + ros : this, + name : '/rosapi/services', + serviceType : 'rosapi/Services' + }); + + var request = new ServiceRequest(); + if (typeof failedCallback === 'function'){ + servicesClient.callService(request, + function(result) { + callback(result.services); + }, + function(message) { + failedCallback(message); + } + ); + }else{ + servicesClient.callService(request, function(result) { + callback(result.services); + }); + } +}; + +/** + * Retrieves list of services in ROS as an array as specific type + * + * @param serviceType service type to find: + * @param callback function with params: + * * topics - Array of service names + */ +Ros.prototype.getServicesForType = function(serviceType, callback, failedCallback) { + var servicesForTypeClient = new Service({ + ros : this, + name : '/rosapi/services_for_type', + serviceType : 'rosapi/ServicesForType' + }); + + var request = new ServiceRequest({ + type: serviceType + }); + if (typeof failedCallback === 'function'){ + servicesForTypeClient.callService(request, + function(result) { + callback(result.services); + }, + function(message) { + failedCallback(message); + } + ); + }else{ + servicesForTypeClient.callService(request, function(result) { + callback(result.services); + }); + } +}; + +/** + * Retrieves a detail of ROS service request. + * + * @param service name of service: + * @param callback - function with params: + * * type - String of the service type + */ +Ros.prototype.getServiceRequestDetails = function(type, callback, failedCallback) { + var serviceTypeClient = new Service({ + ros : this, + name : '/rosapi/service_request_details', + serviceType : 'rosapi/ServiceRequestDetails' + }); + var request = new ServiceRequest({ + type: type + }); + + if (typeof failedCallback === 'function'){ + serviceTypeClient.callService(request, + function(result) { + callback(result); + }, + function(message){ + failedCallback(message); + } + ); + }else{ + serviceTypeClient.callService(request, function(result) { + callback(result); + }); + } +}; + +/** + * Retrieves a detail of ROS service request. + * + * @param service name of service: + * @param callback - function with params: + * * type - String of the service type + */ +Ros.prototype.getServiceResponseDetails = function(type, callback, failedCallback) { + var serviceTypeClient = new Service({ + ros : this, + name : '/rosapi/service_response_details', + serviceType : 'rosapi/ServiceResponseDetails' + }); + var request = new ServiceRequest({ + type: type + }); + + if (typeof failedCallback === 'function'){ + serviceTypeClient.callService(request, + function(result) { + callback(result); + }, + function(message){ + failedCallback(message); + } + ); + }else{ + serviceTypeClient.callService(request, function(result) { + callback(result); + }); + } +}; + +/** + * Retrieves list of active node names in ROS. + * + * @param callback - function with the following params: + * * nodes - array of node names + */ +Ros.prototype.getNodes = function(callback, failedCallback) { + var nodesClient = new Service({ + ros : this, + name : '/rosapi/nodes', + serviceType : 'rosapi/Nodes' + }); + + var request = new ServiceRequest(); + if (typeof failedCallback === 'function'){ + nodesClient.callService(request, + function(result) { + callback(result.nodes); + }, + function(message) { + failedCallback(message); + } + ); + }else{ + nodesClient.callService(request, function(result) { + callback(result.nodes); + }); + } +}; + +/** + * Retrieves list subscribed topics, publishing topics and services of a specific node + * + * @param node name of the node: + * @param callback - function with params: + * * publications - array of published topic names + * * subscriptions - array of subscribed topic names + * * services - array of service names hosted + */ +Ros.prototype.getNodeDetails = function(node, callback, failedCallback) { + var nodesClient = new Service({ + ros : this, + name : '/rosapi/node_details', + serviceType : 'rosapi/NodeDetails' + }); + + var request = new ServiceRequest({ + node: node + }); + if (typeof failedCallback === 'function'){ + nodesClient.callService(request, + function(result) { + callback(result.subscribing, result.publishing, result.services); + }, + function(message) { + failedCallback(message); + } + ); + } else { + nodesClient.callService(request, function(result) { + callback(result); + }); + } +}; + +/** + * Retrieves list of param names from the ROS Parameter Server. + * + * @param callback function with params: + * * params - array of param names. + */ +Ros.prototype.getParams = function(callback, failedCallback) { + var paramsClient = new Service({ + ros : this, + name : '/rosapi/get_param_names', + serviceType : 'rosapi/GetParamNames' + }); + var request = new ServiceRequest(); + if (typeof failedCallback === 'function'){ + paramsClient.callService(request, + function(result) { + callback(result.names); + }, + function(message){ + failedCallback(message); + } + ); + }else{ + paramsClient.callService(request, function(result) { + callback(result.names); + }); + } +}; + +/** + * Retrieves a type of ROS topic. + * + * @param topic name of the topic: + * @param callback - function with params: + * * type - String of the topic type + */ +Ros.prototype.getTopicType = function(topic, callback, failedCallback) { + var topicTypeClient = new Service({ + ros : this, + name : '/rosapi/topic_type', + serviceType : 'rosapi/TopicType' + }); + var request = new ServiceRequest({ + topic: topic + }); + + if (typeof failedCallback === 'function'){ + topicTypeClient.callService(request, + function(result) { + callback(result.type); + }, + function(message){ + failedCallback(message); + } + ); + }else{ + topicTypeClient.callService(request, function(result) { + callback(result.type); + }); + } +}; + +/** + * Retrieves a type of ROS service. + * + * @param service name of service: + * @param callback - function with params: + * * type - String of the service type + */ +Ros.prototype.getServiceType = function(service, callback, failedCallback) { + var serviceTypeClient = new Service({ + ros : this, + name : '/rosapi/service_type', + serviceType : 'rosapi/ServiceType' + }); + var request = new ServiceRequest({ + service: service + }); + + if (typeof failedCallback === 'function'){ + serviceTypeClient.callService(request, + function(result) { + callback(result.type); + }, + function(message){ + failedCallback(message); + } + ); + }else{ + serviceTypeClient.callService(request, function(result) { + callback(result.type); + }); + } +}; + +/** + * Retrieves a detail of ROS message. + * + * @param callback - function with params: + * * details - Array of the message detail + * @param message - String of a topic type + */ +Ros.prototype.getMessageDetails = function(message, callback, failedCallback) { + var messageDetailClient = new Service({ + ros : this, + name : '/rosapi/message_details', + serviceType : 'rosapi/MessageDetails' + }); + var request = new ServiceRequest({ + type: message + }); + + if (typeof failedCallback === 'function'){ + messageDetailClient.callService(request, + function(result) { + callback(result.typedefs); + }, + function(message){ + failedCallback(message); + } + ); + }else{ + messageDetailClient.callService(request, function(result) { + callback(result.typedefs); + }); + } +}; + +/** + * Decode a typedefs into a dictionary like `rosmsg show foo/bar` + * + * @param defs - array of type_def dictionary + */ +Ros.prototype.decodeTypeDefs = function(defs) { + var that = this; + + // calls itself recursively to resolve type definition using hints. + var decodeTypeDefsRec = function(theType, hints) { + var typeDefDict = {}; + for (var i = 0; i < theType.fieldnames.length; i++) { + var arrayLen = theType.fieldarraylen[i]; + var fieldName = theType.fieldnames[i]; + var fieldType = theType.fieldtypes[i]; + if (fieldType.indexOf('/') === -1) { // check the fieldType includes '/' or not + if (arrayLen === -1) { + typeDefDict[fieldName] = fieldType; + } + else { + typeDefDict[fieldName] = [fieldType]; + } + } + else { + // lookup the name + var sub = false; + for (var j = 0; j < hints.length; j++) { + if (hints[j].type.toString() === fieldType.toString()) { + sub = hints[j]; + break; + } + } + if (sub) { + var subResult = decodeTypeDefsRec(sub, hints); + if (arrayLen === -1) { + } + else { + typeDefDict[fieldName] = [subResult]; + } + } + else { + that.emit('error', 'Cannot find ' + fieldType + ' in decodeTypeDefs'); + } + } + } + return typeDefDict; + }; + + return decodeTypeDefsRec(defs[0], defs); +}; + + +module.exports = Ros; + +},{"./Service":13,"./ServiceRequest":14,"./SocketAdapter.js":16,"eventemitter2":1,"object-assign":2,"ws":39}],13:[function(require,module,exports){ +/** + * @fileoverview + * @author Brandon Alexander - baalexander@gmail.com + */ + +var ServiceResponse = require('./ServiceResponse'); +var ServiceRequest = require('./ServiceRequest'); +var EventEmitter2 = require('eventemitter2').EventEmitter2; + +/** + * A ROS service client. + * + * @constructor + * @params options - possible keys include: + * * ros - the ROSLIB.Ros connection handle + * * name - the service name, like /add_two_ints + * * serviceType - the service type, like 'rospy_tutorials/AddTwoInts' + */ +function Service(options) { + options = options || {}; + this.ros = options.ros; + this.name = options.name; + this.serviceType = options.serviceType; + this.isAdvertised = false; + + this._serviceCallback = null; +} +Service.prototype.__proto__ = EventEmitter2.prototype; +/** + * Calls the service. Returns the service response in the callback. + * + * @param request - the ROSLIB.ServiceRequest to send + * @param callback - function with params: + * * response - the response from the service request + * @param failedCallback - the callback function when the service call failed (optional). Params: + * * error - the error message reported by ROS + */ +Service.prototype.callService = function(request, callback, failedCallback) { + if (this.isAdvertised) { + return; + } + + var serviceCallId = 'call_service:' + this.name + ':' + (++this.ros.idCounter); + + if (callback || failedCallback) { + this.ros.once(serviceCallId, function(message) { + if (message.result !== undefined && message.result === false) { + if (typeof failedCallback === 'function') { + failedCallback(message.values); + } + } else if (typeof callback === 'function') { + callback(new ServiceResponse(message.values)); + } + }); + } + + var call = { + op : 'call_service', + id : serviceCallId, + service : this.name, + args : request + }; + this.ros.callOnConnection(call); +}; + +/** + * Every time a message is published for the given topic, the callback + * will be called with the message object. + * + * @param callback - function with the following params: + * * message - the published message + */ +Service.prototype.advertise = function(callback) { + if (this.isAdvertised || typeof callback !== 'function') { + return; + } + + this._serviceCallback = callback; + this.ros.on(this.name, this._serviceResponse.bind(this)); + this.ros.callOnConnection({ + op: 'advertise_service', + type: this.serviceType, + service: this.name + }); + this.isAdvertised = true; +}; + +Service.prototype.unadvertise = function() { + if (!this.isAdvertised) { + return; + } + this.ros.callOnConnection({ + op: 'unadvertise_service', + service: this.name + }); + this.isAdvertised = false; +}; + +Service.prototype._serviceResponse = function(rosbridgeRequest) { + var response = {}; + var success = this._serviceCallback(rosbridgeRequest.args, response); + + var call = { + op: 'service_response', + service: this.name, + values: new ServiceResponse(response), + result: success + }; + + if (rosbridgeRequest.id) { + call.id = rosbridgeRequest.id; + } + + this.ros.callOnConnection(call); +}; + +module.exports = Service; +},{"./ServiceRequest":14,"./ServiceResponse":15,"eventemitter2":1}],14:[function(require,module,exports){ +/** + * @fileoverview + * @author Brandon Alexander - balexander@willowgarage.com + */ + +var assign = require('object-assign'); + +/** + * A ServiceRequest is passed into the service call. + * + * @constructor + * @param values - object matching the fields defined in the .srv definition file + */ +function ServiceRequest(values) { + assign(this, values); +} + +module.exports = ServiceRequest; +},{"object-assign":2}],15:[function(require,module,exports){ +/** + * @fileoverview + * @author Brandon Alexander - balexander@willowgarage.com + */ + +var assign = require('object-assign'); + +/** + * A ServiceResponse is returned from the service call. + * + * @constructor + * @param values - object matching the fields defined in the .srv definition file + */ +function ServiceResponse(values) { + assign(this, values); +} + +module.exports = ServiceResponse; +},{"object-assign":2}],16:[function(require,module,exports){ +/** + * Socket event handling utilities for handling events on either + * WebSocket and TCP sockets + * + * Note to anyone reviewing this code: these functions are called + * in the context of their parent object, unless bound + * @fileOverview + */ +'use strict'; + +var decompressPng = require('../util/decompressPng'); +var WebSocket = require('ws'); +var BSON = null; +if(typeof bson !== 'undefined'){ + BSON = bson().BSON; +} + +/** + * Events listeners for a WebSocket or TCP socket to a JavaScript + * ROS Client. Sets up Messages for a given topic to trigger an + * event on the ROS client. + * + * @namespace SocketAdapter + * @private + */ +function SocketAdapter(client) { + function handleMessage(message) { + if (message.op === 'publish') { + client.emit(message.topic, message.msg); + } else if (message.op === 'service_response') { + client.emit(message.id, message); + } else if (message.op === 'call_service') { + client.emit(message.service, message); + } else if(message.op === 'status'){ + if(message.id){ + client.emit('status:'+message.id, message); + } else { + client.emit('status', message); + } + } + } + + function handlePng(message, callback) { + if (message.op === 'png') { + decompressPng(message.data, callback); + } else { + callback(message); + } + } + + function decodeBSON(data, callback) { + if (!BSON) { + throw 'Cannot process BSON encoded message without BSON header.'; + } + var reader = new FileReader(); + reader.onload = function() { + var uint8Array = new Uint8Array(this.result); + var msg = BSON.deserialize(uint8Array); + callback(msg); + }; + reader.readAsArrayBuffer(data); + } + + return { + /** + * Emits a 'connection' event on WebSocket connection. + * + * @param event - the argument to emit with the event. + * @memberof SocketAdapter + */ + onopen: function onOpen(event) { + client.isConnected = true; + client.emit('connection', event); + }, + + /** + * Emits a 'close' event on WebSocket disconnection. + * + * @param event - the argument to emit with the event. + * @memberof SocketAdapter + */ + onclose: function onClose(event) { + client.isConnected = false; + client.emit('close', event); + }, + + /** + * Emits an 'error' event whenever there was an error. + * + * @param event - the argument to emit with the event. + * @memberof SocketAdapter + */ + onerror: function onError(event) { + client.emit('error', event); + }, + + /** + * Parses message responses from rosbridge and sends to the appropriate + * topic, service, or param. + * + * @param message - the raw JSON message from rosbridge. + * @memberof SocketAdapter + */ + onmessage: function onMessage(data) { + if (typeof Blob !== 'undefined' && data.data instanceof Blob) { + decodeBSON(data.data, function (message) { + handlePng(message, handleMessage); + }); + } else { + var message = JSON.parse(typeof data === 'string' ? data : data.data); + handlePng(message, handleMessage); + } + } + }; +} + +module.exports = SocketAdapter; + +},{"../util/decompressPng":41,"ws":39}],17:[function(require,module,exports){ +/** + * @fileoverview + * @author Brandon Alexander - baalexander@gmail.com + */ + +var EventEmitter2 = require('eventemitter2').EventEmitter2; +var Message = require('./Message'); + +/** + * Publish and/or subscribe to a topic in ROS. + * + * Emits the following events: + * * 'warning' - if there are any warning during the Topic creation + * * 'message' - the message data from rosbridge + * + * @constructor + * @param options - object with following keys: + * * ros - the ROSLIB.Ros connection handle + * * name - the topic name, like /cmd_vel + * * messageType - the message type, like 'std_msgs/String' + * * compression - the type of compression to use, like 'png' + * * throttle_rate - the rate (in ms in between messages) at which to throttle the topics + * * queue_size - the queue created at bridge side for re-publishing webtopics (defaults to 100) + * * latch - latch the topic when publishing + * * queue_length - the queue length at bridge side used when subscribing (defaults to 0, no queueing). + * * reconnect_on_close - the flag to enable resubscription and readvertisement on close event(defaults to true). + */ +function Topic(options) { + options = options || {}; + this.ros = options.ros; + this.name = options.name; + this.messageType = options.messageType; + this.isAdvertised = false; + this.compression = options.compression || 'none'; + this.throttle_rate = options.throttle_rate || 0; + this.latch = options.latch || false; + this.queue_size = options.queue_size || 100; + this.queue_length = options.queue_length || 0; + this.reconnect_on_close = options.reconnect_on_close || true; + + // Check for valid compression types + if (this.compression && this.compression !== 'png' && + this.compression !== 'none') { + this.emit('warning', this.compression + + ' compression is not supported. No compression will be used.'); + } + + // Check if throttle rate is negative + if (this.throttle_rate < 0) { + this.emit('warning', this.throttle_rate + ' is not allowed. Set to 0'); + this.throttle_rate = 0; + } + + var that = this; + if (this.reconnect_on_close) { + this.callForSubscribeAndAdvertise = function(message) { + that.ros.callOnConnection(message); + + that.waitForReconnect = false; + that.reconnectFunc = function() { + if(!that.waitForReconnect) { + that.waitForReconnect = true; + that.ros.callOnConnection(message); + that.ros.once('connection', function() { + that.waitForReconnect = false; + }); + } + }; + that.ros.on('close', that.reconnectFunc); + }; + } + else { + this.callForSubscribeAndAdvertise = this.ros.callOnConnection; + } + + this._messageCallback = function(data) { + that.emit('message', new Message(data)); + }; +} +Topic.prototype.__proto__ = EventEmitter2.prototype; + +/** + * Every time a message is published for the given topic, the callback + * will be called with the message object. + * + * @param callback - function with the following params: + * * message - the published message + */ +Topic.prototype.subscribe = function(callback) { + if (typeof callback === 'function') { + this.on('message', callback); + } + + if (this.subscribeId) { return; } + this.ros.on(this.name, this._messageCallback); + this.subscribeId = 'subscribe:' + this.name + ':' + (++this.ros.idCounter); + + this.callForSubscribeAndAdvertise({ + op: 'subscribe', + id: this.subscribeId, + type: this.messageType, + topic: this.name, + compression: this.compression, + throttle_rate: this.throttle_rate, + queue_length: this.queue_length + }); +}; + +/** + * Unregisters as a subscriber for the topic. Unsubscribing stop remove + * all subscribe callbacks. To remove a call back, you must explicitly + * pass the callback function in. + * + * @param callback - the optional callback to unregister, if + * * provided and other listeners are registered the topic won't + * * unsubscribe, just stop emitting to the passed listener + */ +Topic.prototype.unsubscribe = function(callback) { + if (callback) { + this.off('message', callback); + // If there is any other callbacks still subscribed don't unsubscribe + if (this.listeners('message').length) { return; } + } + if (!this.subscribeId) { return; } + // Note: Don't call this.removeAllListeners, allow client to handle that themselves + this.ros.off(this.name, this._messageCallback); + if(this.reconnect_on_close) { + this.ros.off('close', this.reconnectFunc); + } + this.emit('unsubscribe'); + this.ros.callOnConnection({ + op: 'unsubscribe', + id: this.subscribeId, + topic: this.name + }); + this.subscribeId = null; +}; + + +/** + * Registers as a publisher for the topic. + */ +Topic.prototype.advertise = function() { + if (this.isAdvertised) { + return; + } + this.advertiseId = 'advertise:' + this.name + ':' + (++this.ros.idCounter); + this.callForSubscribeAndAdvertise({ + op: 'advertise', + id: this.advertiseId, + type: this.messageType, + topic: this.name, + latch: this.latch, + queue_size: this.queue_size + }); + this.isAdvertised = true; + + if(!this.reconnect_on_close) { + var that = this; + this.ros.on('close', function() { + that.isAdvertised = false; + }); + } +}; + +/** + * Unregisters as a publisher for the topic. + */ +Topic.prototype.unadvertise = function() { + if (!this.isAdvertised) { + return; + } + if(this.reconnect_on_close) { + this.ros.off('close', this.reconnectFunc); + } + this.emit('unadvertise'); + this.ros.callOnConnection({ + op: 'unadvertise', + id: this.advertiseId, + topic: this.name + }); + this.isAdvertised = false; +}; + +/** + * Publish the message. + * + * @param message - A ROSLIB.Message object. + */ +Topic.prototype.publish = function(message) { + if (!this.isAdvertised) { + this.advertise(); + } + + this.ros.idCounter++; + var call = { + op: 'publish', + id: 'publish:' + this.name + ':' + this.ros.idCounter, + topic: this.name, + msg: message, + latch: this.latch + }; + this.ros.callOnConnection(call); +}; + +module.exports = Topic; + +},{"./Message":10,"eventemitter2":1}],18:[function(require,module,exports){ +var mixin = require('../mixin'); + +var core = module.exports = { + Ros: require('./Ros'), + Topic: require('./Topic'), + Message: require('./Message'), + Param: require('./Param'), + Service: require('./Service'), + ServiceRequest: require('./ServiceRequest'), + ServiceResponse: require('./ServiceResponse') +}; + +mixin(core.Ros, ['Param', 'Service', 'Topic'], core); + +},{"../mixin":24,"./Message":10,"./Param":11,"./Ros":12,"./Service":13,"./ServiceRequest":14,"./ServiceResponse":15,"./Topic":17}],19:[function(require,module,exports){ +/** + * @fileoverview + * @author David Gossow - dgossow@willowgarage.com + */ + +var Vector3 = require('./Vector3'); +var Quaternion = require('./Quaternion'); + +/** + * A Pose in 3D space. Values are copied into this object. + * + * @constructor + * @param options - object with following keys: + * * position - the Vector3 describing the position + * * orientation - the ROSLIB.Quaternion describing the orientation + */ +function Pose(options) { + options = options || {}; + // copy the values into this object if they exist + this.position = new Vector3(options.position); + this.orientation = new Quaternion(options.orientation); +} + +/** + * Apply a transform against this pose. + * + * @param tf the transform + */ +Pose.prototype.applyTransform = function(tf) { + this.position.multiplyQuaternion(tf.rotation); + this.position.add(tf.translation); + var tmp = tf.rotation.clone(); + tmp.multiply(this.orientation); + this.orientation = tmp; +}; + +/** + * Clone a copy of this pose. + * + * @returns the cloned pose + */ +Pose.prototype.clone = function() { + return new Pose(this); +}; + +module.exports = Pose; +},{"./Quaternion":20,"./Vector3":22}],20:[function(require,module,exports){ +/** + * @fileoverview + * @author David Gossow - dgossow@willowgarage.com + */ + +/** + * A Quaternion. + * + * @constructor + * @param options - object with following keys: + * * x - the x value + * * y - the y value + * * z - the z value + * * w - the w value + */ +function Quaternion(options) { + options = options || {}; + this.x = options.x || 0; + this.y = options.y || 0; + this.z = options.z || 0; + this.w = (typeof options.w === 'number') ? options.w : 1; +} + +/** + * Perform a conjugation on this quaternion. + */ +Quaternion.prototype.conjugate = function() { + this.x *= -1; + this.y *= -1; + this.z *= -1; +}; + +/** + * Return the norm of this quaternion. + */ +Quaternion.prototype.norm = function() { + return Math.sqrt(this.x * this.x + this.y * this.y + this.z * this.z + this.w * this.w); +}; + +/** + * Perform a normalization on this quaternion. + */ +Quaternion.prototype.normalize = function() { + var l = Math.sqrt(this.x * this.x + this.y * this.y + this.z * this.z + this.w * this.w); + if (l === 0) { + this.x = 0; + this.y = 0; + this.z = 0; + this.w = 1; + } else { + l = 1 / l; + this.x = this.x * l; + this.y = this.y * l; + this.z = this.z * l; + this.w = this.w * l; + } +}; + +/** + * Convert this quaternion into its inverse. + */ +Quaternion.prototype.invert = function() { + this.conjugate(); + this.normalize(); +}; + +/** + * Set the values of this quaternion to the product of itself and the given quaternion. + * + * @param q the quaternion to multiply with + */ +Quaternion.prototype.multiply = function(q) { + var newX = this.x * q.w + this.y * q.z - this.z * q.y + this.w * q.x; + var newY = -this.x * q.z + this.y * q.w + this.z * q.x + this.w * q.y; + var newZ = this.x * q.y - this.y * q.x + this.z * q.w + this.w * q.z; + var newW = -this.x * q.x - this.y * q.y - this.z * q.z + this.w * q.w; + this.x = newX; + this.y = newY; + this.z = newZ; + this.w = newW; +}; + +/** + * Clone a copy of this quaternion. + * + * @returns the cloned quaternion + */ +Quaternion.prototype.clone = function() { + return new Quaternion(this); +}; + +module.exports = Quaternion; + +},{}],21:[function(require,module,exports){ +/** + * @fileoverview + * @author David Gossow - dgossow@willowgarage.com + */ + +var Vector3 = require('./Vector3'); +var Quaternion = require('./Quaternion'); + +/** + * A Transform in 3-space. Values are copied into this object. + * + * @constructor + * @param options - object with following keys: + * * translation - the Vector3 describing the translation + * * rotation - the ROSLIB.Quaternion describing the rotation + */ +function Transform(options) { + options = options || {}; + // Copy the values into this object if they exist + this.translation = new Vector3(options.translation); + this.rotation = new Quaternion(options.rotation); +} + +/** + * Clone a copy of this transform. + * + * @returns the cloned transform + */ +Transform.prototype.clone = function() { + return new Transform(this); +}; + +module.exports = Transform; +},{"./Quaternion":20,"./Vector3":22}],22:[function(require,module,exports){ +/** + * @fileoverview + * @author David Gossow - dgossow@willowgarage.com + */ + +/** + * A 3D vector. + * + * @constructor + * @param options - object with following keys: + * * x - the x value + * * y - the y value + * * z - the z value + */ +function Vector3(options) { + options = options || {}; + this.x = options.x || 0; + this.y = options.y || 0; + this.z = options.z || 0; +} + +/** + * Set the values of this vector to the sum of itself and the given vector. + * + * @param v the vector to add with + */ +Vector3.prototype.add = function(v) { + this.x += v.x; + this.y += v.y; + this.z += v.z; +}; + +/** + * Set the values of this vector to the difference of itself and the given vector. + * + * @param v the vector to subtract with + */ +Vector3.prototype.subtract = function(v) { + this.x -= v.x; + this.y -= v.y; + this.z -= v.z; +}; + +/** + * Multiply the given Quaternion with this vector. + * + * @param q - the quaternion to multiply with + */ +Vector3.prototype.multiplyQuaternion = function(q) { + var ix = q.w * this.x + q.y * this.z - q.z * this.y; + var iy = q.w * this.y + q.z * this.x - q.x * this.z; + var iz = q.w * this.z + q.x * this.y - q.y * this.x; + var iw = -q.x * this.x - q.y * this.y - q.z * this.z; + this.x = ix * q.w + iw * -q.x + iy * -q.z - iz * -q.y; + this.y = iy * q.w + iw * -q.y + iz * -q.x - ix * -q.z; + this.z = iz * q.w + iw * -q.z + ix * -q.y - iy * -q.x; +}; + +/** + * Clone a copy of this vector. + * + * @returns the cloned vector + */ +Vector3.prototype.clone = function() { + return new Vector3(this); +}; + +module.exports = Vector3; +},{}],23:[function(require,module,exports){ +module.exports = { + Pose: require('./Pose'), + Quaternion: require('./Quaternion'), + Transform: require('./Transform'), + Vector3: require('./Vector3') +}; + +},{"./Pose":19,"./Quaternion":20,"./Transform":21,"./Vector3":22}],24:[function(require,module,exports){ +/** + * Mixin a feature to the core/Ros prototype. + * For example, mixin(Ros, ['Topic'], {Topic: }) + * will add a topic bound to any Ros instances so a user + * can call `var topic = ros.Topic({name: '/foo'});` + * + * @author Graeme Yeates - github.com/megawac + */ +module.exports = function(Ros, classes, features) { + classes.forEach(function(className) { + var Class = features[className]; + Ros.prototype[className] = function(options) { + options.ros = this; + return new Class(options); + }; + }); +}; + +},{}],25:[function(require,module,exports){ +/** + * @fileoverview + * @author David Gossow - dgossow@willowgarage.com + */ + +var ActionClient = require('../actionlib/ActionClient'); +var Goal = require('../actionlib/Goal'); + +var Service = require('../core/Service.js'); +var ServiceRequest = require('../core/ServiceRequest.js'); + +var Transform = require('../math/Transform'); + +/** + * A TF Client that listens to TFs from tf2_web_republisher. + * + * @constructor + * @param options - object with following keys: + * * ros - the ROSLIB.Ros connection handle + * * fixedFrame - the fixed frame, like /base_link + * * angularThres - the angular threshold for the TF republisher + * * transThres - the translation threshold for the TF republisher + * * rate - the rate for the TF republisher + * * updateDelay - the time (in ms) to wait after a new subscription + * to update the TF republisher's list of TFs + * * topicTimeout - the timeout parameter for the TF republisher + * * serverName (optional) - the name of the tf2_web_republisher server + * * repubServiceName (optional) - the name of the republish_tfs service (non groovy compatibility mode only) + * default: '/republish_tfs' + */ +function TFClient(options) { + options = options || {}; + this.ros = options.ros; + this.fixedFrame = options.fixedFrame || '/base_link'; + this.angularThres = options.angularThres || 2.0; + this.transThres = options.transThres || 0.01; + this.rate = options.rate || 10.0; + this.updateDelay = options.updateDelay || 50; + var seconds = options.topicTimeout || 2.0; + var secs = Math.floor(seconds); + var nsecs = Math.floor((seconds - secs) * 1000000000); + this.topicTimeout = { + secs: secs, + nsecs: nsecs + }; + this.serverName = options.serverName || '/tf2_web_republisher'; + this.repubServiceName = options.repubServiceName || '/republish_tfs'; + + this.currentGoal = false; + this.currentTopic = false; + this.frameInfos = {}; + this.republisherUpdateRequested = false; + + // Create an Action client + this.actionClient = this.ros.ActionClient({ + serverName : this.serverName, + actionName : 'tf2_web_republisher/TFSubscriptionAction', + omitStatus : true, + omitResult : true + }); + + // Create a Service client + this.serviceClient = this.ros.Service({ + name: this.repubServiceName, + serviceType: 'tf2_web_republisher/RepublishTFs' + }); +} + +/** + * Process the incoming TF message and send them out using the callback + * functions. + * + * @param tf - the TF message from the server + */ +TFClient.prototype.processTFArray = function(tf) { + var that = this; + tf.transforms.forEach(function(transform) { + var frameID = transform.child_frame_id; + if (frameID[0] === '/') + { + frameID = frameID.substring(1); + } + var info = this.frameInfos[frameID]; + if (info) { + info.transform = new Transform({ + translation : transform.transform.translation, + rotation : transform.transform.rotation + }); + info.cbs.forEach(function(cb) { + cb(info.transform); + }); + } + }, this); +}; + +/** + * Create and send a new goal (or service request) to the tf2_web_republisher + * based on the current list of TFs. + */ +TFClient.prototype.updateGoal = function() { + var goalMessage = { + source_frames : Object.keys(this.frameInfos), + target_frame : this.fixedFrame, + angular_thres : this.angularThres, + trans_thres : this.transThres, + rate : this.rate + }; + + // if we're running in groovy compatibility mode (the default) + // then use the action interface to tf2_web_republisher + if(this.ros.groovyCompatibility) { + if (this.currentGoal) { + this.currentGoal.cancel(); + } + this.currentGoal = new Goal({ + actionClient : this.actionClient, + goalMessage : goalMessage + }); + + this.currentGoal.on('feedback', this.processTFArray.bind(this)); + this.currentGoal.send(); + } + else { + // otherwise, use the service interface + // The service interface has the same parameters as the action, + // plus the timeout + goalMessage.timeout = this.topicTimeout; + var request = new ServiceRequest(goalMessage); + + this.serviceClient.callService(request, this.processResponse.bind(this)); + } + + this.republisherUpdateRequested = false; +}; + +/** + * Process the service response and subscribe to the tf republisher + * topic + * + * @param response the service response containing the topic name + */ +TFClient.prototype.processResponse = function(response) { + // if we subscribed to a topic before, unsubscribe so + // the republisher stops publishing it + if (this.currentTopic) { + this.currentTopic.unsubscribe(); + } + + this.currentTopic = this.ros.Topic({ + name: response.topic_name, + messageType: 'tf2_web_republisher/TFArray' + }); + this.currentTopic.subscribe(this.processTFArray.bind(this)); +}; + +/** + * Subscribe to the given TF frame. + * + * @param frameID - the TF frame to subscribe to + * @param callback - function with params: + * * transform - the transform data + */ +TFClient.prototype.subscribe = function(frameID, callback) { + // remove leading slash, if it's there + if (frameID[0] === '/') + { + frameID = frameID.substring(1); + } + // if there is no callback registered for the given frame, create emtpy callback list + if (!this.frameInfos[frameID]) { + this.frameInfos[frameID] = { + cbs: [] + }; + if (!this.republisherUpdateRequested) { + setTimeout(this.updateGoal.bind(this), this.updateDelay); + this.republisherUpdateRequested = true; + } + } + // if we already have a transform, call back immediately + else if (this.frameInfos[frameID].transform) { + callback(this.frameInfos[frameID].transform); + } + this.frameInfos[frameID].cbs.push(callback); +}; + +/** + * Unsubscribe from the given TF frame. + * + * @param frameID - the TF frame to unsubscribe from + * @param callback - the callback function to remove + */ +TFClient.prototype.unsubscribe = function(frameID, callback) { + // remove leading slash, if it's there + if (frameID[0] === '/') + { + frameID = frameID.substring(1); + } + var info = this.frameInfos[frameID]; + for (var cbs = info && info.cbs || [], idx = cbs.length; idx--;) { + if (cbs[idx] === callback) { + cbs.splice(idx, 1); + } + } + if (!callback || cbs.length === 0) { + delete this.frameInfos[frameID]; + } +}; + +/** + * Unsubscribe and unadvertise all topics associated with this TFClient. + */ +TFClient.prototype.dispose = function() { + this.actionClient.dispose(); + if (this.currentTopic) { + this.currentTopic.unsubscribe(); + } +}; + +module.exports = TFClient; + +},{"../actionlib/ActionClient":5,"../actionlib/Goal":7,"../core/Service.js":13,"../core/ServiceRequest.js":14,"../math/Transform":21}],26:[function(require,module,exports){ +var Ros = require('../core/Ros'); +var mixin = require('../mixin'); + +var tf = module.exports = { + TFClient: require('./TFClient') +}; + +mixin(Ros, ['TFClient'], tf); +},{"../core/Ros":12,"../mixin":24,"./TFClient":25}],27:[function(require,module,exports){ +/** + * @fileOverview + * @author Benjamin Pitzer - ben.pitzer@gmail.com + * @author Russell Toris - rctoris@wpi.edu + */ + +var Vector3 = require('../math/Vector3'); +var UrdfTypes = require('./UrdfTypes'); + +/** + * A Box element in a URDF. + * + * @constructor + * @param options - object with following keys: + * * xml - the XML element to parse + */ +function UrdfBox(options) { + this.dimension = null; + this.type = UrdfTypes.URDF_BOX; + + // Parse the xml string + var xyz = options.xml.getAttribute('size').split(' '); + this.dimension = new Vector3({ + x : parseFloat(xyz[0]), + y : parseFloat(xyz[1]), + z : parseFloat(xyz[2]) + }); +} + +module.exports = UrdfBox; +},{"../math/Vector3":22,"./UrdfTypes":36}],28:[function(require,module,exports){ +/** + * @fileOverview + * @author Benjamin Pitzer - ben.pitzer@gmail.com + * @author Russell Toris - rctoris@wpi.edu + */ + +/** + * A Color element in a URDF. + * + * @constructor + * @param options - object with following keys: + * * xml - the XML element to parse + */ +function UrdfColor(options) { + // Parse the xml string + var rgba = options.xml.getAttribute('rgba').split(' '); + this.r = parseFloat(rgba[0]); + this.g = parseFloat(rgba[1]); + this.b = parseFloat(rgba[2]); + this.a = parseFloat(rgba[3]); +} + +module.exports = UrdfColor; +},{}],29:[function(require,module,exports){ +/** + * @fileOverview + * @author Benjamin Pitzer - ben.pitzer@gmail.com + * @author Russell Toris - rctoris@wpi.edu + */ + +var UrdfTypes = require('./UrdfTypes'); + +/** + * A Cylinder element in a URDF. + * + * @constructor + * @param options - object with following keys: + * * xml - the XML element to parse + */ +function UrdfCylinder(options) { + this.type = UrdfTypes.URDF_CYLINDER; + this.length = parseFloat(options.xml.getAttribute('length')); + this.radius = parseFloat(options.xml.getAttribute('radius')); +} + +module.exports = UrdfCylinder; +},{"./UrdfTypes":36}],30:[function(require,module,exports){ +/** + * @fileOverview + * @author David V. Lu!! davidvlu@gmail.com + */ + +/** + * A Joint element in a URDF. + * + * @constructor + * @param options - object with following keys: + * * xml - the XML element to parse + */ +function UrdfJoint(options) { + this.name = options.xml.getAttribute('name'); + this.type = options.xml.getAttribute('type'); + + var parents = options.xml.getElementsByTagName('parent'); + if(parents.length > 0) { + this.parent = parents[0].getAttribute('link'); + } + + var children = options.xml.getElementsByTagName('child'); + if(children.length > 0) { + this.child = children[0].getAttribute('link'); + } + + var limits = options.xml.getElementsByTagName('limit'); + if (limits.length > 0) { + this.minval = parseFloat( limits[0].getAttribute('lower') ); + this.maxval = parseFloat( limits[0].getAttribute('upper') ); + } +} + +module.exports = UrdfJoint; + +},{}],31:[function(require,module,exports){ +/** + * @fileOverview + * @author Benjamin Pitzer - ben.pitzer@gmail.com + * @author Russell Toris - rctoris@wpi.edu + */ + +var UrdfVisual = require('./UrdfVisual'); + +/** + * A Link element in a URDF. + * + * @constructor + * @param options - object with following keys: + * * xml - the XML element to parse + */ +function UrdfLink(options) { + this.name = options.xml.getAttribute('name'); + this.visuals = []; + var visuals = options.xml.getElementsByTagName('visual'); + + for( var i=0; i 0) { + this.textureFilename = textures[0].getAttribute('filename'); + } + + // Color + var colors = options.xml.getElementsByTagName('color'); + if (colors.length > 0) { + // Parse the RBGA string + this.color = new UrdfColor({ + xml : colors[0] + }); + } +} + +UrdfMaterial.prototype.isLink = function() { + return this.color === null && this.textureFilename === null; +}; + +var assign = require('object-assign'); + +UrdfMaterial.prototype.assign = function(obj) { + return assign(this, obj); +}; + +module.exports = UrdfMaterial; + +},{"./UrdfColor":28,"object-assign":2}],33:[function(require,module,exports){ +/** + * @fileOverview + * @author Benjamin Pitzer - ben.pitzer@gmail.com + * @author Russell Toris - rctoris@wpi.edu + */ + +var Vector3 = require('../math/Vector3'); +var UrdfTypes = require('./UrdfTypes'); + +/** + * A Mesh element in a URDF. + * + * @constructor + * @param options - object with following keys: + * * xml - the XML element to parse + */ +function UrdfMesh(options) { + this.scale = null; + + this.type = UrdfTypes.URDF_MESH; + this.filename = options.xml.getAttribute('filename'); + + // Check for a scale + var scale = options.xml.getAttribute('scale'); + if (scale) { + // Get the XYZ + var xyz = scale.split(' '); + this.scale = new Vector3({ + x : parseFloat(xyz[0]), + y : parseFloat(xyz[1]), + z : parseFloat(xyz[2]) + }); + } +} + +module.exports = UrdfMesh; +},{"../math/Vector3":22,"./UrdfTypes":36}],34:[function(require,module,exports){ +/** + * @fileOverview + * @author Benjamin Pitzer - ben.pitzer@gmail.com + * @author Russell Toris - rctoris@wpi.edu + */ + +var UrdfMaterial = require('./UrdfMaterial'); +var UrdfLink = require('./UrdfLink'); +var UrdfJoint = require('./UrdfJoint'); +var DOMParser = require('xmldom').DOMParser; + +// See https://developer.mozilla.org/docs/XPathResult#Constants +var XPATH_FIRST_ORDERED_NODE_TYPE = 9; + +/** + * A URDF Model can be used to parse a given URDF into the appropriate elements. + * + * @constructor + * @param options - object with following keys: + * * xml - the XML element to parse + * * string - the XML element to parse as a string + */ +function UrdfModel(options) { + options = options || {}; + var xmlDoc = options.xml; + var string = options.string; + this.materials = {}; + this.links = {}; + this.joints = {}; + + // Check if we are using a string or an XML element + if (string) { + // Parse the string + var parser = new DOMParser(); + xmlDoc = parser.parseFromString(string, 'text/xml'); + } + + // Initialize the model with the given XML node. + // Get the robot tag + var robotXml = xmlDoc.documentElement; + + // Get the robot name + this.name = robotXml.getAttribute('name'); + + // Parse all the visual elements we need + for (var nodes = robotXml.childNodes, i = 0; i < nodes.length; i++) { + var node = nodes[i]; + if (node.tagName === 'material') { + var material = new UrdfMaterial({ + xml : node + }); + // Make sure this is unique + if (this.materials[material.name] !== void 0) { + if( this.materials[material.name].isLink() ) { + this.materials[material.name].assign( material ); + } else { + console.warn('Material ' + material.name + 'is not unique.'); + } + } else { + this.materials[material.name] = material; + } + } else if (node.tagName === 'link') { + var link = new UrdfLink({ + xml : node + }); + // Make sure this is unique + if (this.links[link.name] !== void 0) { + console.warn('Link ' + link.name + ' is not unique.'); + } else { + // Check for a material + for( var j=0; j 0) { + var geom = geoms[0]; + var shape = null; + // Check for the shape + for (var i = 0; i < geom.childNodes.length; i++) { + var node = geom.childNodes[i]; + if (node.nodeType === 1) { + shape = node; + break; + } + } + // Check the type + var type = shape.nodeName; + if (type === 'sphere') { + this.geometry = new UrdfSphere({ + xml : shape + }); + } else if (type === 'box') { + this.geometry = new UrdfBox({ + xml : shape + }); + } else if (type === 'cylinder') { + this.geometry = new UrdfCylinder({ + xml : shape + }); + } else if (type === 'mesh') { + this.geometry = new UrdfMesh({ + xml : shape + }); + } else { + console.warn('Unknown geometry type ' + type); + } + } + + // Material + var materials = xml.getElementsByTagName('material'); + if (materials.length > 0) { + this.material = new UrdfMaterial({ + xml : materials[0] + }); + } +} + +module.exports = UrdfVisual; +},{"../math/Pose":19,"../math/Quaternion":20,"../math/Vector3":22,"./UrdfBox":27,"./UrdfCylinder":29,"./UrdfMaterial":32,"./UrdfMesh":33,"./UrdfSphere":35}],38:[function(require,module,exports){ +module.exports = require('object-assign')({ + UrdfBox: require('./UrdfBox'), + UrdfColor: require('./UrdfColor'), + UrdfCylinder: require('./UrdfCylinder'), + UrdfLink: require('./UrdfLink'), + UrdfMaterial: require('./UrdfMaterial'), + UrdfMesh: require('./UrdfMesh'), + UrdfModel: require('./UrdfModel'), + UrdfSphere: require('./UrdfSphere'), + UrdfVisual: require('./UrdfVisual') +}, require('./UrdfTypes')); + +},{"./UrdfBox":27,"./UrdfColor":28,"./UrdfCylinder":29,"./UrdfLink":31,"./UrdfMaterial":32,"./UrdfMesh":33,"./UrdfModel":34,"./UrdfSphere":35,"./UrdfTypes":36,"./UrdfVisual":37,"object-assign":2}],39:[function(require,module,exports){ +(function (global){ +module.exports = global.WebSocket; +}).call(this,typeof global !== "undefined" ? global : typeof self !== "undefined" ? self : typeof window !== "undefined" ? window : {}) +},{}],40:[function(require,module,exports){ +/* global document */ +module.exports = function Canvas() { + return document.createElement('canvas'); +}; +},{}],41:[function(require,module,exports){ +(function (global){ +/** + * @fileOverview + * @author Graeme Yeates - github.com/megawac + */ + +'use strict'; + +var Canvas = require('canvas'); +var Image = Canvas.Image || global.Image; + +/** + * If a message was compressed as a PNG image (a compression hack since + * gzipping over WebSockets * is not supported yet), this function places the + * "image" in a canvas element then decodes the * "image" as a Base64 string. + * + * @private + * @param data - object containing the PNG data. + * @param callback - function with params: + * * data - the uncompressed data + */ +function decompressPng(data, callback) { + // Uncompresses the data before sending it through (use image/canvas to do so). + var image = new Image(); + // When the image loads, extracts the raw data (JSON message). + image.onload = function() { + // Creates a local canvas to draw on. + var canvas = new Canvas(); + var context = canvas.getContext('2d'); + + // Sets width and height. + canvas.width = image.width; + canvas.height = image.height; + + // Prevents anti-aliasing and loosing data + context.imageSmoothingEnabled = false; + context.webkitImageSmoothingEnabled = false; + context.mozImageSmoothingEnabled = false; + + // Puts the data into the image. + context.drawImage(image, 0, 0); + // Grabs the raw, uncompressed data. + var imageData = context.getImageData(0, 0, image.width, image.height).data; + + // Constructs the JSON. + var jsonData = ''; + for (var i = 0; i < imageData.length; i += 4) { + // RGB + jsonData += String.fromCharCode(imageData[i], imageData[i + 1], imageData[i + 2]); + } + callback(JSON.parse(jsonData)); + }; + // Sends the image data to load. + image.src = 'data:image/png;base64,' + data; +} + +module.exports = decompressPng; +}).call(this,typeof global !== "undefined" ? global : typeof self !== "undefined" ? self : typeof window !== "undefined" ? window : {}) +},{"canvas":40}],42:[function(require,module,exports){ +(function (global){ +exports.DOMImplementation = global.DOMImplementation; +exports.XMLSerializer = global.XMLSerializer; +exports.DOMParser = global.DOMParser; +}).call(this,typeof global !== "undefined" ? global : typeof self !== "undefined" ? self : typeof window !== "undefined" ? window : {}) +},{}]},{},[4]); diff --git a/apps/android/app/src/main/assets/telemetry.js b/apps/android/app/src/main/assets/telemetry.js new file mode 100644 index 00000000..bae841d8 --- /dev/null +++ b/apps/android/app/src/main/assets/telemetry.js @@ -0,0 +1,115 @@ +var url = 'ws://192.168.11.1:9090'; +var modeEl = document.querySelector('.telemetry .mode'); +var batteryEl = document.querySelector('.battery'); +var notificationsEl = document.querySelector('.notifications'); + +var ros = new ROSLIB.Ros({ url: url }); + +ros.on('connection', function () { + body.classList.add('connected'); +}); + +ros.on('close', function () { + body.classList.remove('connected'); + modeEl.classList.remove('armed'); + modeEl.innerHTML = 'DISCONNECTED'; + batteryEl.innerHTML = ''; + setTimeout(function() { + modeEl.innerHTML = 'RECONNECTING'; + ros.connect(url); + }, 2000); +}); + +var fcuState; + +new ROSLIB.Topic({ + ros: ros, + name: '/state_latched', + messageType: 'mavros_msgs/State' +}).subscribe(function(message) { + body.classList.toggle('fcu-disconnected', !message.connected); + body.classList.toggle('armed', message.armed); + fcuState = message; + modeEl.classList.toggle('armed', fcuState.armed); + modeEl.innerHTML = message.connected ? fcuState.mode : 'DISCONNECTED FROM FCU'; + console.log('state', message); +}); + +function notifyLowBattery() { + console.log('low battery'); + callNativeApp('lowBattery'); + body.classList.remove('low-battery'); + void body.offsetWidth; // trick for repeating animation + body.classList.add('low-battery'); +} + +notifyLowBatteryThrottled = throttle(notifyLowBattery, 15000); + +new ROSLIB.Topic({ + ros: ros, + name: '/mavros/battery', + messageType: 'sensor_msgs/BatteryState', + throttle_rate: 5000 +}).subscribe(function(message) { + var LOW_BATTERY = 3.8; + batteryEl.innerHTML = (message.cell_voltage[0].toFixed(2) + ' V') || ''; + + if (message.cell_voltage[0] < LOW_BATTERY) { + notifyLowBatteryThrottled(); + } else { + body.classList.remove('low-battery'); + } +}); + +var notificationHideTimer; + +function notify(text, severity) { + var item = document.createElement('div'); + item.innerHTML = text; + item.classList.add('item'); + notificationsEl.prepend(item); + var itemHeight = item.offsetHeight; + notificationsEl.classList.remove('anim'); + notificationsEl.style.transform = 'translateY(' + -itemHeight + 'px)'; + setTimeout(function() { + notificationsEl.classList.add('anim'); + notificationsEl.style.transform = 'translateY(0)'; + }, 0); + clearTimeout(notificationHideTimer); + notificationHideTimer = setTimeout(function() { + notificationsEl.style.transform = ''; + notificationsEl.classList.add('hidden'); + setTimeout(function() { + notificationsEl.innerHTML = ''; + }, 210); + }, 4000); +} + +new ROSLIB.Topic({ + ros: ros, + name: '/mavros/statustext/recv', + messageType: 'mavros_msgs/StatusText' +}).subscribe(function(message) { + var BLACKLIST = ['CMD: ', 'PR: ', 'DROPPED', 'Clock skew detected', 'MANUAL CONTROL LOST']; + if (message.severity <= 4) { + if (BLACKLIST.some(function(e) { + return message.text.indexOf(e) != -1; + })) { + console.log('Filtered out message ' + message.text); + return; + } + notify(message.text, message.severity); + callNativeApp('notification', message); + } +}); + +var setMode = new ROSLIB.Service({ + ros: ros, + name : '/mavros/set_mode', + serviceType : 'mavros_msgs/SetMode' +}); + +function setControlMode() { + var CONTROL_MODE = 'STABILIZED'; + setMode.callService(new ROSLIB.ServiceRequest({ custom_mode: CONTROL_MODE })); +} diff --git a/apps/android/app/src/main/ic_launcher-web.png b/apps/android/app/src/main/ic_launcher-web.png new file mode 100644 index 00000000..9d7884b5 Binary files /dev/null and b/apps/android/app/src/main/ic_launcher-web.png differ diff --git a/apps/android/app/src/main/java/express/copter/cleverrc/MainActivity.kt b/apps/android/app/src/main/java/express/copter/cleverrc/MainActivity.kt new file mode 100644 index 00000000..b7ab0d4e --- /dev/null +++ b/apps/android/app/src/main/java/express/copter/cleverrc/MainActivity.kt @@ -0,0 +1,86 @@ +package express.copter.cleverrc + +import android.content.Context +import android.os.Build +import android.support.v7.app.AppCompatActivity +import android.os.Bundle +import android.view.View +import android.view.WindowManager +import android.webkit.JavascriptInterface +import kotlinx.android.synthetic.main.activity_main.* +import org.json.JSONObject +import java.net.DatagramPacket +import java.net.DatagramSocket +import java.net.InetAddress +import java.nio.ByteBuffer + +fun pack(x: Short, y: Short, z: Short, r: Short): ByteArray { + val pump_on_buf: ByteBuffer = ByteBuffer.allocate(8) + pump_on_buf.putShort(r) + pump_on_buf.putShort(z) + pump_on_buf.putShort(y) + pump_on_buf.putShort(x) + + return pump_on_buf.array().reversedArray() +} + +fun send(host: String, port: Int, data: ByteArray, senderPort: Int = 0): Boolean { + var ret = false + var socket: DatagramSocket? = null + try { + socket = DatagramSocket(senderPort) + val address = InetAddress.getByName(host) + val packet = DatagramPacket(data, data.size, address, port) + socket.send(packet) + ret = true + } catch (e: Exception) { + e.printStackTrace() + } finally { + socket?.close() + } + return ret +} + +class MainActivity : AppCompatActivity() { + + override fun onCreate(savedInstanceState: Bundle?) { + super.onCreate(savedInstanceState) + setContentView(R.layout.activity_main) + fullScreenCall() + main_web.loadUrl("file:///android_asset/index.html") + + main_web.settings.apply { + domStorageEnabled = true + javaScriptEnabled = true + loadWithOverviewMode = true + useWideViewPort = true + setSupportZoom(false) + } + + main_web.addJavascriptInterface(WebAppInterface(this), "appInterface") + } + + private fun fullScreenCall() { + window.setFlags( + WindowManager.LayoutParams.FLAG_FULLSCREEN, + WindowManager.LayoutParams.FLAG_FULLSCREEN + ) + if (Build.VERSION.SDK_INT < 19) { + val v = this.window.decorView + v.systemUiVisibility = View.GONE + } else { + //for higher api versions. + val decorView = window.decorView + val uiOptions = View.SYSTEM_UI_FLAG_HIDE_NAVIGATION or View.SYSTEM_UI_FLAG_IMMERSIVE_STICKY + decorView.systemUiVisibility = uiOptions + } + } +} + +class WebAppInterface(c: Context) { + @JavascriptInterface + public fun postMessage(message: String) { + val data = JSONObject(message) + send("255.255.255.255", 35602, pack(data.getInt("x").toShort(), data.getInt("y").toShort(), data.getInt("z").toShort(), data.getInt("r").toShort())) + } +} \ No newline at end of file diff --git a/apps/android/app/src/main/res/drawable-v24/ic_launcher_foreground.xml b/apps/android/app/src/main/res/drawable-v24/ic_launcher_foreground.xml new file mode 100644 index 00000000..6348baae --- /dev/null +++ b/apps/android/app/src/main/res/drawable-v24/ic_launcher_foreground.xml @@ -0,0 +1,34 @@ + + + + + + + + + + + diff --git a/apps/android/app/src/main/res/drawable/ic_launcher_background.xml b/apps/android/app/src/main/res/drawable/ic_launcher_background.xml new file mode 100644 index 00000000..a0ad202f --- /dev/null +++ b/apps/android/app/src/main/res/drawable/ic_launcher_background.xml @@ -0,0 +1,74 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/apps/android/app/src/main/res/layout/activity_main.xml b/apps/android/app/src/main/res/layout/activity_main.xml new file mode 100644 index 00000000..1721a730 --- /dev/null +++ b/apps/android/app/src/main/res/layout/activity_main.xml @@ -0,0 +1,14 @@ + + + + + \ No newline at end of file diff --git a/apps/android/app/src/main/res/mipmap-anydpi-v26/ic_launcher.xml b/apps/android/app/src/main/res/mipmap-anydpi-v26/ic_launcher.xml new file mode 100644 index 00000000..036d09bc --- /dev/null +++ b/apps/android/app/src/main/res/mipmap-anydpi-v26/ic_launcher.xml @@ -0,0 +1,5 @@ + + + + + \ No newline at end of file diff --git a/apps/android/app/src/main/res/mipmap-anydpi-v26/ic_launcher_round.xml b/apps/android/app/src/main/res/mipmap-anydpi-v26/ic_launcher_round.xml new file mode 100644 index 00000000..036d09bc --- /dev/null +++ b/apps/android/app/src/main/res/mipmap-anydpi-v26/ic_launcher_round.xml @@ -0,0 +1,5 @@ + + + + + \ No newline at end of file diff --git a/apps/android/app/src/main/res/mipmap-hdpi/ic_launcher.png b/apps/android/app/src/main/res/mipmap-hdpi/ic_launcher.png new file mode 100644 index 00000000..e062b5c8 Binary files /dev/null and b/apps/android/app/src/main/res/mipmap-hdpi/ic_launcher.png differ diff --git a/apps/android/app/src/main/res/mipmap-hdpi/ic_launcher_foreground.png b/apps/android/app/src/main/res/mipmap-hdpi/ic_launcher_foreground.png new file mode 100644 index 00000000..a159a5ca Binary files /dev/null and b/apps/android/app/src/main/res/mipmap-hdpi/ic_launcher_foreground.png differ diff --git a/apps/android/app/src/main/res/mipmap-hdpi/ic_launcher_round.png b/apps/android/app/src/main/res/mipmap-hdpi/ic_launcher_round.png new file mode 100644 index 00000000..84b2328d Binary files /dev/null and b/apps/android/app/src/main/res/mipmap-hdpi/ic_launcher_round.png differ diff --git a/apps/android/app/src/main/res/mipmap-mdpi/ic_launcher.png b/apps/android/app/src/main/res/mipmap-mdpi/ic_launcher.png new file mode 100644 index 00000000..03e81a15 Binary files /dev/null and b/apps/android/app/src/main/res/mipmap-mdpi/ic_launcher.png differ diff --git a/apps/android/app/src/main/res/mipmap-mdpi/ic_launcher_foreground.png b/apps/android/app/src/main/res/mipmap-mdpi/ic_launcher_foreground.png new file mode 100644 index 00000000..30c30068 Binary files /dev/null and b/apps/android/app/src/main/res/mipmap-mdpi/ic_launcher_foreground.png differ diff --git a/apps/android/app/src/main/res/mipmap-mdpi/ic_launcher_round.png b/apps/android/app/src/main/res/mipmap-mdpi/ic_launcher_round.png new file mode 100644 index 00000000..51b2703f Binary files /dev/null and b/apps/android/app/src/main/res/mipmap-mdpi/ic_launcher_round.png differ diff --git a/apps/android/app/src/main/res/mipmap-xhdpi/ic_launcher.png b/apps/android/app/src/main/res/mipmap-xhdpi/ic_launcher.png new file mode 100644 index 00000000..6bb3a54e Binary files /dev/null and b/apps/android/app/src/main/res/mipmap-xhdpi/ic_launcher.png differ diff --git a/apps/android/app/src/main/res/mipmap-xhdpi/ic_launcher_foreground.png b/apps/android/app/src/main/res/mipmap-xhdpi/ic_launcher_foreground.png new file mode 100644 index 00000000..9e486b66 Binary files /dev/null and b/apps/android/app/src/main/res/mipmap-xhdpi/ic_launcher_foreground.png differ diff --git a/apps/android/app/src/main/res/mipmap-xhdpi/ic_launcher_round.png b/apps/android/app/src/main/res/mipmap-xhdpi/ic_launcher_round.png new file mode 100644 index 00000000..c35cbc39 Binary files /dev/null and b/apps/android/app/src/main/res/mipmap-xhdpi/ic_launcher_round.png differ diff --git a/apps/android/app/src/main/res/mipmap-xxhdpi/ic_launcher.png b/apps/android/app/src/main/res/mipmap-xxhdpi/ic_launcher.png new file mode 100644 index 00000000..ee8f1eee Binary files /dev/null and b/apps/android/app/src/main/res/mipmap-xxhdpi/ic_launcher.png differ diff --git a/apps/android/app/src/main/res/mipmap-xxhdpi/ic_launcher_foreground.png b/apps/android/app/src/main/res/mipmap-xxhdpi/ic_launcher_foreground.png new file mode 100644 index 00000000..126b4641 Binary files /dev/null and b/apps/android/app/src/main/res/mipmap-xxhdpi/ic_launcher_foreground.png differ diff --git a/apps/android/app/src/main/res/mipmap-xxhdpi/ic_launcher_round.png b/apps/android/app/src/main/res/mipmap-xxhdpi/ic_launcher_round.png new file mode 100644 index 00000000..7be26a92 Binary files /dev/null and b/apps/android/app/src/main/res/mipmap-xxhdpi/ic_launcher_round.png differ diff --git a/apps/android/app/src/main/res/mipmap-xxxhdpi/ic_launcher.png b/apps/android/app/src/main/res/mipmap-xxxhdpi/ic_launcher.png new file mode 100644 index 00000000..3db62ede Binary files /dev/null and b/apps/android/app/src/main/res/mipmap-xxxhdpi/ic_launcher.png differ diff --git a/apps/android/app/src/main/res/mipmap-xxxhdpi/ic_launcher_foreground.png b/apps/android/app/src/main/res/mipmap-xxxhdpi/ic_launcher_foreground.png new file mode 100644 index 00000000..d2a36044 Binary files /dev/null and b/apps/android/app/src/main/res/mipmap-xxxhdpi/ic_launcher_foreground.png differ diff --git a/apps/android/app/src/main/res/mipmap-xxxhdpi/ic_launcher_round.png b/apps/android/app/src/main/res/mipmap-xxxhdpi/ic_launcher_round.png new file mode 100644 index 00000000..6a722e91 Binary files /dev/null and b/apps/android/app/src/main/res/mipmap-xxxhdpi/ic_launcher_round.png differ diff --git a/apps/android/app/src/main/res/values/colors.xml b/apps/android/app/src/main/res/values/colors.xml new file mode 100644 index 00000000..1fea4407 --- /dev/null +++ b/apps/android/app/src/main/res/values/colors.xml @@ -0,0 +1,6 @@ + + + #fafafa + #d1d1d1 + #757575 + diff --git a/apps/android/app/src/main/res/values/ic_launcher_background.xml b/apps/android/app/src/main/res/values/ic_launcher_background.xml new file mode 100644 index 00000000..c5d5899f --- /dev/null +++ b/apps/android/app/src/main/res/values/ic_launcher_background.xml @@ -0,0 +1,4 @@ + + + #FFFFFF + \ No newline at end of file diff --git a/apps/android/app/src/main/res/values/strings.xml b/apps/android/app/src/main/res/values/strings.xml new file mode 100644 index 00000000..ed4edd0e --- /dev/null +++ b/apps/android/app/src/main/res/values/strings.xml @@ -0,0 +1,3 @@ + + CLEVER RC + diff --git a/apps/android/app/src/main/res/values/styles.xml b/apps/android/app/src/main/res/values/styles.xml new file mode 100644 index 00000000..33dce5b5 --- /dev/null +++ b/apps/android/app/src/main/res/values/styles.xml @@ -0,0 +1,18 @@ + + + + + + + + diff --git a/apps/android/app/src/test/java/express/copter/cleverrc/ExampleUnitTest.kt b/apps/android/app/src/test/java/express/copter/cleverrc/ExampleUnitTest.kt new file mode 100644 index 00000000..7a4907af --- /dev/null +++ b/apps/android/app/src/test/java/express/copter/cleverrc/ExampleUnitTest.kt @@ -0,0 +1,17 @@ +package express.copter.cleverrc + +import org.junit.Test + +import org.junit.Assert.* + +/** + * Example local unit test, which will execute on the development machine (host). + * + * See [testing documentation](http://d.android.com/tools/testing). + */ +class ExampleUnitTest { + @Test + fun addition_isCorrect() { + assertEquals(4, 2 + 2) + } +} diff --git a/apps/android/build.gradle b/apps/android/build.gradle new file mode 100644 index 00000000..a3176b9e --- /dev/null +++ b/apps/android/build.gradle @@ -0,0 +1,27 @@ +// Top-level build file where you can add configuration options common to all sub-projects/modules. + +buildscript { + ext.kotlin_version = '1.2.71' + repositories { + google() + jcenter() + } + dependencies { + classpath 'com.android.tools.build:gradle:3.2.1' + classpath "org.jetbrains.kotlin:kotlin-gradle-plugin:$kotlin_version" + + // NOTE: Do not place your application dependencies here; they belong + // in the individual module build.gradle files + } +} + +allprojects { + repositories { + google() + jcenter() + } +} + +task clean(type: Delete) { + delete rootProject.buildDir +} diff --git a/apps/android/gradle.properties b/apps/android/gradle.properties new file mode 100644 index 00000000..85be9ead --- /dev/null +++ b/apps/android/gradle.properties @@ -0,0 +1,15 @@ +# Project-wide Gradle settings. +# IDE (e.g. Android Studio) users: +# Gradle settings configured through the IDE *will override* +# any settings specified in this file. +# For more details on how to configure your build environment visit +# http://www.gradle.org/docs/current/userguide/build_environment.html +# Specifies the JVM arguments used for the daemon process. +# The setting is particularly useful for tweaking memory settings. +org.gradle.jvmargs=-Xmx1536m +# When configured, Gradle will run in incubating parallel mode. +# This option should only be used with decoupled projects. More details, visit +# http://www.gradle.org/docs/current/userguide/multi_project_builds.html#sec:decoupled_projects +# org.gradle.parallel=true +# Kotlin code style for this project: "official" or "obsolete": +kotlin.code.style=official diff --git a/apps/android/gradle/wrapper/gradle-wrapper.jar b/apps/android/gradle/wrapper/gradle-wrapper.jar new file mode 100644 index 00000000..f6b961fd Binary files /dev/null and b/apps/android/gradle/wrapper/gradle-wrapper.jar differ diff --git a/apps/android/gradle/wrapper/gradle-wrapper.properties b/apps/android/gradle/wrapper/gradle-wrapper.properties new file mode 100644 index 00000000..9a4163a4 --- /dev/null +++ b/apps/android/gradle/wrapper/gradle-wrapper.properties @@ -0,0 +1,5 @@ +distributionBase=GRADLE_USER_HOME +distributionPath=wrapper/dists +distributionUrl=https\://services.gradle.org/distributions/gradle-4.6-all.zip +zipStoreBase=GRADLE_USER_HOME +zipStorePath=wrapper/dists diff --git a/apps/android/gradlew b/apps/android/gradlew new file mode 100644 index 00000000..cccdd3d5 --- /dev/null +++ b/apps/android/gradlew @@ -0,0 +1,172 @@ +#!/usr/bin/env sh + +############################################################################## +## +## Gradle start up script for UN*X +## +############################################################################## + +# Attempt to set APP_HOME +# Resolve links: $0 may be a link +PRG="$0" +# Need this for relative symlinks. +while [ -h "$PRG" ] ; do + ls=`ls -ld "$PRG"` + link=`expr "$ls" : '.*-> \(.*\)$'` + if expr "$link" : '/.*' > /dev/null; then + PRG="$link" + else + PRG=`dirname "$PRG"`"/$link" + fi +done +SAVED="`pwd`" +cd "`dirname \"$PRG\"`/" >/dev/null +APP_HOME="`pwd -P`" +cd "$SAVED" >/dev/null + +APP_NAME="Gradle" +APP_BASE_NAME=`basename "$0"` + +# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. +DEFAULT_JVM_OPTS="" + +# Use the maximum available, or set MAX_FD != -1 to use that value. +MAX_FD="maximum" + +warn () { + echo "$*" +} + +die () { + echo + echo "$*" + echo + exit 1 +} + +# OS specific support (must be 'true' or 'false'). +cygwin=false +msys=false +darwin=false +nonstop=false +case "`uname`" in + CYGWIN* ) + cygwin=true + ;; + Darwin* ) + darwin=true + ;; + MINGW* ) + msys=true + ;; + NONSTOP* ) + nonstop=true + ;; +esac + +CLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar + +# Determine the Java command to use to start the JVM. +if [ -n "$JAVA_HOME" ] ; then + if [ -x "$JAVA_HOME/jre/sh/java" ] ; then + # IBM's JDK on AIX uses strange locations for the executables + JAVACMD="$JAVA_HOME/jre/sh/java" + else + JAVACMD="$JAVA_HOME/bin/java" + fi + if [ ! -x "$JAVACMD" ] ; then + die "ERROR: JAVA_HOME is set to an invalid directory: $JAVA_HOME + +Please set the JAVA_HOME variable in your environment to match the +location of your Java installation." + fi +else + JAVACMD="java" + which java >/dev/null 2>&1 || die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. + +Please set the JAVA_HOME variable in your environment to match the +location of your Java installation." +fi + +# Increase the maximum file descriptors if we can. +if [ "$cygwin" = "false" -a "$darwin" = "false" -a "$nonstop" = "false" ] ; then + MAX_FD_LIMIT=`ulimit -H -n` + if [ $? -eq 0 ] ; then + if [ "$MAX_FD" = "maximum" -o "$MAX_FD" = "max" ] ; then + MAX_FD="$MAX_FD_LIMIT" + fi + ulimit -n $MAX_FD + if [ $? -ne 0 ] ; then + warn "Could not set maximum file descriptor limit: $MAX_FD" + fi + else + warn "Could not query maximum file descriptor limit: $MAX_FD_LIMIT" + fi +fi + +# For Darwin, add options to specify how the application appears in the dock +if $darwin; then + GRADLE_OPTS="$GRADLE_OPTS \"-Xdock:name=$APP_NAME\" \"-Xdock:icon=$APP_HOME/media/gradle.icns\"" +fi + +# For Cygwin, switch paths to Windows format before running java +if $cygwin ; then + APP_HOME=`cygpath --path --mixed "$APP_HOME"` + CLASSPATH=`cygpath --path --mixed "$CLASSPATH"` + JAVACMD=`cygpath --unix "$JAVACMD"` + + # We build the pattern for arguments to be converted via cygpath + ROOTDIRSRAW=`find -L / -maxdepth 1 -mindepth 1 -type d 2>/dev/null` + SEP="" + for dir in $ROOTDIRSRAW ; do + ROOTDIRS="$ROOTDIRS$SEP$dir" + SEP="|" + done + OURCYGPATTERN="(^($ROOTDIRS))" + # Add a user-defined pattern to the cygpath arguments + if [ "$GRADLE_CYGPATTERN" != "" ] ; then + OURCYGPATTERN="$OURCYGPATTERN|($GRADLE_CYGPATTERN)" + fi + # Now convert the arguments - kludge to limit ourselves to /bin/sh + i=0 + for arg in "$@" ; do + CHECK=`echo "$arg"|egrep -c "$OURCYGPATTERN" -` + CHECK2=`echo "$arg"|egrep -c "^-"` ### Determine if an option + + if [ $CHECK -ne 0 ] && [ $CHECK2 -eq 0 ] ; then ### Added a condition + eval `echo args$i`=`cygpath --path --ignore --mixed "$arg"` + else + eval `echo args$i`="\"$arg\"" + fi + i=$((i+1)) + done + case $i in + (0) set -- ;; + (1) set -- "$args0" ;; + (2) set -- "$args0" "$args1" ;; + (3) set -- "$args0" "$args1" "$args2" ;; + (4) set -- "$args0" "$args1" "$args2" "$args3" ;; + (5) set -- "$args0" "$args1" "$args2" "$args3" "$args4" ;; + (6) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" ;; + (7) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" ;; + (8) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" "$args7" ;; + (9) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" "$args7" "$args8" ;; + esac +fi + +# Escape application args +save () { + for i do printf %s\\n "$i" | sed "s/'/'\\\\''/g;1s/^/'/;\$s/\$/' \\\\/" ; done + echo " " +} +APP_ARGS=$(save "$@") + +# Collect all arguments for the java command, following the shell quoting and substitution rules +eval set -- $DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS "\"-Dorg.gradle.appname=$APP_BASE_NAME\"" -classpath "\"$CLASSPATH\"" org.gradle.wrapper.GradleWrapperMain "$APP_ARGS" + +# by default we should be in the correct project dir, but when run from Finder on Mac, the cwd is wrong +if [ "$(uname)" = "Darwin" ] && [ "$HOME" = "$PWD" ]; then + cd "$(dirname "$0")" +fi + +exec "$JAVACMD" "$@" diff --git a/apps/android/gradlew.bat b/apps/android/gradlew.bat new file mode 100644 index 00000000..f9553162 --- /dev/null +++ b/apps/android/gradlew.bat @@ -0,0 +1,84 @@ +@if "%DEBUG%" == "" @echo off +@rem ########################################################################## +@rem +@rem Gradle startup script for Windows +@rem +@rem ########################################################################## + +@rem Set local scope for the variables with windows NT shell +if "%OS%"=="Windows_NT" setlocal + +set DIRNAME=%~dp0 +if "%DIRNAME%" == "" set DIRNAME=. +set APP_BASE_NAME=%~n0 +set APP_HOME=%DIRNAME% + +@rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. +set DEFAULT_JVM_OPTS= + +@rem Find java.exe +if defined JAVA_HOME goto findJavaFromJavaHome + +set JAVA_EXE=java.exe +%JAVA_EXE% -version >NUL 2>&1 +if "%ERRORLEVEL%" == "0" goto init + +echo. +echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. +echo. +echo Please set the JAVA_HOME variable in your environment to match the +echo location of your Java installation. + +goto fail + +:findJavaFromJavaHome +set JAVA_HOME=%JAVA_HOME:"=% +set JAVA_EXE=%JAVA_HOME%/bin/java.exe + +if exist "%JAVA_EXE%" goto init + +echo. +echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% +echo. +echo Please set the JAVA_HOME variable in your environment to match the +echo location of your Java installation. + +goto fail + +:init +@rem Get command-line arguments, handling Windows variants + +if not "%OS%" == "Windows_NT" goto win9xME_args + +:win9xME_args +@rem Slurp the command line arguments. +set CMD_LINE_ARGS= +set _SKIP=2 + +:win9xME_args_slurp +if "x%~1" == "x" goto execute + +set CMD_LINE_ARGS=%* + +:execute +@rem Setup the command line + +set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar + +@rem Execute Gradle +"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %CMD_LINE_ARGS% + +:end +@rem End local scope for the variables with windows NT shell +if "%ERRORLEVEL%"=="0" goto mainEnd + +:fail +rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of +rem the _cmd.exe /c_ return code! +if not "" == "%GRADLE_EXIT_CONSOLE%" exit 1 +exit /b 1 + +:mainEnd +if "%OS%"=="Windows_NT" endlocal + +:omega diff --git a/apps/android/settings.gradle b/apps/android/settings.gradle new file mode 100644 index 00000000..e7b4def4 --- /dev/null +++ b/apps/android/settings.gradle @@ -0,0 +1 @@ +include ':app' diff --git a/aruco_pose/CMakeLists.txt b/aruco_pose/CMakeLists.txt index 90403cf5..7ac1ece6 100644 --- a/aruco_pose/CMakeLists.txt +++ b/aruco_pose/CMakeLists.txt @@ -21,6 +21,8 @@ find_package(catkin REQUIRED COMPONENTS #aruco_msgs ) +find_package(OpenCV 3 REQUIRED) + ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) @@ -158,7 +160,7 @@ link_directories(/opt/ros/kinetic/lib) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} - "/opt/ros/kinetic/lib/libopencv_aruco3.so" # TODO: fix launch fails with .so loading + ${OpenCV_LIBRARIES} ) ############# diff --git a/book.json b/book.json index ea6999d0..ba357259 100644 --- a/book.json +++ b/book.json @@ -17,7 +17,7 @@ "id": 49359238 }, "bulk-redirect": { - "basepath": "/", + "basepath": "", "redirectsFile": "redirects.json" } } diff --git a/builder/assets/clever.service b/builder/assets/clever.service index 1aeb0604..ad3287df 100644 --- a/builder/assets/clever.service +++ b/builder/assets/clever.service @@ -4,6 +4,7 @@ Requires=roscore.service After=roscore.service [Service] +User=pi EnvironmentFile=/lib/systemd/system/roscore.env ExecStart=/opt/ros/kinetic/bin/roslaunch clever clever.launch --wait Restart=on-abort diff --git a/builder/assets/kinetic-rosdep-clever.yaml b/builder/assets/kinetic-rosdep-clever.yaml index 80ad31f5..25db72f8 100644 --- a/builder/assets/kinetic-rosdep-clever.yaml +++ b/builder/assets/kinetic-rosdep-clever.yaml @@ -526,3 +526,15 @@ theora_image_transport: libogg: debian: stretch: [libtheora0=1.1.1+dfsg.1-14] +vl53l1x: + debian: + stretch: [ros-kinetic-vl53l1x] +interactive_markers: + debian: + stretch: [ros-kinetic-interactive-markers] +interactive_marker_proxy: + debian: + stretch: [ros-kinetic-interactive-marker-proxy] +tf2_web_republisher: + debian: + stretch: [ros-kinetic-tf2-web-republisher] \ No newline at end of file diff --git a/builder/assets/monkey-clever b/builder/assets/monkey similarity index 90% rename from builder/assets/monkey-clever rename to builder/assets/monkey index 112d990f..81e5c916 100644 --- a/builder/assets/monkey-clever +++ b/builder/assets/monkey @@ -20,7 +20,7 @@ # Example: # DocumentRoot /home/krypton/htdocs - DocumentRoot /usr/share/monkey-static + DocumentRoot /home/pi/catkin_ws/src/clever/clever/www # Redirect: # --------- @@ -36,13 +36,13 @@ # ---------- # Registration file of correct request. - AccessLog /var/log/monkey-clever/access.log + AccessLog /var/log/monkey/access.log # ErrorLog: # --------- # Registration file of incorrect request. - ErrorLog /var/log/monkey-clever/error.log + ErrorLog /var/log/monkey/error.log [ERROR_PAGES] 404 404.html diff --git a/builder/assets/roscore.service b/builder/assets/roscore.service index 5e3a5a7e..85c5fe59 100644 --- a/builder/assets/roscore.service +++ b/builder/assets/roscore.service @@ -3,6 +3,7 @@ Description=Launcher for the ROS master, parameter server and rosout logging nod After=network.target [Service] +User=pi EnvironmentFile=/lib/systemd/system/roscore.env ExecStart=/opt/ros/kinetic/bin/roscore Restart=on-abort diff --git a/builder/image-build.sh b/builder/image-build.sh index da6a994d..97eb6b45 100755 --- a/builder/image-build.sh +++ b/builder/image-build.sh @@ -53,7 +53,7 @@ IMAGE_NAME="${REPO_NAME}_${IMAGE_VERSION}.img" IMAGE_PATH="${IMAGES_DIR}/${IMAGE_NAME}" get_image() { - # TEMPLATE: get_image + # TEMPLATE: get_image local BUILD_DIR=$(dirname $1) local RPI_ZIP_NAME=$(basename $2) local RPI_IMAGE_NAME=$(echo ${RPI_ZIP_NAME} | sed 's/zip/img/') @@ -81,17 +81,7 @@ ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/hardwar ${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/' - -# Gitbook -apt-get install -y curl gnupg -curl -sL https://deb.nodesource.com/setup_11.x | bash - -apt-get install -y nodejs -npm install gitbook-cli -g -gitbook build ${REPO_DIR}'/docs' -${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${REPO_DIR}'/docs/_book/' '/usr/share/monkey-static/docs/' - +${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/monkey' '/root/' # Butterfly ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/butterfly.service' '/lib/systemd/system/' @@ -110,6 +100,6 @@ ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/roscore ${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-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} diff --git a/builder/image-ros.sh b/builder/image-ros.sh index 81f74956..27046a12 100755 --- a/builder/image-ros.sh +++ b/builder/image-ros.sh @@ -66,6 +66,9 @@ echo_stamp "Init rosdep" \ && echo "yaml file:///etc/ros/rosdep/kinetic-rosdep-clever.yaml" >> /etc/ros/rosdep/sources.list.d/20-default.list \ && rosdep update +echo_stamp "Populate rosdep for ROS user" +sudo -u pi rosdep update + resolve_rosdep() { # TEMPLATE: resolve_rosdep CATKIN_PATH=$1 @@ -92,7 +95,7 @@ if [ "${INSTALL_ROS_PACK_SOURCES}" = "true" ]; then echo_stamp "Preparing other ROS-packages to kinetic-custom_ros.rosinstall" \ && cd /home/pi/ros_catkin_ws \ && rosinstall_generator \ - actionlib actionlib_msgs angles async_web_server_cpp bond bond_core bondcpp bondpy camera_calibration_parsers camera_info_manager catkin class_loader cmake_modules cpp_common cv_bridge cv_camera diagnostic_msgs diagnostic_updater dynamic_reconfigure eigen_conversions gencpp geneus genlisp genmsg gennodejs genpy geographic_msgs geometry_msgs geometry2 image_transport compressed_image_transport libmavconn mavlink mavros_msgs message_filters message_generation message_runtime mk nav_msgs nodelet orocos_kdl pluginlib python_orocos_kdl ros ros_comm rosapi rosauth rosbag rosbag_migration_rule rosbag_storage rosbash rosboost_cfg rosbridge_library rosbridge_server rosbridge_suite rosbuild rosclean rosconsole rosconsole_bridge roscpp roscpp_serialization roscpp_traits roscreate rosgraph rosgraph_msgs roslang roslaunch roslib roslint roslisp roslz4 rosmake rosmaster rosmsg rosnode rosout rospack rosparam rospy rospy_tutorials rosserial rosserial_client rosserial_msgs rosserial_python rosservice rostest rostime rostopic rosunit roswtf sensor_msgs smclib std_msgs std_srvs stereo_msgs tf tf2 tf2_bullet tf2_eigen tf2_geometry_msgs tf2_kdl tf2_msgs tf2_py tf2_ros tf2_sensor_msgs tf2_tools topic_tools trajectory_msgs urdf urdf_parser_plugin usb_cam uuid_msgs visualization_msgs web_video_server xmlrpcpp mavros opencv3 mavros_extras interactive_markers tf2_web_republisher interactive_marker_proxy \ + actionlib actionlib_msgs angles async_web_server_cpp bond bond_core bondcpp bondpy camera_calibration_parsers camera_info_manager catkin class_loader cmake_modules cpp_common cv_bridge cv_camera diagnostic_msgs diagnostic_updater dynamic_reconfigure eigen_conversions gencpp geneus genlisp genmsg gennodejs genpy geographic_msgs geometry_msgs geometry2 image_transport compressed_image_transport libmavconn mavlink mavros_msgs message_filters message_generation message_runtime mk nav_msgs nodelet orocos_kdl pluginlib python_orocos_kdl ros ros_comm rosapi rosauth rosbag rosbag_migration_rule rosbag_storage rosbash rosboost_cfg rosbridge_library rosbridge_server rosbridge_suite rosbuild rosclean rosconsole rosconsole_bridge roscpp roscpp_serialization roscpp_traits roscreate rosgraph rosgraph_msgs roslang roslaunch roslib roslint roslisp roslz4 rosmake rosmaster rosmsg rosnode rosout rospack rosparam rospy rospy_tutorials rosserial rosserial_client rosserial_msgs rosserial_python rosservice rostest rostime rostopic rosunit roswtf sensor_msgs smclib std_msgs std_srvs stereo_msgs tf tf2 tf2_bullet tf2_eigen tf2_geometry_msgs tf2_kdl tf2_msgs tf2_py tf2_ros tf2_sensor_msgs tf2_tools topic_tools trajectory_msgs urdf urdf_parser_plugin usb_cam uuid_msgs visualization_msgs web_video_server xmlrpcpp mavros opencv3 mavros_extras interactive_markers tf2_web_republisher interactive_marker_proxy vl53l1x \ --rosdistro kinetic --deps --wet-only --tar > kinetic-custom_ros.rosinstall \ && wstool merge -j${NUMBER_THREADS} -t src kinetic-custom_ros.rosinstall \ && wstool update -j${NUMBER_THREADS} -t src \ @@ -148,6 +151,23 @@ echo_stamp "Installing CLEVER" \ && echo_stamp "All CLEVER was installed!" "SUCCESS" \ || (echo_stamp "CLEVER installation was failed!" "ERROR"; exit 1) +echo_stamp "Build CLEVER documentation" +cd /home/pi/catkin_ws/src/clever +NPM_CONFIG_UNSAFE_PERM=true npm install gitbook-cli -g +NPM_CONFIG_UNSAFE_PERM=true gitbook install +gitbook build + +echo_stamp "Installing additional ROS packages" +apt-get install -y --no-install-recommends \ + ros-kinetic-dynamic-reconfigure \ + ros-kinetic-tf2-web-republisher \ + ros-kinetic-compressed-image-transport \ + ros-kinetic-rosbridge-suite \ + ros-kinetic-rosserial \ + ros-kinetic-usb-cam \ + ros-kinetic-vl53l1x \ + ros-kinetic-opencv3=3.3.1neon-0stretch + # 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 diff --git a/builder/image-software.sh b/builder/image-software.sh index ab6a5ec9..4813c2ec 100755 --- a/builder/image-software.sh +++ b/builder/image-software.sh @@ -92,7 +92,7 @@ libjpeg8-dev=8d1-2 \ tcpdump \ ltrace \ libpoco-dev=1.7.6+dfsg1-5+deb9u1 \ -python-rosdep=0.13.0-1 \ +python-rosdep=0.15.0-1 \ python-rosinstall-generator=0.1.14-1 \ python-wstool=0.1.17-1 \ python-rosinstall=0.7.8-1 \ @@ -100,6 +100,8 @@ build-essential=12.3 \ libffi-dev \ monkey=1.6.9-1 \ pigpio python-pigpio python3-pigpio \ +i2c-tools \ +ntpdate \ && echo_stamp "Everything was installed!" "SUCCESS" \ || (echo_stamp "Some packages wasn't installed!" "ERROR"; exit 1) @@ -127,9 +129,16 @@ my_travis_retry pip install rpi_ws281x echo_stamp "Setup Monkey" mv /etc/monkey/sites/default /etc/monkey/sites/default.orig -mv /root/monkey-clever /etc/monkey/sites/default +mv /root/monkey /etc/monkey/sites/default systemctl enable monkey.service +echo_stamp "Install Node.js" +cd /home/pi +wget https://nodejs.org/dist/v10.15.0/node-v10.15.0-linux-armv6l.tar.gz +tar -xzf node-v10.15.0-linux-armv6l.tar.gz +cp -R node-v10.15.0-linux-armv6l/* /usr/local/ +rm -rf node-v10.15.0-linux-armv6l/ + echo_stamp "Add .vimrc" cat << EOF > /home/pi/.vimrc set mouse-=a @@ -137,4 +146,10 @@ syntax on autocmd BufNewFile,BufRead *.launch set syntax=xml EOF +echo_stamp "Attempting to kill dirmngr" +gpgconf --kill dirmngr +# dirmngr is only used by apt-key, so we can safely kill it. +# We ignore killall's exit value as well. +killall -w -9 dirmngr || true + echo_stamp "End of software installation" diff --git a/clever/CMakeLists.txt b/clever/CMakeLists.txt index 6ce7ec76..3507a438 100644 --- a/clever/CMakeLists.txt +++ b/clever/CMakeLists.txt @@ -26,6 +26,9 @@ find_package(catkin REQUIRED COMPONENTS cv_bridge ) +list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/cmake") + +find_package(GeographicLib REQUIRED) ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) @@ -74,7 +77,6 @@ add_service_files( Navigate.srv NavigateGlobal.srv SetPosition.srv - SetPositionGlobal.srv SetVelocity.srv SetAttitude.srv SetRates.srv @@ -138,6 +140,7 @@ catkin_package( include_directories( # include ${catkin_INCLUDE_DIRS} + ${GeographicLib_INCLUDE_DIRS} ) # Declare a C++ library @@ -145,10 +148,6 @@ add_library(clever src/optical_flow.cpp ) -add_library(fcu_horiz - src/fcu_horiz.cpp -) - add_library(aruco_vpe src/aruco_vpe.cpp ) @@ -161,18 +160,27 @@ add_library(aruco_vpe ## Declare a C++ executable ## With catkin_make all packages are built within a single CMake context ## The recommended prefix ensures that target names across packages don't collide +add_executable(simple_offboard src/simple_offboard.cpp) + add_executable(rc src/rc.cpp) add_executable(camera_markers src/camera_markers.cpp) add_executable(frames src/frames.cpp) +target_link_libraries(simple_offboard + ${catkin_LIBRARIES} + ${GeographicLib_LIBRARIES} +) + target_link_libraries(rc ${catkin_LIBRARIES}) target_link_libraries(camera_markers ${catkin_LIBRARIES}) target_link_libraries(frames ${catkin_LIBRARIES}) +add_dependencies(simple_offboard clever_generate_messages_cpp) + ## 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 @@ -188,11 +196,6 @@ target_link_libraries(clever ${catkin_LIBRARIES} ) -target_link_libraries(fcu_horiz - ${catkin_LIBRARIES} - "/opt/ros/kinetic/lib/libtf2_ros.so" -) - target_link_libraries(aruco_vpe ${catkin_LIBRARIES} ) diff --git a/clever/cmake/FindGeographicLib.cmake b/clever/cmake/FindGeographicLib.cmake new file mode 100644 index 00000000..9b29c320 --- /dev/null +++ b/clever/cmake/FindGeographicLib.cmake @@ -0,0 +1,18 @@ +# taken from: https://github.com/mavlink/mavros/blob/master/libmavconn/cmake/Modules/FindGeographicLib.cmake + +# Look for GeographicLib +# +# Set +# GEOGRAPHICLIB_FOUND = TRUE +# GeographicLib_INCLUDE_DIRS = /usr/local/include +# GeographicLib_LIBRARIES = /usr/local/lib/libGeographic.so +# GeographicLib_LIBRARY_DIRS = /usr/local/lib + +find_path (GeographicLib_INCLUDE_DIRS NAMES GeographicLib/Config.h) + +find_library (GeographicLib_LIBRARIES NAMES Geographic) + +include (FindPackageHandleStandardArgs) +find_package_handle_standard_args (GeographicLib DEFAULT_MSG + GeographicLib_LIBRARIES GeographicLib_INCLUDE_DIRS) +mark_as_advanced (GeographicLib_LIBRARIES GeographicLib_INCLUDE_DIRS) diff --git a/clever/launch/arduino.launch b/clever/launch/arduino.launch deleted file mode 100644 index 593f38d2..00000000 --- a/clever/launch/arduino.launch +++ /dev/null @@ -1,9 +0,0 @@ - - - - - - - - - diff --git a/clever/launch/aruco.launch b/clever/launch/aruco.launch index f9dc88c5..14264af9 100644 --- a/clever/launch/aruco.launch +++ b/clever/launch/aruco.launch @@ -16,8 +16,8 @@ - - + + diff --git a/clever/launch/clever.launch b/clever/launch/clever.launch index 8610bcb4..576639cc 100644 --- a/clever/launch/clever.launch +++ b/clever/launch/clever.launch @@ -8,15 +8,14 @@ + - - @@ -36,16 +35,20 @@ - - + - - - - + + + body: map + base_link: map + + + + + @@ -53,15 +56,21 @@ + + + - - - + + + + - + + + diff --git a/clever/launch/main_camera.launch b/clever/launch/main_camera.launch index 8c0aba46..fbb9abc8 100755 --- a/clever/launch/main_camera.launch +++ b/clever/launch/main_camera.launch @@ -1,20 +1,20 @@ - + - + - + - + - + diff --git a/clever/launch/mavros.launch b/clever/launch/mavros.launch index 205107d9..f895a441 100644 --- a/clever/launch/mavros.launch +++ b/clever/launch/mavros.launch @@ -5,7 +5,7 @@ - + @@ -27,6 +27,9 @@ + + + distance_sensor: @@ -51,12 +54,12 @@ - + - - + + - + - safety_area - image_pub @@ -81,14 +84,14 @@ - + - - + + diff --git a/clever/launch/sitl.launch b/clever/launch/sitl.launch index b53cec3e..e574a1ae 100644 --- a/clever/launch/sitl.launch +++ b/clever/launch/sitl.launch @@ -8,12 +8,11 @@ - - + diff --git a/clever/nodelet_plugins.xml b/clever/nodelet_plugins.xml index 36e583b6..656bc151 100644 --- a/clever/nodelet_plugins.xml +++ b/clever/nodelet_plugins.xml @@ -3,11 +3,6 @@ - - - - - diff --git a/clever/package.xml b/clever/package.xml index 4a018903..0da5a81d 100644 --- a/clever/package.xml +++ b/clever/package.xml @@ -1,5 +1,5 @@ - + clever 0.0.1 The CLEVER package @@ -7,29 +7,36 @@ Oleg Kalachev MIT - + https://clever.copterexpress.com/ Oleg Kalachev Artem Smirnov - - nodelet - roscpp - visualization_msgs - tf2_geometry_msgs - catkin - - catkin - roscpp - nodelet - mavros - mavros_extras - lxml - cv_camera - mjpg-streamer - rosbridge_server - web_video_server - ros_commr + + + roscpp + rospy + std_srvs + tf + tf2 + tf2_ros + tf2_geometry_msgs + std_msgs + geometry_msgs + sensor_msgs + visualization_msgs + geographiclib + nodelet + mavros + mavros_extras + lxml + cv_camera + cv_bridge + opencv3 + mjpg-streamer + rosbridge_server + web_video_server diff --git a/clever/src/aruco_vpe.cpp b/clever/src/aruco_vpe.cpp index 21ca69de..a1009910 100644 --- a/clever/src/aruco_vpe.cpp +++ b/clever/src/aruco_vpe.cpp @@ -39,7 +39,7 @@ private: ros::NodeHandle& nh = getNodeHandle(); ros::NodeHandle& nh_priv = getPrivateNodeHandle(); - nh_priv.param("aruco_orientation", aruco_orientation_, "local_origin"); + nh_priv.param("aruco_orientation", aruco_orientation_, "map"); bool use_mocap; nh_priv.param("use_mocap", use_mocap, false); nh_priv.param("reset_vpe", reset_vpe_, !use_mocap); @@ -107,20 +107,20 @@ private: (reset_vpe_ && (ros::Time::now() - last_published_ > reset_timeout_))) // vpe origin outdated { ROS_INFO("Reset VPE"); - t = tf_buffer.lookupTransform("local_origin", "aruco_map_vision", stamp, lookup_timeout_); + t = tf_buffer.lookupTransform("map", "aruco_map_vision", stamp, lookup_timeout_); t.child_frame_id = "aruco_map"; static_br.sendTransform(t); } // Calculate VPE - ps.header.frame_id = "fcu_horiz"; + ps.header.frame_id = "body"; ps.header.stamp = stamp; ps.pose.orientation.w = 1; tf_buffer.transform(ps, vpe_raw, "aruco_map_vision", lookup_timeout_); vpe_raw.header.frame_id = "aruco_map"; - tf_buffer.transform(vpe_raw, vpe, "local_origin", lookup_timeout_); + tf_buffer.transform(vpe_raw, vpe, "map", lookup_timeout_); vision_position_pub_.publish(vpe); diff --git a/clever/src/fcu_horiz.cpp b/clever/src/fcu_horiz.cpp deleted file mode 100644 index aa17c1e3..00000000 --- a/clever/src/fcu_horiz.cpp +++ /dev/null @@ -1,40 +0,0 @@ -#include -#include -#include -#include -#include - -#include "util.h" - -class FcuHoriz : public nodelet::Nodelet -{ - geometry_msgs::TransformStamped t_; - - void handlePose(const geometry_msgs::PoseStampedConstPtr& msg) - { - static tf2_ros::TransformBroadcaster br; - double roll, pitch, yaw; - - t_.header.stamp = msg->header.stamp; - t_.header.frame_id = msg->header.frame_id; - t_.transform.translation.x = msg->pose.position.x; - t_.transform.translation.y = msg->pose.position.y; - t_.transform.translation.z = msg->pose.position.z; - - // Warning: this is not thead-safe - quaternionToEuler(msg->pose.orientation, roll, pitch, yaw); - eulerToQuaternion(t_.transform.rotation, 0, 0, yaw); - - br.sendTransform(t_); - } - - void onInit() - { - t_.child_frame_id = "fcu_horiz"; - t_.transform.rotation.w = 1; - static ros::Subscriber pose_sub = getNodeHandle().subscribe("mavros/local_position/pose", 1, &FcuHoriz::handlePose, this); - ROS_INFO("fcu_horiz initialized"); - } -}; - -PLUGINLIB_EXPORT_CLASS(FcuHoriz, nodelet::Nodelet) diff --git a/clever/src/global_local.py b/clever/src/global_local.py deleted file mode 100644 index dd5f7d85..00000000 --- a/clever/src/global_local.py +++ /dev/null @@ -1,43 +0,0 @@ -import rospy -import math -import geopy -from geometry_msgs.msg import PoseStamped -from geopy.distance import VincentyDistance, vincenty -from sensor_msgs.msg import NavSatFix - - -def global_to_local(lat, lon): - # TODO: refactor - - try: - position_global = rospy.wait_for_message('mavros/global_position/global', NavSatFix, timeout=0.5) - except rospy.exceptions.ROSException: - raise Exception('No global position') - - try: - pose = rospy.wait_for_message('mavros/local_position/pose', PoseStamped, timeout=0.5) - except rospy.exceptions.ROSException: - raise Exception('No local position') - - d = math.hypot(pose.pose.position.x, pose.pose.position.y) - - bearing = math.degrees(math.atan2(-pose.pose.position.x, -pose.pose.position.y)) - if bearing < 0: - bearing += 360 - - cur = geopy.Point(position_global.latitude, position_global.longitude) - origin = VincentyDistance(meters=d).destination(cur, bearing) - - _origin = origin.latitude, origin.longitude - olat_tlon = origin.latitude, lon - tlat_olon = lat, origin.longitude - - N = vincenty(_origin, tlat_olon) - if lat < origin.latitude: - N = -N - - E = vincenty(_origin, olat_tlon) - if lon < origin.longitude: - E = -E - - return E.meters, N.meters diff --git a/clever/src/interactive.py b/clever/src/interactive.py index 1ec5d7ab..4547d5b7 100755 --- a/clever/src/interactive.py +++ b/clever/src/interactive.py @@ -35,7 +35,7 @@ def make_box_control(msg): def make_quadcopter_marker(): marker = InteractiveMarker() - marker.header.frame_id = 'fcu' + marker.header.frame_id = 'base_link' marker.header.stamp = rospy.get_rostime() marker.scale = 1 marker.pose.orientation.w = 1 diff --git a/clever/src/optical_flow.cpp b/clever/src/optical_flow.cpp index 02eae273..c5351606 100644 --- a/clever/src/optical_flow.cpp +++ b/clever/src/optical_flow.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include #include @@ -41,7 +42,7 @@ public: private: ros::Publisher flow_pub_, velo_pub_, shift_pub_; ros::Time prev_stamp_; - std::string fcu_frame_id_; + std::string fcu_frame_id_, local_frame_id_; image_transport::CameraSubscriber img_sub_; image_transport::Publisher img_pub_; mavros_msgs::OpticalFlowRad flow_; @@ -51,6 +52,7 @@ private: Mat camera_matrix_, dist_coeffs_; tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; + bool calc_flow_gyro_; void onInit() { @@ -59,9 +61,11 @@ private: image_transport::ImageTransport it(nh); image_transport::ImageTransport it_priv(nh_priv); - nh_priv.param("mavros/local_position/tf/child_frame_id", fcu_frame_id_, "fcu"); + nh.param("mavros/local_position/tf/frame_id", local_frame_id_, "map"); + nh.param("mavros/local_position/tf/child_frame_id", fcu_frame_id_, "base_link"); nh_priv.param("roi", roi_, 128); roi_2_ = roi_ / 2; + nh_priv.param("calc_flow_gyro", calc_flow_gyro_, false); img_sub_ = it.subscribeCamera("image", 1, &OpticalFlow::flow, this); img_pub_ = it_priv.advertise("debug", 1); @@ -163,6 +167,21 @@ private: ros::Duration integration_time = msg->header.stamp - prev_stamp_; uint32_t integration_time_us = integration_time.toSec() * 1.0e6; + if (calc_flow_gyro_) { + try { + auto flow_gyro_camera = calcFlowGyro(msg->header.frame_id, prev_stamp_, msg->header.stamp); + static geometry_msgs::Vector3Stamped flow_gyro_fcu; + tf_buffer_.transform(flow_gyro_camera, flow_gyro_fcu, fcu_frame_id_); + flow_.integrated_xgyro = flow_gyro_fcu.vector.x; + flow_.integrated_ygyro = flow_gyro_fcu.vector.y; + flow_.integrated_zgyro = flow_gyro_fcu.vector.z; + } catch (const tf2::TransformException& e) { + // Invalidate previous frame + prev_.release(); + return; + } + } + // Publish flow in fcu frame flow_.header.stamp = /*prev_stamp_*/ msg->header.stamp; flow_.integration_time_us = integration_time_us; @@ -195,6 +214,23 @@ private: prev_stamp_ = msg->header.stamp; } } + + geometry_msgs::Vector3Stamped calcFlowGyro(const std::string& frame_id, const ros::Time& prev, const ros::Time& curr) + { + tf2::Quaternion prev_rot, curr_rot; + tf2::fromMsg(tf_buffer_.lookupTransform(frame_id, local_frame_id_, prev).transform.rotation, prev_rot); + tf2::fromMsg(tf_buffer_.lookupTransform(frame_id, local_frame_id_, curr).transform.rotation, curr_rot); + + geometry_msgs::Vector3Stamped flow; + flow.header.frame_id = frame_id; + flow.header.stamp = curr; + auto diff = ((curr_rot - prev_rot) * prev_rot.inverse()) * 2.0f; + flow.vector.x = diff.x(); + flow.vector.y = diff.y(); + flow.vector.z = diff.z(); + + return flow; + } }; PLUGINLIB_EXPORT_CLASS(OpticalFlow, nodelet::Nodelet) diff --git a/clever/src/selfcheck.py b/clever/src/selfcheck.py index 7a34d6e6..8ef2e496 100755 --- a/clever/src/selfcheck.py +++ b/clever/src/selfcheck.py @@ -16,7 +16,7 @@ import tf.transformations as t # TODO: clever.service is running # TODO: check attitude is present # TODO: disk free space -# TODO: local_origin, fcu, fcu_horiz +# TODO: map, base_link, body # TODO: rc service # TODO: perform commander check, ekf2 status on PX4 # TODO: check if FCU params setter succeed diff --git a/clever/src/simple_offboard.cpp b/clever/src/simple_offboard.cpp new file mode 100644 index 00000000..b4a31416 --- /dev/null +++ b/clever/src/simple_offboard.cpp @@ -0,0 +1,742 @@ +/* + * Simplified copter control in OFFBOARD mode + * Copyright (C) 2019 Copter Express Technologies + * + * Author: Oleg Kalachev + * + * 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +using std::string; +using namespace geometry_msgs; +using namespace sensor_msgs; +using namespace clever; +using mavros_msgs::PositionTarget; +using mavros_msgs::AttitudeTarget; +using mavros_msgs::Thrust; + +// tf2 +tf2_ros::Buffer tf_buffer; + +// Parameters +string local_frame; +string fcu_frame; +ros::Duration transform_timeout; +ros::Duration telemetry_transform_timeout; +ros::Duration offboard_timeout; +ros::Duration land_timeout; +ros::Duration arming_timeout; +ros::Duration local_position_timeout; +ros::Duration state_timeout; +ros::Duration velocity_timeout; +ros::Duration global_position_timeout; +ros::Duration battery_timeout; +float default_speed; +bool auto_release; +std::map reference_frames; + +// Publishers +ros::Publisher attitude_pub, attitude_raw_pub, position_pub, position_raw_pub, rates_pub, thrust_pub; + +// Service clients +ros::ServiceClient arming, set_mode; + +// Containers +ros::Timer setpoint_timer; +tf::Quaternion tq; +PoseStamped position_msg; +PositionTarget position_raw_msg; +AttitudeTarget att_raw_msg; +Thrust thrust_msg; +TwistStamped rates_msg; +TransformStamped target; + +// State +PoseStamped nav_start; +PoseStamped setpoint_position, setpoint_position_transformed; +Vector3Stamped setpoint_velocity, setpoint_velocity_transformed; +QuaternionStamped setpoint_attitude, setpoint_attitude_transformed; +float setpoint_yaw_rate; +float nav_speed; +bool busy = false; +bool wait_armed = false; + +enum setpoint_type_t { + NONE, + NAVIGATE, + NAVIGATE_GLOBAL, + POSITION, + VELOCITY, + ATTITUDE, + RATES +}; + +enum setpoint_type_t setpoint_type = NONE; + +enum { YAW, YAW_RATE, TOWARDS } setpoint_yaw_type; + +// Last received telemetry messages +mavros_msgs::State state; +PoseStamped local_position; +TwistStamped velocity; +NavSatFix global_position; +BatteryState battery; + +// Common subcriber callback template that stores message to the variable +template +void handleMessage(const T& msg) +{ + STORAGE = msg; +} + +// wait for transform without interrupting publishing setpoints +inline bool waitTransform(const string& target, const string& source, + const ros::Time& stamp, const ros::Duration& timeout) +{ + ros::Rate r(10); + auto start = ros::Time::now(); + while (ros::ok()) { + if (ros::Time::now() - start > timeout) return false; + if (tf_buffer.canTransform(target, source, stamp)) return true; + ros::spinOnce(); + r.sleep(); + } +} + +#define TIMEOUT(msg, timeout) (ros::Time::now() - msg.header.stamp > timeout) + +bool getTelemetry(GetTelemetry::Request& req, GetTelemetry::Response& res) +{ + ros::Time stamp = ros::Time::now(); + + if (req.frame_id.empty()) + req.frame_id = local_frame; + + res.frame_id = req.frame_id; + res.x = NAN; + res.y = NAN; + res.z = NAN; + res.lat = NAN; + res.lon = NAN; + res.alt = NAN; + res.vx = NAN; + res.vy = NAN; + res.vz = NAN; + res.pitch = NAN; + res.roll = NAN; + res.yaw = NAN; + res.pitch_rate = NAN; + res.roll_rate = NAN; + res.yaw_rate = NAN; + res.voltage = NAN; + res.cell_voltage = NAN; + + if (!TIMEOUT(state, state_timeout)) { + res.connected = state.connected; + res.armed = state.armed; + res.mode = state.mode; + } + + waitTransform(local_frame, req.frame_id, stamp, telemetry_transform_timeout); + + if (!TIMEOUT(local_position, local_position_timeout)) { + try { + // transform pose + PoseStamped pose; + tf_buffer.transform(local_position, pose, req.frame_id); + res.x = pose.pose.position.x; + res.y = pose.pose.position.y; + res.z = pose.pose.position.z; + + // Tait-Bryan angles, order z-y-x + double yaw, pitch, roll; + tf2::getEulerYPR(pose.pose.orientation, yaw, pitch, roll); + res.yaw = yaw; + res.pitch = pitch; + res.roll = roll; + } catch (const tf2::TransformException& e) {} + } + + if (!TIMEOUT(velocity, velocity_timeout)) { + try { + // transform velocity + Vector3Stamped vec, vec_out; + vec.header = velocity.header; + vec.vector = velocity.twist.linear; + tf_buffer.transform(vec, vec_out, req.frame_id); + + res.vx = vec_out.vector.x; + res.vy = vec_out.vector.y; + res.vz = vec_out.vector.z; + } catch (const tf2::TransformException& e) {} + + // use angular velocities as they are + res.yaw_rate = velocity.twist.angular.z; + res.pitch_rate = velocity.twist.angular.y; + res.roll_rate = velocity.twist.angular.x; + } + + if (!TIMEOUT(global_position, global_position_timeout)) { + res.lat = global_position.latitude; + res.lon = global_position.longitude; + res.alt = global_position.altitude; + } + + if (!TIMEOUT(battery, battery_timeout)) { + res.voltage = battery.voltage; + if (!battery.cell_voltage.empty()) { + res.cell_voltage = battery.cell_voltage[0]; + } + } + + return true; +} + +// throws std::runtime_error +void offboardAndArm() +{ + ros::Rate r(10); + + if (state.mode != "OFFBOARD") { + auto start = ros::Time::now(); + ROS_INFO("simple_offboard: switch to OFFBOARD"); + static mavros_msgs::SetMode sm; + sm.request.custom_mode = "OFFBOARD"; + + if (!set_mode.call(sm)) + throw std::runtime_error("Error calling set_mode service"); + + // wait for OFFBOARD mode + while (ros::ok()) { + ros::spinOnce(); + if (state.mode == "OFFBOARD") { + break; + } else if (ros::Time::now() - start > offboard_timeout) { + throw std::runtime_error("OFFBOARD request timed out"); + } + ros::spinOnce(); + r.sleep(); + } + } + + if (!state.armed) { + ros::Time start = ros::Time::now(); + ROS_INFO("simple_offboard: arming"); + mavros_msgs::CommandBool srv; + srv.request.value = true; + if (!arming.call(srv)) { + throw std::runtime_error("Error calling arming service"); + } + + // wait until armed + while (ros::ok()) { + ros::spinOnce(); + if (state.armed) { + break; + } else if (ros::Time::now() - start > arming_timeout) { + throw std::runtime_error("Arming timed out"); + } + ros::spinOnce(); + r.sleep(); + } + } +} + +inline double hypot(double x, double y, double z) +{ + return std::sqrt(x * x + y * y + z * z); +} + +inline float getDistance(const Point& from, const Point& to) +{ + return hypot(from.x - to.x, from.y - to.y, from.z - to.z); +} + +void getNavigateSetpoint(const ros::Time& stamp, float speed, Point& nav_setpoint) +{ + if (wait_armed) { + // don't start navigating if we're waiting arming + nav_start.header.stamp = stamp; + } + + float distance = getDistance(nav_start.pose.position, setpoint_position_transformed.pose.position); + float time = distance / speed; + float passed = std::min((stamp - nav_start.header.stamp).toSec() / time, 1.0); + + nav_setpoint.x = nav_start.pose.position.x + (setpoint_position_transformed.pose.position.x - nav_start.pose.position.x) * passed; + nav_setpoint.y = nav_start.pose.position.y + (setpoint_position_transformed.pose.position.y - nav_start.pose.position.y) * passed; + nav_setpoint.z = nav_start.pose.position.z + (setpoint_position_transformed.pose.position.z - nav_start.pose.position.z) * passed; +} + +PoseStamped globalToLocal(double lat, double lon) +{ + auto earth = GeographicLib::Geodesic::WGS84(); + + // Determine azimuth and distance between current and destination point + double _, distance, azimuth; + earth.Inverse(global_position.latitude, global_position.longitude, lat, lon, distance, _, azimuth); + + double x_offset, y_offset; + double azimuth_radians = azimuth * M_PI / 180; + x_offset = distance * sin(azimuth_radians); + y_offset = distance * cos(azimuth_radians); + + auto local = tf_buffer.lookupTransform(local_frame, fcu_frame, global_position.header.stamp); + + PoseStamped pose; + pose.header.stamp = global_position.header.stamp; // TODO: ? + pose.header.frame_id = local_frame; + pose.pose.position.x = local.transform.translation.x + x_offset; + pose.pose.position.y = local.transform.translation.y + y_offset; + pose.pose.orientation.w = 1; + return pose; +} + +void publish(const ros::Time stamp) +{ + if (setpoint_type == NONE) return; + + position_raw_msg.header.stamp = stamp; + thrust_msg.header.stamp = stamp; + rates_msg.header.stamp = stamp; + + try { + // transform position and/or yaw + if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == POSITION || setpoint_type == VELOCITY || setpoint_type == ATTITUDE) { + setpoint_position.header.stamp = stamp; + tf_buffer.transform(setpoint_position, setpoint_position_transformed, local_frame, ros::Duration(0.05)); + } + + // transform velocity + if (setpoint_type == VELOCITY) { + setpoint_velocity.header.stamp = stamp; + tf_buffer.transform(setpoint_velocity, setpoint_velocity_transformed, local_frame, ros::Duration(0.05)); + } + + } catch (const tf2::TransformException& e) { + ROS_WARN_THROTTLE(10, "simple_offboard: can't transform"); + } + + if (!target.child_frame_id.empty()) { + if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == POSITION) { + static tf2_ros::TransformBroadcaster tf_broadcaster; + target.header = setpoint_position_transformed.header; + target.transform.translation.x = setpoint_position_transformed.pose.position.x; + target.transform.translation.y = setpoint_position_transformed.pose.position.y; + target.transform.translation.z = setpoint_position_transformed.pose.position.z; + target.transform.rotation = setpoint_position_transformed.pose.orientation; + tf_broadcaster.sendTransform(target); + } + } + + if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL) { + position_msg.pose.orientation = setpoint_position_transformed.pose.orientation; // copy yaw + getNavigateSetpoint(stamp, nav_speed, position_msg.pose.position); + + if (setpoint_yaw_type == TOWARDS) { + double yaw_towards = atan2(position_msg.pose.position.y - nav_start.pose.position.y, + position_msg.pose.position.x - nav_start.pose.position.x); + position_msg.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, yaw_towards); + } + } + + if (setpoint_type == POSITION) { + position_msg = setpoint_position_transformed; + } + + if (setpoint_type == POSITION || setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL) { + if (setpoint_yaw_type == YAW || setpoint_yaw_type == TOWARDS) { + position_msg.header.stamp = stamp; + position_pub.publish(position_msg); + + } else { + position_raw_msg.type_mask = PositionTarget::IGNORE_VX + + PositionTarget::IGNORE_VY + + PositionTarget::IGNORE_VZ + + PositionTarget::IGNORE_AFX + + PositionTarget::IGNORE_AFY + + PositionTarget::IGNORE_AFZ + + PositionTarget::IGNORE_YAW; + position_raw_msg.yaw_rate = setpoint_yaw_rate; + position_raw_msg.position = position_msg.pose.position; + position_raw_pub.publish(position_raw_msg); + } + } + + if (setpoint_type == VELOCITY) { + position_raw_msg.type_mask = PositionTarget::IGNORE_PX + + PositionTarget::IGNORE_PY + + PositionTarget::IGNORE_PZ + + PositionTarget::IGNORE_AFX + + PositionTarget::IGNORE_AFY + + PositionTarget::IGNORE_AFZ; + position_raw_msg.type_mask += setpoint_yaw_type == YAW ? PositionTarget::IGNORE_YAW_RATE : PositionTarget::IGNORE_YAW; + position_raw_msg.velocity = setpoint_velocity_transformed.vector; + position_raw_msg.yaw = tf2::getYaw(setpoint_position_transformed.pose.orientation); + position_raw_msg.yaw_rate = setpoint_yaw_rate; + position_raw_pub.publish(position_raw_msg); + } + + if (setpoint_type == ATTITUDE) { + attitude_pub.publish(setpoint_position_transformed); + thrust_pub.publish(thrust_msg); + } + + if (setpoint_type == RATES) { + // rates_pub.publish(rates_msg); + // thrust_pub.publish(thrust_msg); + // mavros rates topics waits for rates in local frame + // use rates in body frame for simplicity + att_raw_msg.header.stamp = stamp; + att_raw_msg.header.frame_id = fcu_frame; + att_raw_msg.type_mask = AttitudeTarget::IGNORE_ATTITUDE; + att_raw_msg.body_rate = rates_msg.twist.angular; + att_raw_msg.thrust = thrust_msg.thrust; + attitude_raw_pub.publish(att_raw_msg); + } +} + +void publishSetpoint(const ros::TimerEvent& event) +{ + publish(event.current_real); +} + +inline void checkState() +{ + if (TIMEOUT(state, state_timeout)) + throw std::runtime_error("State timeout, check mavros settings"); + + if (!state.connected) + throw std::runtime_error("No connection to FCU, https://clever.copterexpress.com/connection.html"); +} + +inline void serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, float vy, float vz, + float pitch, float roll, float yaw, float pitch_rate, float roll_rate, float yaw_rate, + float lat, float lon, float thrust, float speed, string frame_id, bool auto_arm, + uint8_t& success, string& message) +{ + auto stamp = ros::Time::now(); + + try { + if (busy) + throw std::runtime_error("Busy"); + + busy = true; + + // Checks + checkState(); + + if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL) { + if (TIMEOUT(local_position, local_position_timeout)) + throw std::runtime_error("No local position, check settings"); + + if (speed < 0) + throw std::runtime_error("Navigate speed must be positive, " + std::to_string(speed) + " passed"); + + if (speed == 0) + speed = default_speed; + } + + if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == POSITION || sp_type == VELOCITY) { + if (yaw_rate != 0 && !std::isnan(yaw)) + throw std::runtime_error("Yaw value should be NaN for setting yaw rate"); + + if (std::isnan(yaw_rate) && std::isnan(yaw)) + throw std::runtime_error("Both yaw and yaw_rate cannot be NaN"); + } + + if (sp_type == NAVIGATE_GLOBAL) { + if (TIMEOUT(global_position, global_position_timeout)) + throw std::runtime_error("No global position"); + } + + // default frame is local frame + if (frame_id.empty()) + frame_id = local_frame; + + // look up for reference frame + auto search = reference_frames.find(frame_id); + const string& reference_frame = search == reference_frames.end() ? frame_id : search->second; + + if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == POSITION || sp_type == VELOCITY || sp_type == ATTITUDE) { + // make sure transform from frame_id to reference frame available + if (!waitTransform(reference_frame, frame_id, stamp, transform_timeout)) + throw std::runtime_error("Can't transform from " + frame_id + " to " + reference_frame); + + // make sure transform from reference frame to local frame available + if (!waitTransform(local_frame, reference_frame, stamp, transform_timeout)) + throw std::runtime_error("Can't transform from " + reference_frame + " to " + local_frame); + } + + if (sp_type == NAVIGATE_GLOBAL) { + // Calculate x and from lat and lot in request's frame + auto xy_in_req_frame = tf_buffer.transform(globalToLocal(lat, lon), frame_id); + x = xy_in_req_frame.pose.position.x; + y = xy_in_req_frame.pose.position.y; + } + + // Everything fine - switch setpoint type + setpoint_type = sp_type; + + if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL) { + // starting point + nav_start = local_position; + nav_speed = speed; + } + + if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == POSITION || sp_type == VELOCITY) { + if (std::isnan(yaw) && yaw_rate == 0) { + // keep yaw unchanged + yaw = tf2::getYaw(local_position.pose.orientation); + } + } + + if (sp_type == POSITION || sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == VELOCITY || sp_type == ATTITUDE) { + // destination point and/or yaw + static PoseStamped ps; + ps.header.frame_id = frame_id; + ps.header.stamp = stamp; + ps.pose.position.x = x; + ps.pose.position.y = y; + ps.pose.position.z = z; + + if (std::isnan(yaw)) { + setpoint_yaw_type = YAW_RATE; + setpoint_yaw_rate = yaw_rate; + } else if (std::isinf(yaw) && yaw > 0) { + // yaw towards + setpoint_yaw_type = TOWARDS; + yaw = 0; + setpoint_yaw_rate = 0; + } else { + setpoint_yaw_type = YAW; + setpoint_yaw_rate = 0; + ps.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw); + } + + tf_buffer.transform(ps, setpoint_position, reference_frame); + } + + if (sp_type == VELOCITY) { + static Vector3Stamped vel; + vel.header.frame_id = frame_id; + vel.header.stamp = stamp; + vel.vector.x = vx; + vel.vector.y = vy; + vel.vector.z = vz; + tf_buffer.transform(vel, setpoint_velocity, reference_frame); + } + + if (sp_type == ATTITUDE || sp_type == RATES) { + thrust_msg.thrust = thrust; + } + + if (sp_type == RATES) { + rates_msg.twist.angular.x = roll_rate; + rates_msg.twist.angular.y = pitch_rate; + rates_msg.twist.angular.z = yaw_rate; + } + + wait_armed = auto_arm; + + publish(stamp); // calculate initial transformed messages first + setpoint_timer.start(); + + if (auto_arm) { + offboardAndArm(); + wait_armed = false; + } else if (state.mode != "OFFBOARD") { + setpoint_timer.stop(); + throw std::runtime_error("Copter is not in OFFBOARD mode, use auto_arm?"); + } else if (!state.armed) { + setpoint_timer.stop(); + throw std::runtime_error("Copter is not armed, use auto_arm?"); + } + + } catch (const std::exception& e) { + message = e.what(); + ROS_INFO("simple_offboard: %s", message.c_str()); + busy = false; + return; + } + + success = true; + busy = false; + return; +} + +bool navigate(Navigate::Request& req, Navigate::Response& res) { + serve(NAVIGATE, req.x, req.y, req.z, 0, 0, 0, 0, 0, req.yaw, 0, 0, req.yaw_rate, 0, 0, 0, req.speed, req.frame_id, req.auto_arm, res.success, res.message); + return true; +} + +bool navigateGlobal(NavigateGlobal::Request& req, NavigateGlobal::Response& res) { + serve(NAVIGATE_GLOBAL, 0, 0, req.z, 0, 0, 0, 0, 0, req.yaw, 0, 0, req.yaw_rate, req.lat, req.lon, 0, req.speed, req.frame_id, req.auto_arm, res.success, res.message); + return true; +} + +bool setPosition(SetPosition::Request& req, SetPosition::Response& res) { + serve(POSITION, req.x, req.y, req.z, 0, 0, 0, 0, 0, req.yaw, 0, 0, req.yaw_rate, 0, 0, 0, 0, req.frame_id, req.auto_arm, res.success, res.message); + return true; +} + +bool setVelocity(SetVelocity::Request& req, SetVelocity::Response& res) { + serve(VELOCITY, 0, 0, 0, req.vx, req.vy, req.vz, 0, 0, req.yaw, 0, 0, req.yaw_rate, 0, 0, 0, 0, req.frame_id, req.auto_arm, res.success, res.message); + return true; +} + +bool setAttitude(SetAttitude::Request& req, SetAttitude::Response& res) { + serve(ATTITUDE, 0, 0, 0, 0, 0, 0, req.pitch, req.roll, req.yaw, 0, 0, 0, 0, 0, req.thrust, 0, req.frame_id, req.auto_arm, res.success, res.message); + return true; +} + +bool setRates(SetRates::Request& req, SetRates::Response& res) { + serve(RATES, 0, 0, 0, 0, 0, 0, 0, 0, 0, req.pitch_rate, req.roll_rate, req.yaw_rate, 0, 0, req.thrust, 0, "", req.auto_arm, res.success, res.message); + return true; +} + +bool land(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) +{ + try { + if (busy) + throw std::runtime_error("Busy"); + + busy = true; + + checkState(); + + static mavros_msgs::SetMode sm; + sm.request.custom_mode = "AUTO.LAND"; + + if (!set_mode.call(sm)) + throw std::runtime_error("Can't call set_mode service"); + + if (!sm.response.mode_sent) + throw std::runtime_error("Can't send set_mode request"); + + static ros::Rate r(10); + auto start = ros::Time::now(); + while (ros::ok()) { + if (state.mode == "AUTO.LAND") { + res.success = true; + busy = false; + return true; + } + if (ros::Time::now() - start > land_timeout) + throw std::runtime_error("Land request timed out"); + + ros::spinOnce(); + r.sleep(); + } + + } catch (const std::exception& e) { + res.message = e.what(); + ROS_INFO("simple_offboard: %s", e.what()); + busy = false; + return true; + } +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "simple_offboard"); + ros::NodeHandle nh, nh_priv("~"); + + tf2_ros::TransformListener tf_listener(tf_buffer); + + // Params + nh.param("mavros/local_position/tf/frame_id", local_frame, "map"); + nh.param("mavros/local_position/tf/child_frame_id", fcu_frame, "base_link"); + nh_priv.param("target_frame", target.child_frame_id, string("target")); + nh_priv.param("auto_release", auto_release, true); + nh_priv.param("default_speed", default_speed, 0.5f); + nh_priv.getParam("reference_frames", reference_frames); + + state_timeout = ros::Duration(nh_priv.param("state_timeout", 3.0)); + local_position_timeout = ros::Duration(nh_priv.param("local_position_timeout", 2.0)); + velocity_timeout = ros::Duration(nh_priv.param("velocity_timeout", 2.0)); + global_position_timeout = ros::Duration(nh_priv.param("global_position_timeout", 10.0)); + battery_timeout = ros::Duration(nh_priv.param("battery_timeout", 2.0)); + + transform_timeout = ros::Duration(nh_priv.param("transform_timeout", 0.5)); + telemetry_transform_timeout = ros::Duration(nh_priv.param("telemetry_transform_timeout", 0.5)); + offboard_timeout = ros::Duration(nh_priv.param("offboard_timeout", 3.0)); + land_timeout = ros::Duration(nh_priv.param("land_timeout", 3.0)); + arming_timeout = ros::Duration(nh_priv.param("arming_timeout", 4.0)); + + // Service clients + arming = nh.serviceClient("mavros/cmd/arming"); + set_mode = nh.serviceClient("mavros/set_mode"); + + // Telemetry subscribers + auto state_sub = nh.subscribe("mavros/state", 1, &handleMessage); + auto local_position_sub = nh.subscribe("mavros/local_position/pose", 1, &handleMessage); + auto velocity_sub = nh.subscribe("mavros/local_position/velocity", 1, &handleMessage); + auto global_position_sub = nh.subscribe("mavros/global_position/global", 1, &handleMessage); + auto battery_sub = nh.subscribe("mavros/battery", 1, &handleMessage); + + // Setpoint publishers + position_pub = nh.advertise("mavros/setpoint_position/local", 1); + position_raw_pub = nh.advertise("mavros/setpoint_raw/local", 1); + attitude_pub = nh.advertise("mavros/setpoint_attitude/attitude", 1); + attitude_raw_pub = nh.advertise("mavros/setpoint_raw/attitude", 1); + rates_pub = nh.advertise("mavros/setpoint_attitude/cmd_vel", 1); + thrust_pub = nh.advertise("mavros/setpoint_attitude/thrust", 1); + + // Service servers + auto gt_serv = nh.advertiseService("get_telemetry", &getTelemetry); + auto na_serv = nh.advertiseService("navigate", &navigate); + auto ng_serv = nh.advertiseService("navigate_global", &navigateGlobal); + auto sp_serv = nh.advertiseService("set_position", &setPosition); + auto sv_serv = nh.advertiseService("set_velocity", &setVelocity); + auto sa_serv = nh.advertiseService("set_attitude", &setAttitude); + auto sr_serv = nh.advertiseService("set_rates", &setRates); + auto ld_serv = nh.advertiseService("land", &land); + + // Setpoint timer + setpoint_timer = nh.createTimer(ros::Duration(1 / nh_priv.param("setpoint_rate", 30.0)), &publishSetpoint, false, false); + + position_msg.header.frame_id = local_frame; + position_raw_msg.header.frame_id = local_frame; + position_raw_msg.coordinate_frame = PositionTarget::FRAME_LOCAL_NED; + rates_msg.header.frame_id = fcu_frame; + + ROS_INFO("simple_offboard: ready"); + ros::spin(); +} diff --git a/clever/src/simple_offboard.py b/clever/src/simple_offboard.py deleted file mode 100755 index 887bca41..00000000 --- a/clever/src/simple_offboard.py +++ /dev/null @@ -1,480 +0,0 @@ -#!/usr/bin/env python -from __future__ import division - -import rospy -from geometry_msgs.msg import TransformStamped, PoseStamped, Point, PointStamped, Vector3, \ - Vector3Stamped, TwistStamped, QuaternionStamped -from sensor_msgs.msg import NavSatFix, BatteryState -import tf2_ros -import tf2_geometry_msgs -from mavros_msgs.msg import PositionTarget, AttitudeTarget, State -from mavros_msgs.srv import CommandBool, SetMode -from threading import Lock -import math - -from global_local import global_to_local -from util import euler_from_orientation, vector3_from_point, orientation_from_euler -from std_srvs.srv import Trigger -from clever import srv - - -rospy.init_node('simple_offboard') - - -# TF2 stuff -tf_broadcaster = tf2_ros.TransformBroadcaster() -static_tf_broadcaster = tf2_ros.StaticTransformBroadcaster() - -tf_buffer = tf2_ros.Buffer() -tf_listener = tf2_ros.TransformListener(tf_buffer) - - -position_pub = rospy.Publisher('/mavros/setpoint_raw/local', PositionTarget, queue_size=1) -attitude_pub = rospy.Publisher('/mavros/setpoint_raw/attitude', AttitudeTarget, queue_size=1) -target_pub = rospy.Publisher('~target', PoseStamped, queue_size=1) - -arming = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool, persistent=True) -set_mode = rospy.ServiceProxy('/mavros/set_mode', SetMode, persistent=True) - - -pose = None -global_position = None -velocity = None -state = None -battery = None - - -def pose_update(data): - global pose - pose = data - - -def global_position_update(data): - global global_position - global_position = data - - -def velocity_update(data): - global velocity - velocity = data - - -def state_update(data): - global state - state = data - - -def battery_update(data): - global battery - battery = data - - -rospy.Subscriber('/mavros/state', State, state_update) -rospy.Subscriber('/mavros/local_position/pose', PoseStamped, pose_update) -rospy.Subscriber('/mavros/local_position/velocity', TwistStamped, velocity_update) -rospy.Subscriber('/mavros/global_position/global', NavSatFix, global_position_update) -rospy.Subscriber('/mavros/battery', BatteryState, battery_update) - - -PT = PositionTarget -AT = AttitudeTarget -AUTO_OFFBOARD = rospy.get_param('~auto_offboard', True) -AUTO_ARM = AUTO_OFFBOARD and rospy.get_param('~auto_arm', True) -OFFBOARD_TIMEOUT = rospy.Duration(rospy.get_param('~offboard_timeout', 3)) -ARM_TIMEOUT = rospy.Duration(rospy.get_param('~arm_timeout', 5)) -LOCAL_POSITION_TIMEOUT = rospy.Duration(rospy.get_param('~local_position_timeout', 0.5)) -NAVIGATE_AFTER_ARMED = rospy.Duration(rospy.get_param('~navigate_after_armed', True)) -TRANSFORM_TIMEOUT = rospy.Duration(rospy.get_param('~transform_timeout', 3)) -SETPOINT_RATE = rospy.get_param('~setpoint_rate', 30) -LOCAL_FRAME = rospy.get_param('mavros/local_position/frame_id', 'local_origin') -LAND_MODE = rospy.get_param('~land_mode', 'AUTO.LAND') -LAND_TIMEOUT = rospy.Duration(rospy.get_param('~land_timeout', 2)) -DEFAULT_SPEED = rospy.get_param('~default_speed', 0.5) - - -def offboard_and_arm(): - if AUTO_OFFBOARD and state.mode != 'OFFBOARD': - rospy.sleep(.3) - rospy.loginfo('Switch mode to OFFBOARD') - res = set_mode(base_mode=0, custom_mode='OFFBOARD') - - start = rospy.get_rostime() - while True: - if state.mode == 'OFFBOARD': - break - if rospy.get_rostime() - start > OFFBOARD_TIMEOUT: - raise Exception('OFFBOARD request timed out') - rospy.sleep(0.1) - - if AUTO_ARM and not state.armed: - rospy.loginfo('Arming') - res = arming(True) - - start = rospy.get_rostime() - while True: - if state.armed: - return True - if rospy.get_rostime() - start > ARM_TIMEOUT: - raise Exception('Arming timed out') - rospy.sleep(0.1) - - -ps = PoseStamped() -vs = Vector3Stamped() -pt = PositionTarget() -at = AttitudeTarget() - - -BRAKE_TIME = rospy.Duration(0) - - -def get_navigate_setpoint(stamp, start, finish, start_stamp, speed): - distance = math.sqrt((finish.z - start.z)**2 + (finish.x - start.x)**2 + (finish.y - start.y)**2) - time = rospy.Duration(distance / speed) - if time == rospy.Duration(0): - k = 0 - else: - k = (stamp - start_stamp) / time - time_left = start_stamp + time - stamp - - if BRAKE_TIME and time_left < BRAKE_TIME: - # time to brake - time_before_braking = time - BRAKE_TIME - brake_time_passed = (stamp - start_stamp - time_before_braking) - - if brake_time_passed > 2 * BRAKE_TIME: - # finish - k = 1 - else: - # brake! - k_before_braking = time_before_braking / time - k_after_braking = (speed * brake_time_passed.to_sec() - brake_time_passed.to_sec() ** 2 * speed / 4 / BRAKE_TIME.to_sec()) / distance - k = k_before_braking + k_after_braking - - k = min(k, 1) - - p = Point() - p.x = start.x + (finish.x - start.x) * k - p.y = start.y + (finish.y - start.y) * k - p.z = start.z + (finish.z - start.z) * k - return p - - -def get_publisher_and_message(req, stamp, continued=True, update_frame=True): - ps.header.stamp = stamp - vs.header.stamp = stamp - - # don't block on setpoints publishing - transform_timeout = rospy.Duration(0.1) if continued else TRANSFORM_TIMEOUT - - if isinstance(req, (srv.NavigateRequest, srv.NavigateGlobalRequest)): - global current_nav_start, current_nav_start_stamp, current_nav_finish - - if update_frame: - ps.header.frame_id = req.frame_id or LOCAL_FRAME - ps.pose.position = Point(getattr(req, 'x', 0), getattr(req, 'y', 0), req.z) - ps.pose.orientation = orientation_from_euler(0, 0, req.yaw, axes='sxyz') - current_nav_finish = tf_buffer.transform(ps, LOCAL_FRAME, transform_timeout) - - if isinstance(req, srv.NavigateGlobalRequest): - # Recalculate x and y from lat and lon - current_nav_finish.pose.position.x, current_nav_finish.pose.position.y = \ - global_to_local(req.lat, req.lon) - - if not continued: - current_nav_start = pose.pose.position - current_nav_start_stamp = stamp - - if NAVIGATE_AFTER_ARMED and not state.armed: - current_nav_start_stamp = stamp - - setpoint = get_navigate_setpoint(stamp, current_nav_start, current_nav_finish.pose.position, - current_nav_start_stamp, req.speed) - - yaw_rate_flag = math.isnan(req.yaw) - msg = pt - msg.coordinate_frame = PT.FRAME_LOCAL_NED - msg.type_mask = PT.IGNORE_VX + PT.IGNORE_VY + PT.IGNORE_VZ + \ - PT.IGNORE_AFX + PT.IGNORE_AFY + PT.IGNORE_AFZ + \ - (PT.IGNORE_YAW if yaw_rate_flag else PT.IGNORE_YAW_RATE) - msg.position = setpoint - msg.yaw = euler_from_orientation(current_nav_finish.pose.orientation, 'sxyz')[2] - msg.yaw_rate = req.yaw_rate - return position_pub, msg - - elif isinstance(req, (srv.SetPositionRequest, srv.SetPositionGlobalRequest)): - ps.header.frame_id = req.frame_id or LOCAL_FRAME - ps.pose.position = Point(getattr(req, 'x', 0), getattr(req, 'y', 0), req.z) - ps.pose.orientation = orientation_from_euler(0, 0, req.yaw) - pose_local = tf_buffer.transform(ps, LOCAL_FRAME, transform_timeout) - - if isinstance(req, srv.SetPositionGlobalRequest): - pose_local.pose.position.x, pose_local.pose.position.y = global_to_local(req.lat, req.lon) - - yaw_rate_flag = math.isnan(req.yaw) - msg = pt - msg.coordinate_frame = PT.FRAME_LOCAL_NED - msg.type_mask = PT.IGNORE_VX + PT.IGNORE_VY + PT.IGNORE_VZ + \ - PT.IGNORE_AFX + PT.IGNORE_AFY + PT.IGNORE_AFZ + \ - (PT.IGNORE_YAW if yaw_rate_flag else PT.IGNORE_YAW_RATE) - msg.position = pose_local.pose.position - msg.yaw = euler_from_orientation(pose_local.pose.orientation, 'sxyz')[2] - msg.yaw_rate = req.yaw_rate - return position_pub, msg - - elif isinstance(req, srv.SetVelocityRequest): - vs.vector = Vector3(req.vx, req.vy, req.vz) - vs.header.frame_id = req.frame_id or LOCAL_FRAME - ps.header.frame_id = req.frame_id or LOCAL_FRAME - ps.pose.orientation = orientation_from_euler(0, 0, req.yaw) - pose_local = tf_buffer.transform(ps, LOCAL_FRAME, transform_timeout) - vector_local = tf_buffer.transform(vs, LOCAL_FRAME, transform_timeout) - - yaw_rate_flag = math.isnan(req.yaw) - msg = pt - msg.coordinate_frame = PT.FRAME_LOCAL_NED - msg.type_mask = PT.IGNORE_PX + PT.IGNORE_PY + PT.IGNORE_PZ + \ - PT.IGNORE_AFX + PT.IGNORE_AFY + PT.IGNORE_AFZ + \ - (PT.IGNORE_YAW if yaw_rate_flag else PT.IGNORE_YAW_RATE) - msg.velocity = vector_local.vector - msg.yaw = euler_from_orientation(pose_local.pose.orientation, 'sxyz')[2] - msg.yaw_rate = req.yaw_rate - return position_pub, msg - - elif isinstance(req, srv.SetAttitudeRequest): - ps.header.frame_id = req.frame_id or LOCAL_FRAME - ps.pose.orientation = orientation_from_euler(req.roll, req.pitch, req.yaw) - pose_local = tf_buffer.transform(ps, LOCAL_FRAME, transform_timeout) - msg = at - msg.orientation = pose_local.pose.orientation - msg.thrust = req.thrust - msg.type_mask = AT.IGNORE_YAW_RATE + AT.IGNORE_PITCH_RATE + AT.IGNORE_ROLL_RATE - return attitude_pub, msg - - elif isinstance(req, srv.SetRatesRequest): - msg = at - msg.thrust = req.thrust - msg.type_mask = AT.IGNORE_ATTITUDE - msg.body_rate.x = req.roll_rate - msg.body_rate.y = req.pitch_rate - msg.body_rate.z = req.yaw_rate - return attitude_pub, msg - - -current_pub = None -current_msg = None -current_req = None -current_nav_start = None -current_nav_finish = None -current_nav_start_stamp = None -handle_lock = Lock() - - -def handle(req): - global current_pub, current_msg, current_req - - if not state or not state.connected: - rospy.logwarn('No connection to the FCU') - return {'message': 'No connection to the FCU'} - - if isinstance(req, (srv.NavigateRequest, srv.NavigateGlobalRequest)): - if req.speed < 0: - rospy.logwarn('Navigate speed must be positive, %s passed') - return {'message': 'Navigate speed must be positive, %s passed' % req.speed} - elif req.speed == 0: - req.speed = DEFAULT_SPEED - - if isinstance(req, (srv.NavigateRequest, srv.NavigateGlobalRequest)) and \ - (pose is None or rospy.get_rostime() - pose.header.stamp > LOCAL_POSITION_TIMEOUT): - rospy.logwarn('No local position') - return {'message': 'No local position'} - - if getattr(req, 'yaw_rate', 0) != 0 and not math.isnan(getattr(req, 'yaw')): - rospy.logwarn('Yaw value should be NaN for setting yaw rate') - return {'message': 'Yaw value should be NaN for setting yaw rate'} - - if math.isnan(getattr(req, 'yaw', 0)) and math.isnan(getattr(req, 'yaw_rate', 0)): - rospy.logwarn('Both yaw and yaw_rate cannot be NaN') - return {'message': 'Both yaw and yaw_rate cannot be NaN'} - - try: - # check frame_id existance - # (for non-blocking setpoint's publishing in get_publisher_and_message) - stamp = rospy.get_rostime() - if hasattr(req, 'frame_id'): - tf_buffer.lookup_transform(req.frame_id or LOCAL_FRAME, LOCAL_FRAME, stamp, TRANSFORM_TIMEOUT) - - with handle_lock: - current_req = req - current_pub, current_msg = get_publisher_and_message(req, stamp, False) - rospy.loginfo('Topic: %s, message: %s', current_pub.name, current_msg) - - current_msg.header.stamp = stamp - current_pub.publish(current_msg) - - if req.auto_arm: - offboard_and_arm() - else: - if state.mode != 'OFFBOARD': - return {'message': 'Copter is not in OFFBOARD mode, use auto_arm?'} - if not state.armed: - return {'message': 'Copter is not armed, use auto_arm?'} - - return {'success': True} - - except Exception as e: - rospy.logerr(str(e)) - return {'success': False, 'message': str(e)} - - -def land(req): - if not state or not state.connected: - rospy.logwarn('No connection to the FCU') - return {'message': 'No connection to the FCU'} - - rospy.loginfo('Set %s mode', LAND_MODE) - res = set_mode(custom_mode=LAND_MODE) - if not res.mode_sent: - return {'message': 'Cannot send %s mode request' % LAND_MODE} - - start = rospy.get_rostime() - while True: - if state.mode == LAND_MODE: - return {'success': True} - if rospy.get_rostime() - start > LAND_TIMEOUT: - return {'message': '%s mode request timed out' % LAND_MODE} - rospy.sleep(0.1) - - -def release(req): - global current_pub - current_pub = None - rospy.loginfo('simple_offboard: release') - return {'success': True} - - -rospy.Service('navigate', srv.Navigate, handle) -rospy.Service('navigate_global', srv.NavigateGlobal, handle) -rospy.Service('set_position', srv.SetPosition, handle) -rospy.Service('set_position_global', srv.SetPositionGlobal, handle) -rospy.Service('set_velocity', srv.SetVelocity, handle) -rospy.Service('set_attitude', srv.SetAttitude, handle) -rospy.Service('set_rates', srv.SetRates, handle) -rospy.Service('land', Trigger, land) -rospy.Service('release', Trigger, release) - - -def get_telemetry(req): - res = { - 'frame_id': req.frame_id or LOCAL_FRAME, - 'x': float('nan'), - 'y': float('nan'), - 'z': float('nan'), - 'lat': float('nan'), - 'lon': float('nan'), - 'alt': float('nan'), - 'vx': float('nan'), - 'vy': float('nan'), - 'vz': float('nan'), - 'pitch': float('nan'), - 'roll': float('nan'), - 'yaw': float('nan'), - 'pitch_rate': float('nan'), - 'roll_rate': float('nan'), - 'yaw_rate': float('nan'), - 'voltage': float('nan'), - 'cell_voltage': float('nan') - } - frame_id = req.frame_id or LOCAL_FRAME - stamp = rospy.get_rostime() - - transform_timeout = rospy.Duration(0.4) - try: - if pose: - p = tf_buffer.transform(pose, frame_id, transform_timeout) - res['x'] = p.pose.position.x - res['y'] = p.pose.position.y - res['z'] = p.pose.position.z - - # Calculate roll pitch and yaw as Tait-Bryan angles, order z-y-x - res['yaw'], res['pitch'], res['roll'] = euler_from_orientation(p.pose.orientation, axes='rzyx') - except: - pass - - if velocity: - try: - v = Vector3Stamped() - v.header.stamp = velocity.header.stamp - v.header.frame_id = velocity.header.frame_id - v.vector = velocity.twist.linear - linear = tf_buffer.transform(v, frame_id, transform_timeout) - res['vx'] = linear.vector.x - res['vy'] = linear.vector.y - res['vz'] = linear.vector.z - except: - pass - - res['yaw_rate'] = velocity.twist.angular.z - res['pitch_rate'] = velocity.twist.angular.y - res['roll_rate'] = velocity.twist.angular.x - - if global_position and stamp - global_position.header.stamp < rospy.Duration(5): - res['lat'] = global_position.latitude - res['lon'] = global_position.longitude - res['alt'] = global_position.altitude - - if state: - res['connected'] = state.connected - res['armed'] = state.armed - res['mode'] = state.mode - - if battery: - res['voltage'] = battery.voltage - try: - res['cell_voltage'] = battery.cell_voltage[0] - except: - pass - - return res - - -rospy.Service('get_telemetry', srv.GetTelemetry, get_telemetry) - - -rospy.loginfo('simple_offboard inited') - - -def start_loop(): - global current_pub, current_msg, current_req - r = rospy.Rate(SETPOINT_RATE) - - while not rospy.is_shutdown(): - with handle_lock: - if current_pub is not None: - try: - stamp = rospy.get_rostime() - - if getattr(current_req, 'update_frame', False) or \ - isinstance(current_req, (srv.NavigateRequest, srv.NavigateGlobalRequest)): - current_pub, current_msg = get_publisher_and_message(current_req, stamp, True, - getattr(current_req, 'update_frame', False)) - - except Exception as e: - rospy.logwarn_throttle(10, str(e)) - - current_msg.header.stamp = stamp - current_pub.publish(current_msg) - - # For monitoring - if isinstance(current_msg, PositionTarget): - p = PoseStamped() - p.header.frame_id = LOCAL_FRAME - p.header.stamp = stamp - p.pose.position = current_msg.position - p.pose.orientation = orientation_from_euler(0, 0, current_msg.yaw) - target_pub.publish(p) - - r.sleep() - - -start_loop() diff --git a/clever/src/util.py b/clever/src/util.py deleted file mode 100644 index 275fe009..00000000 --- a/clever/src/util.py +++ /dev/null @@ -1,28 +0,0 @@ -from geometry_msgs.msg import Quaternion, Vector3, Point -import tf.transformations as t - - -def orientation_from_quaternion(q): - return Quaternion(*q) - - -def orientation_from_euler(roll, pitch, yaw, axes='rzyx'): - q = t.quaternion_from_euler(roll, pitch, yaw, axes) - return orientation_from_quaternion(q) - - -def quaternion_from_orientation(o): - return o.x, o.y, o.z, o.w - - -def euler_from_orientation(o, axes='rzyx'): - q = quaternion_from_orientation(o) - return t.euler_from_quaternion(q, axes) - - -def vector3_from_point(p): - return Vector3(p.x, p.y, p.z) - - -def point_from_vector3(v): - return Point(v.x, v.y, v.z) diff --git a/clever/src/vl53l1x.py b/clever/src/vl53l1x.py deleted file mode 100755 index 346c382b..00000000 --- a/clever/src/vl53l1x.py +++ /dev/null @@ -1,38 +0,0 @@ -#!/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 diff --git a/clever/srv/Navigate.srv b/clever/srv/Navigate.srv index 6605e900..f607a437 100644 --- a/clever/srv/Navigate.srv +++ b/clever/srv/Navigate.srv @@ -5,7 +5,6 @@ float32 yaw float32 yaw_rate float32 speed string frame_id -bool update_frame bool auto_arm --- bool success diff --git a/clever/srv/NavigateGlobal.srv b/clever/srv/NavigateGlobal.srv index 52305d29..69fbc444 100644 --- a/clever/srv/NavigateGlobal.srv +++ b/clever/srv/NavigateGlobal.srv @@ -1,11 +1,10 @@ -float32 lat -float32 lon +float64 lat +float64 lon float32 z float32 yaw float32 yaw_rate float32 speed string frame_id -bool update_frame bool auto_arm --- bool success diff --git a/clever/srv/SetAttitude.srv b/clever/srv/SetAttitude.srv index 05ba13db..b41c1072 100644 --- a/clever/srv/SetAttitude.srv +++ b/clever/srv/SetAttitude.srv @@ -3,7 +3,6 @@ float32 roll float32 yaw float32 thrust string frame_id -bool update_frame bool auto_arm --- bool success diff --git a/clever/srv/SetPosition.srv b/clever/srv/SetPosition.srv index 0a4288e3..20c73f05 100644 --- a/clever/srv/SetPosition.srv +++ b/clever/srv/SetPosition.srv @@ -4,7 +4,6 @@ float32 z float32 yaw float32 yaw_rate string frame_id -bool update_frame bool auto_arm --- bool success diff --git a/clever/srv/SetPositionGlobal.srv b/clever/srv/SetPositionGlobal.srv deleted file mode 100644 index 29d84f8c..00000000 --- a/clever/srv/SetPositionGlobal.srv +++ /dev/null @@ -1,11 +0,0 @@ -float32 lat -float32 lon -float32 z -float32 yaw -float32 yaw_rate -string frame_id -bool update_frame -bool auto_arm ---- -bool success -string message diff --git a/clever/srv/SetVelocity.srv b/clever/srv/SetVelocity.srv index 4251d021..25e174ac 100644 --- a/clever/srv/SetVelocity.srv +++ b/clever/srv/SetVelocity.srv @@ -4,7 +4,6 @@ float32 vz float32 yaw float32 yaw_rate string frame_id -bool update_frame bool auto_arm --- bool success diff --git a/clever/www/docs b/clever/www/docs new file mode 120000 index 00000000..d7fb46af --- /dev/null +++ b/clever/www/docs @@ -0,0 +1 @@ +../../_book/ \ No newline at end of file diff --git a/builder/assets/index.html b/clever/www/index.html similarity index 56% rename from builder/assets/index.html rename to clever/www/index.html index 422d1fbf..029377df 100644 --- a/builder/assets/index.html +++ b/clever/www/index.html @@ -1,11 +1,10 @@

CLEVER Drone Kit Tools

+ + + + 3D visualization + + +
+ + + diff --git a/docs/assets/Mikhail.jpg b/docs/assets/Mikhail.jpg new file mode 100644 index 00000000..91d334a8 Binary files /dev/null and b/docs/assets/Mikhail.jpg differ diff --git a/docs/assets/Mikhail_output.jpg b/docs/assets/Mikhail_output.jpg new file mode 100644 index 00000000..e51cf6c0 Binary files /dev/null and b/docs/assets/Mikhail_output.jpg differ diff --git a/docs/assets/Timofey.jpg b/docs/assets/Timofey.jpg new file mode 100644 index 00000000..ad0aa204 Binary files /dev/null and b/docs/assets/Timofey.jpg differ diff --git a/docs/assets/Timofey_output.jpg b/docs/assets/Timofey_output.jpg new file mode 100644 index 00000000..9747cd03 Binary files /dev/null and b/docs/assets/Timofey_output.jpg differ diff --git a/docs/assets/calibration.jpg b/docs/assets/calibration.jpg new file mode 100644 index 00000000..dc1978dc Binary files /dev/null and b/docs/assets/calibration.jpg differ diff --git a/docs/assets/calibresult.jpg b/docs/assets/calibresult.jpg new file mode 100644 index 00000000..06123ee2 Binary files /dev/null and b/docs/assets/calibresult.jpg differ diff --git a/docs/assets/calibresult1.jpg b/docs/assets/calibresult1.jpg new file mode 100644 index 00000000..815d28dd Binary files /dev/null and b/docs/assets/calibresult1.jpg differ diff --git a/docs/assets/camera_option_1_clever.jpg b/docs/assets/camera_option_1_clever.jpg index 43797a8d..3e8a67fd 100644 Binary files a/docs/assets/camera_option_1_clever.jpg and b/docs/assets/camera_option_1_clever.jpg differ diff --git a/docs/assets/camera_option_2_clever.jpg b/docs/assets/camera_option_2_clever.jpg index d2e2ee64..cbe48c7c 100644 Binary files a/docs/assets/camera_option_2_clever.jpg and b/docs/assets/camera_option_2_clever.jpg differ diff --git a/docs/assets/camera_option_3_clever.jpg b/docs/assets/camera_option_3_clever.jpg index 3d6595ee..3f127d87 100644 Binary files a/docs/assets/camera_option_3_clever.jpg and b/docs/assets/camera_option_3_clever.jpg differ diff --git a/docs/assets/camera_option_4_clever.jpg b/docs/assets/camera_option_4_clever.jpg index 1715a4dd..a9379f83 100644 Binary files a/docs/assets/camera_option_4_clever.jpg and b/docs/assets/camera_option_4_clever.jpg differ diff --git a/docs/assets/chessboard.jpg b/docs/assets/chessboard.jpg new file mode 100644 index 00000000..d175b005 Binary files /dev/null and b/docs/assets/chessboard.jpg differ diff --git a/docs/assets/google_play.png b/docs/assets/google_play.png new file mode 100644 index 00000000..2039d636 Binary files /dev/null and b/docs/assets/google_play.png differ diff --git a/docs/assets/img1.jpg b/docs/assets/img1.jpg new file mode 100644 index 00000000..ae5df1d1 Binary files /dev/null and b/docs/assets/img1.jpg differ diff --git a/docs/assets/img2.jpg b/docs/assets/img2.jpg new file mode 100644 index 00000000..236705d2 Binary files /dev/null and b/docs/assets/img2.jpg differ diff --git a/docs/assets/misha_calib.jpg b/docs/assets/misha_calib.jpg new file mode 100644 index 00000000..08a6c0c4 Binary files /dev/null and b/docs/assets/misha_calib.jpg differ diff --git a/docs/assets/pty1.jpg b/docs/assets/pty1.jpg new file mode 100644 index 00000000..6584701f Binary files /dev/null and b/docs/assets/pty1.jpg differ diff --git a/docs/assets/pty2.jpg b/docs/assets/pty2.jpg new file mode 100644 index 00000000..4e60b3dd Binary files /dev/null and b/docs/assets/pty2.jpg differ diff --git a/docs/assets/pty3.jpg b/docs/assets/pty3.jpg new file mode 100644 index 00000000..d727ec6a Binary files /dev/null and b/docs/assets/pty3.jpg differ diff --git a/docs/assets/rqt_image_view.jpg b/docs/assets/rqt_image_view.jpg new file mode 100644 index 00000000..8d800b92 Binary files /dev/null and b/docs/assets/rqt_image_view.jpg differ diff --git a/docs/assets/rqt_image_view_dyn_rec.jpg b/docs/assets/rqt_image_view_dyn_rec.jpg new file mode 100644 index 00000000..46eec556 Binary files /dev/null and b/docs/assets/rqt_image_view_dyn_rec.jpg differ diff --git a/docs/assets/screen.jpg b/docs/assets/screen.jpg new file mode 100644 index 00000000..b2898577 Binary files /dev/null and b/docs/assets/screen.jpg differ diff --git a/docs/assets/tim_calib.jpg b/docs/assets/tim_calib.jpg new file mode 100644 index 00000000..c84b87f2 Binary files /dev/null and b/docs/assets/tim_calib.jpg differ diff --git a/docs/assets/wcp1.png b/docs/assets/wcp1.png new file mode 100644 index 00000000..1aa8c4ed Binary files /dev/null and b/docs/assets/wcp1.png differ diff --git a/docs/assets/wcp2.jpg b/docs/assets/wcp2.jpg new file mode 100644 index 00000000..98c781a5 Binary files /dev/null and b/docs/assets/wcp2.jpg differ diff --git a/docs/assets/web_interface.png b/docs/assets/web_interface.png new file mode 100644 index 00000000..244a1c3f Binary files /dev/null and b/docs/assets/web_interface.png differ diff --git a/docs/en/4in1.md b/docs/en/4in1.md index 9efbba56..09ace03a 100644 --- a/docs/en/4in1.md +++ b/docs/en/4in1.md @@ -6,7 +6,7 @@ Appropriate phase wires and their control signal (Fig. 1b) are marked with the s For example, orange color -> bottom-right motor -> S1 - orange wire. -## PixRacer flight controller pin-out +## Pixracer flight controller pin-out Fig. 2a shows the pin-out of the terminal strip: @@ -15,7 +15,7 @@ Fig. 2a shows the pin-out of the terminal strip: * 1, 2, 3, 4 are ports for connecting ESCs. * 1, 2 are ports for expanding the output PWM signal (can be setup in QGroundControl, can also can be used to control the hexacopter). - Fig. 2b shows motor numbering of the PixRacer flight controller. + Fig. 2b shows motor numbering of the Pixracer flight controller. * The arrow is the flight controller orientation. * Black M3, M4 are the motors that rotate clockwise. @@ -23,8 +23,8 @@ Fig. 2a shows the pin-out of the terminal strip: ## Picture of the connection, based on the current orientation of the 4 in 1 ESC board -Using Fig. 1a, 1b, 2a, 2b, map its own control signal to each motor, and connect in accordance with the PixRacer motor numbering order. +Using Fig. 1a, 1b, 2a, 2b, map its own control signal to each motor, and connect in accordance with the Pixracer motor numbering order. For example, motor M3 that rotates counter-clockwise (top left corner) is controlled by signal S4 (green wire). It is connected to port 3. -![Connecting 4 in 1 ESCs](../assets/cl3_connectionESC4in1.jpg) \ No newline at end of file +![Connecting 4 in 1 ESCs](../assets/cl3_connectionESC4in1.jpg) diff --git a/docs/en/README.md b/docs/en/README.md index 99753213..c11e5f76 100644 --- a/docs/en/README.md +++ b/docs/en/README.md @@ -24,11 +24,11 @@ The image includes: * Raspbian Stretch * ROS Kinetic -* Configured [networking] (network.md) +* Configured [networking](network.md) * OpenCV * mavros * A software set for working with Clever [API description](simple_offboard.md) for autonomous flights. -The source code of the collector of the image and only can be found at [GitHub](https://github.com/CopterExpress/clever). \ No newline at end of file +The source code of the collector of the image and only can be found at [GitHub](https://github.com/CopterExpress/clever). diff --git a/docs/en/assemble_2.md b/docs/en/assemble_2.md index de66dd4e..f094a9ed 100644 --- a/docs/en/assemble_2.md +++ b/docs/en/assemble_2.md @@ -26,7 +26,7 @@ Clever 2 construction kit assembly instruction * EFEST Luc V4 Li-lon Charger ×1. * Regulators protective case ×4. * Legs attachment ×8. -* PixHawk flight controller ×1. +* Pixhawk flight controller ×1. * FlySky i6 radio receiver×1. * FlySky i6 radio transmitter ×1. * EFEST LUC V4 Charger ×1. @@ -292,7 +292,7 @@ IMPORTANT NOTE about polarity ![Connecting the radio receiver power](../assets/connectingRadio.jpg) -[Radio equipment troubleshooting manual ](radioerrors.md) +[Radio equipment troubleshooting manual](radioerrors.md) ### Checking the motors rotation direction @@ -322,13 +322,13 @@ IMPORTANT NOTE about polarity ![Turn the assembly upside down](../assets/topPreview.png) -#### Installation of the PixHawk flight controller +#### Installation of the Pixhawk flight controller 1. Stick the two-sided adhesive tape in the corners of the flight controller. ![Flight controller](../assets/pixhawk.png) - > **IMPORTANT** When the motors rotate, vibrations occur, which affect sensors of the PixHawk flight controller. To avoid this effect, the number of double-sided tape layers + > **IMPORTANT** When the motors rotate, vibrations occur, which affect sensors of the Pixhawk flight controller. To avoid this effect, the number of double-sided tape layers should be increased up to 4 – 5. 2. Install the flight controller in the center of the frame. ![Flight controller](../assets/topviewpixhawk.jpg) - > **IMPORTANT** The arrows on the frame and PixHawk should point in the same direction + > **IMPORTANT** The arrows on the frame and Pixhawk should point in the same direction #### Connecting the flight controller according to the circuit diagram @@ -385,4 +385,4 @@ The copter is ready for configuration! 2. When connecting (disconnecting) batteries, hold only the connectors, never pull or tug the wires. 3. If you see open connectors, violation of insulation or battery compartment integrity, do not touch it, and immediately inform the instructor. -See article [safety precautions when soldering and during copter flight operation](safety.md) \ No newline at end of file +See article [safety precautions when soldering and during copter flight operation](safety.md) diff --git a/docs/en/assemble_3.md b/docs/en/assemble_3.md index 80a9e145..9e963a22 100644 --- a/docs/en/assemble_3.md +++ b/docs/en/assemble_3.md @@ -95,7 +95,7 @@ TODO ![Installation of the BEC voltage Converter](../assets/cl3_mountBEC.JPG) -## Installation of the 4 in 1 ESC board and the PDB power-board +## Installation of the 4 in 1 ESC board and the PDB power-board 1. Install the 4 in 1 ESC circuit-board as shown in the picture. @@ -188,7 +188,7 @@ article [remote faults](radioerrors.md). ![Installation of Raspberry Pi Model B](../assets/cl3_mountRaspberryPi.JPG) -## Installation of Arduino and the FlySky radio receiver +## Installation of Arduino and the FlySky radio receiver 1. Mount the pins of the Arduino Nano micro-controller using soldering. 2. Install the micro-controller into a special mount, and attach to the lower deck using М3х16 screws (4 pcs). diff --git a/docs/en/connectortypes.md b/docs/en/connectortypes.md index d5ac0dcd..2d4f1506 100644 --- a/docs/en/connectortypes.md +++ b/docs/en/connectortypes.md @@ -25,4 +25,4 @@ This connector may be used together with a buzzer (beeper) for monitoring batter There is a great variety of Gold bullet pin connectors. Connectors of this type may have different diameters and size. The most widespread connectors are those with the diameter of 2 mm, 3 mm, and 4 mm. They are often used for solderless connections on PDB and motors. -Banana \ No newline at end of file +Banana diff --git a/docs/en/gloss.md b/docs/en/gloss.md index 83a184a6..c21761b1 100644 --- a/docs/en/gloss.md +++ b/docs/en/gloss.md @@ -19,13 +19,13 @@ Pixhawk, Ardupilot, Naze32, CC3D. An electric motor that rotates propellers of the multicopter. Usually, brushless motors are used. These motors are connected to ESC. -## ESC / motor controller / "regul" +## ESC / motor controller An Electronic Speed Controller. A specialized circuit-board that controls the speed of the brushless motor. It is controlled by a flight controller using pulse width modulation (PWM). ESC has the firmware that determines the characteristics of its operation. -## Remote control / radio control equipment/ "appa" +## Remote control / radio control equipment A radio-operated quadcopter remote control. Operation of the remote control requires connecting a receiver to the flight controller. @@ -67,4 +67,4 @@ A library that is a link between the aircraft operating using the MAVLink protoc ## UART -A serial asynchronous data transfer interface used in many devices. For example, GPS antennas, Wi-Fi routers, or Pixhawk. \ No newline at end of file +A serial asynchronous data transfer interface used in many devices. For example, GPS antennas, Wi-Fi routers, or Pixhawk. diff --git a/docs/en/mavros.md b/docs/en/mavros.md index a02af712..fc437f95 100644 --- a/docs/en/mavros.md +++ b/docs/en/mavros.md @@ -2,7 +2,7 @@ Main documentation: [http://wiki.ros.org/mavros](http://wiki.ros.org/mavros) -MAVROS \(MAVLink + ROS\) is a package for ROS that provides the possibility of controlling drones via the [MAVLink] protocol (mavlink.md). MAVROS supports flight stacks PX4 and APM. Communication is established via UART, USB, TCP or UDP. +MAVROS \(MAVLink + ROS\) is a package for ROS that provides the possibility of controlling drones via the [MAVLink](mavlink.md) protocol. MAVROS supports flight stacks PX4 and APM. Communication is established via UART, USB, TCP or UDP. MAVROS subscribes to certain ROS topics while waiting for commands, publishes telemetry to other topics, and provides services. diff --git a/docs/en/modes.md b/docs/en/modes.md index dde20c5d..6b63c75c 100644 --- a/docs/en/modes.md +++ b/docs/en/modes.md @@ -30,7 +30,7 @@ In manual mode, the pilot controls the drone directly. GPS, computer vision data In the automatic flight mode, the quadcopter ignores the control signals from the transmitter. -* **AUTO.MISSION** – PX4 completes the mission pre-loaded into the drone (the mission is downloaded using the QGroundControl, or from [MAVLink] (mavlink.md) using [MAVROS](mavros.md). +* **AUTO.MISSION** – PX4 completes the mission pre-loaded into the drone (the mission is downloaded using the QGroundControl, or from [MAVLink](mavlink.md) using [MAVROS](mavros.md). * **AUTO.RTL** – the copter automatically returns to the takeoff point. * **AUTO.LAND** – the copter lands automatically. @@ -44,4 +44,4 @@ The main used MAVLink packages are: * [SET_POSITION_TARGET_LOCAL_NED](https://mavlink.io/en/messages/common.html#SET_POSITION_TARGET_LOCAL_NED) * [SET_ATTITUDE_TARGET](https://mavlink.io/en/messages/common.html#SET_ATTITUDE_TARGET) -See: [autonomous flying the quadcopter in the OFFBOARD mode](simple_offboard.md). \ No newline at end of file +See: [autonomous flying the quadcopter in the OFFBOARD mode](simple_offboard.md). diff --git a/docs/en/network.md b/docs/en/network.md index fca3c7fe..89a00ffb 100644 --- a/docs/en/network.md +++ b/docs/en/network.md @@ -2,7 +2,9 @@ The Raspberry Pi Wi-Fi adapter of has two main operating modes: -1. **Client mode** – RPi connects to an existing Wi-Fi network. + +1. **Client mode** – RPi connects to an existing Wi-Fi network. + 2. **Access point mode** – RPi creates a Wi-Fi network that you can connect to. When using [the RPi image](microsd_images.md), the Wi-Fi adapter works in the [access point mode] by default (Wi-Fi.md). diff --git a/docs/en/safety.md b/docs/en/safety.md index ab91bc72..7f468ca9 100644 --- a/docs/en/safety.md +++ b/docs/en/safety.md @@ -35,7 +35,7 @@ If anyone says the opposite – don't believe him, he is trying to misguide you: Flying ------ -### Safety during preparation to flying +### Safety during pre-flight preparations * Make sure that the Li-ion batteries are charged. * Make sure the batteries in the control equipment are charged. @@ -88,4 +88,4 @@ After a scheduled landing, do the following: 1. Disarm (Move the left stick left-down for 3 seconds) 2. Disconnect the Li-ion battery on the copter. -3. Turn off the remote. \ No newline at end of file +3. Turn off the remote. diff --git a/docs/en/setup.md b/docs/en/setup.md index 0c7071b2..134c61c8 100644 --- a/docs/en/setup.md +++ b/docs/en/setup.md @@ -146,7 +146,6 @@ Start the calibration procedure ![Gyroscope calibration](../assets/calibrategyro.jpg) - > *Warning* During calibration, the drone should remain in position, be stable, etc. ## Flight modes diff --git a/docs/en/simple_offboard.md b/docs/en/simple_offboard.md index c72035aa..c8b870f2 100644 --- a/docs/en/simple_offboard.md +++ b/docs/en/simple_offboard.md @@ -269,7 +269,6 @@ set_velocity(vx=0.4, vy=0.0, vz=0, yaw=float('nan'), yaw_rate=0.4, frame_id='bod Setting pitch, roll, yaw and throttle level (approximate analogue to control in [the `STABILIZED` mode](modes.md)). This service may be used for lower level monitoring of the copter behavior or controlling the copter, if no reliable data about its position are available. - Parameters: * `pitch`, `roll`, `yaw` – required pitch, roll, and yaw angle *(radians)*; diff --git a/docs/en/web_video_server.md b/docs/en/web_video_server.md index 11e582f1..68e36e3d 100644 --- a/docs/en/web_video_server.md +++ b/docs/en/web_video_server.md @@ -1,6 +1,6 @@ # Viewing images from cameras -To view images from cameras (or other Ros topics), you can use [rviz](rviz.md), rqt, or watch them in a browser using web\_video\_server. +To view images from cameras (or other ROS topics), you can use [rviz](rviz.md), rqt, or watch them in a browser using web\_video\_server. See read more about [using rqt](rviz.md). diff --git a/docs/en/zap.md b/docs/en/zap.md index 8863015b..88f7f3d7 100644 --- a/docs/en/zap.md +++ b/docs/en/zap.md @@ -21,4 +21,4 @@ Blanching a wire means doing the following: 3. Apply flux to the twisted stripped wires 4. Apply a layer of solder. -![Blanching wires](../assets/zap.jpg) \ No newline at end of file +![Blanching wires](../assets/zap.jpg) diff --git a/docs/ru/SUMMARY.md b/docs/ru/SUMMARY.md index 57fb5dbe..fb003eb7 100644 --- a/docs/ru/SUMMARY.md +++ b/docs/ru/SUMMARY.md @@ -50,13 +50,16 @@ * [3G-модем](3g.md) * Проекты на базе Клевера * [Шаровая защита коптера](shield.md) + * [Распознавание лиц](face_recognition.md) + * [Пульт на Андроид](android.md) * [CopterHack-2018](copterhack2018.md) * [CopterHack-2017](copterhack2017.md) * Дополнительные материалы * [Вклад в Клевер](contributing.md) - * [Прошивка ESC контроллеров с помощью Arduino](esc_firmware.md) + * [Прошивка ESC контроллеров](esc_firmware.md) * [Протокол MAVLink](mavlink.md) * [Работа с логами PX4](flight_logs.md) + * [Калибровка камеры](calibration.md) * Учебник * [Теория и видеоуроки](lessons.md) * [Учебно-методическое пособие](metod.md) diff --git a/docs/ru/android.md b/docs/ru/android.md new file mode 100644 index 00000000..cf4821c4 --- /dev/null +++ b/docs/ru/android.md @@ -0,0 +1,142 @@ + +# Пульт на Android + +Все владельцы мобильных устройств фирмы *Apple* ещё морозным январем 2018го обзавелись приятным приложением под *iOS* для пилотирования квадрокоптеров с помощью **WiFi**. И вот, спустя год вышло такое же приложение но уже для другой операционной системы. Актуальную версию вы можете скачать [**тут**](https://vk.com/away.php?to=https%3A%2F%2Fplay.google.com%2Fstore%2Fapps%2Fdetails%3Fid%3Dexpress.copter.cleverrc&cc_key=) . + +## Введение + +В данной статье я расскажу вам о том, как можно написать свой или доработать уже имеющийся пульт для Андроид своими руками. Для работы будем использовать модный язык *Kotlin*, а в качестве среды разработки возьмем *Android Studio*. Для тех кто ни разу ей не пользовался рекомендую к ознакомлению следующие [*материалы*](https://www.google.com/search?ei=xQxDXMH0C8OOmgW4mYigDQ&q=%D0%A7%D1%82%D0%BE+%D0%B4%D0%B5%D0%BB%D0%B0%D1%82%D1%8C+%D0%B5%D1%81%D0%BB%D0%B8+%D1%8F+%D0%BD%D0%B5+%D1%83%D0%BC%D0%B5%D1%8E+%D0%BF%D0%B8%D1%81%D0%B0%D1%82%D1%8C+%D0%BF%D0%BE%D0%B4+%D0%B0%D0%BD%D0%B4%D1%80%D0%BE%D0%B8%D0%B4%3F&oq=%D0%A7%D1%82%D0%BE+%D0%B4%D0%B5%D0%BB%D0%B0%D1%82%D1%8C+%D0%B5%D1%81%D0%BB%D0%B8+%D1%8F+%D0%BD%D0%B5+%D1%83%D0%BC%D0%B5%D1%8E+%D0%BF%D0%B8%D1%81%D0%B0%D1%82%D1%8C+%D0%BF%D0%BE%D0%B4+%D0%B0%D0%BD%D0%B4%D1%80%D0%BE%D0%B8%D0%B4%3F&gs_l=psy-ab.3...4413.17423..17726...9.0..2.442.4577.45j5j1j0j1....2..0....1..gws-wiz.....6..0i71j35i39j0i131j0j0i67j0i131i67j0i22i30j33i22i29i30j33i21j33i160.0bZz-WGxoHY). Весь код приложения можно найти [**тут**](https://github.com/Tennessium/android). Если вы хотите сразу получить приложение с целью дальнейшей доработки, выполните следующую команду: + +```Bash +git clone https://github.com/Tennessium/android +``` + +Однако чтобы вы смогли полностью понять устройство приложения, я расскажу вам о каждом этапе создания проекта, как если бы вы делали его с нуля. + +## Обертка + +Начнем с самого простого - внешнего вида нашего приложения. На [**гитхабе**](https://github.com/CopterExpress/clever/tree/master/apps/android/app/src/main/assets) вы можете найти *HTML*, *CSS* и *JavaScript* файлы, это и есть веб страница с которой будет происходить управление коптером. Чтобы эта страница отображалась у нас в приложении надо: + +1. Создать папку **assets** в главной папке приложения **app** + +2. Добавить в нее файлы все файлы [отсюда](https://github.com/CopterExpress/clever/tree/master/apps/android/app/src/main/assets) + +Если вы дошли до этого этапа то у вас уже есть необходимая веб страница, поздравляю! Теперь нам надо её как-то отобразить в приложении. Для этого в классе вашего *activity* в методе **onCreate** необходимо написать следующий код: + +```Kotlin +main_web.loadUrl("file:///android_asset/index.html") +``` + +Где *main_web* - id вашего *WebView*, который должен находится в *xml* файле выбранного вами *activity*. + +К сожалению, пульт для управления коптером требует всего экрана устройства, а элементы интерфейса системы мешают полноценному использованию программы. Для этого надо в начале метода **onCreate** вызвать следующую функцию: + +```Kotlin +private fun fullScreenCall() { + window.setFlags(WindowManager.LayoutParams.FLAG_FULLSCREEN, WindowManager.LayoutParams.FLAG_FULLSCREEN) + if (Build.VERSION.SDK_INT < 19) { + val v = this.window.decorView + v.systemUiVisibility = View.GONE + } else { + //for higher api versions. + val decorView = window.decorView + val uiOptions = View.SYSTEM_UI_FLAG_HIDE_NAVIGATION or View.SYSTEM_UI_FLAG_IMMERSIVE_STICKY + decorView.systemUiVisibility = uiOptions + } +} +``` + +Данная функция позволяет избавиться от элементов интерфейса системы. Идем дальше. + +Вот так выглядит пульт на этом этапе: + + + +Если вы запустите приложение, то заметите что стики не работают. Это происходит по тому, что на нашей странице отключен *JavaScript*. чтобы его включить надо прописать следующее: + +```Kotlin + main_web.settings.apply { + domStorageEnabled = true + javaScriptEnabled = true + loadWithOverviewMode = true + useWideViewPort = true + setSupportZoom(false) +} +``` + +Этим куском кода мы разрешаем странице использовать *JavaScript* и заодно готовимся к следующему этапу - **логике**. + +## Прием данных с веб страницы + +Чтобы телефон мог принимать данные с *HTML страницы*, надо создать класс для взаимодействия с веб интерфейсом + +```Kotlin +class WebAppInterface(c: Context) { + @JavascriptInterface + public fun postMessage(message: String) { + val data = JSONObject(message) + send("255.255.255.255", 35602, pack( + data.getInt("x").toShort(), + data.getInt("y").toShort(), + data.getInt("z").toShort(), + data.getInt("r").toShort())) + } +} +``` + +Данный класс будет получать сообщение с веб страницы отправленное методом *postMessage*, где аргумент *message* - сообщение от страницы. + +Теперь надо связать классы **WebAppInterface** и **MainActivity**. Для этого надо всего лишь добавить одну строку в метод **onCreate**: + +```Kotlin +main_web.addJavascriptInterface(WebAppInterface(this), "appInterface") +``` + +## Отправка данных на коптер + +**Важно!** +Для любой работы с интернетом на платформе *Android* в файле **AndroidManifest.xml** внутри тега *manifest* необходимо добавить такую строку: + +```XML + +``` + +Она дает вашему приложению доступ в интернет и возможность передавать данные по средствам **WiFi**. А как это делать, мы с вами сейчас и узнаем. Идём дальше! + +Вы наверное заметили функцию *send* в классе **WebAppInterface**. Именно она отправляет данные на коптер. Давайте объявим ее **вне классов**: + +```Kotlin +fun send(host: String, port: Int, data: ByteArray, senderPort: Int = 0): Boolean { + var ret = false + var socket: DatagramSocket? = null + try { + socket = DatagramSocket(senderPort) + val address = InetAddress.getByName(host) + val packet = DatagramPacket(data, data.size, address, port) + socket.send(packet) + ret = true + } catch (e: Exception) { + e.printStackTrace() + } finally { + socket?.close() + } + return ret +} +``` + +Данная функция отправляет данные при помощи [*протокола пользовательских датаграмм*](https://www.google.com/search?q=udp+%D0%BF%D1%80%D0%BE%D1%82%D0%BE%D0%BA%D0%BE%D0%BB&oq=udp+&aqs=chrome.0.69i59j69i57j35i39j0l3.1434j1j7&sourceid=chrome&ie=UTF-8) на коптер. Программа отправляет **байты**, поэтому неплохо бы было объявить функцию для создания массива **байтов** из четырех переменных: + +```Kotlin +fun pack(x: Short, y: Short, z: Short, r: Short): ByteArray { + val pump_on_buf: ByteBuffer = ByteBuffer.allocate(8) + pump_on_buf.putShort(r) + pump_on_buf.putShort(z) + pump_on_buf.putShort(y) + pump_on_buf.putShort(x) + return pump_on_buf.array().reversedArray() +} +``` + +## Итог + +Теперь ваше приложение имеет полный функционал аналога под **iOS**. Можете кастомизировать его как пожелаете. По любым вопросам о приложении можете обращаться в Телеграм @Tenessinum. diff --git a/docs/ru/arduino.md b/docs/ru/arduino.md index 0150ec60..b5837219 100644 --- a/docs/ru/arduino.md +++ b/docs/ru/arduino.md @@ -110,7 +110,7 @@ void setup() nav_req.x = 0; nav_req.y = 0; nav_req.z = 2; - nav_req.frame_id = "fcu_horiz"; + nav_req.frame_id = "body"; nav_req.speed = 0.5; navigate.call(nav_req, nav_res); @@ -128,7 +128,7 @@ void setup() nav_req.x = 3; nav_req.y = 0; nav_req.z = 0; - nav_req.frame_id = "fcu_horiz"; + nav_req.frame_id = "body"; nav_req.speed = 0.8; navigate.call(nav_req, nav_res); @@ -145,7 +145,6 @@ void setup() nav_req.y = 0; nav_req.z = 2; nav_req.frame_id = "aruco_map"; - nav_req.update_frame = true; nav_req.speed = 0.8; navigate.call(nav_req, nav_res); diff --git a/docs/ru/aruco.md b/docs/ru/aruco.md index 60c0d05a..4242f006 100644 --- a/docs/ru/aruco.md +++ b/docs/ru/aruco.md @@ -1,5 +1,7 @@ # Навигация с использованием ArUco-маркеров +> **Note** Документация для версий [образа](microsd_images.md), начиная с **0.15**. Для более ранних версий см. [документацию для версии **0.14**](https://github.com/CopterExpress/clever/blob/v0.14/docs/ru/aruco.md). + [ArUco-маркеры](https://docs.opencv.org/3.2.0/d5/dae/tutorial_aruco_detection.html) — это популярная технология для позиционирования роботехнических систем с использованием компьютерного зрения. @@ -141,12 +143,12 @@ _Примечание_: указанное выше определение пр ```python # Вначале необходимо взлететь, чтобы коптер увидел карту меток # и появился фрейм aruco_map: -navigate(0, 0, 2, frame_id='fcu_horiz', speed=0.5, auto_arm=True) # взлет на 2 метра +navigate(0, 0, 2, frame_id='body', speed=0.5, auto_arm=True) # взлет на 2 метра time.sleep(5) # Полет в координату 2:2 маркерного поля, высота 2 метра -navigate(2, 2, 2, speed=1, frame_id='aruco_map', update_frame=True) # полет в координату 2:2, высота 3 метра +navigate(2, 2, 2, speed=1, frame_id='aruco_map') # полет в координату 2:2, высота 3 метра ``` См. [другие функции](simple_offboard.md) simple_offboard. @@ -160,7 +162,7 @@ navigate(2, 2, 2, speed=1, frame_id='aruco_map', update_frame=True) # поле Чтобы задавать карту маркеров в "перевернутой" системе координат, необходимо изменить параметр `aruco_orientation` в файле `~/catkin_ws/src/clever/clever/aruco.launch`: ```xml - + ``` При задании вышеуказанного параметра фрейм aruco\_map также окажется "перевернутым". Таким образом, для полета на высоту 2 метра ниже потолка, аргумент `z` нужно устанавливать в 2: diff --git a/docs/ru/calibration.md b/docs/ru/calibration.md new file mode 100644 index 00000000..887f6fec --- /dev/null +++ b/docs/ru/calibration.md @@ -0,0 +1,227 @@ +# Калибровка камеры + +Компьютерное зрение получает все более широкое распространение. Зачастую, алгоритмы компьютерного зрения работают неточно, получая искаженное изображение с камеры, что особенно характерно для fisheye-камер. + +![img](../assets/img1.jpg) + +> Изображение "скруглено" ближе к краям. + +Какой-либо алгоритм компьютерного зрения будет воспринимать информацию с этой картинки неправильно. Для устранения подобных искажений камера, получающая изображения, должна быть откалибрована в соответствии со своими особенностями. + +## Установка скрипта + +Для начала, необходимо установить необходимые библиотеки: + +``` +pip install numpy +pip install opencv-python +pip install glob +pip install pyyaml +pip install urllib.request +``` + +Затем скачиваем скрипт из репозитория: + +```bash +git clone https://github.com/tinderad/clever_cam_calibration.git +``` + +Переходим в скачанную папку и устанавливаем скрипт: + +```bash +cd clever_cam_calibration +sudo python setup.py build +sudo python setup.py install +``` + +Если вы используете Windows, тогда скачайте архив из [репозитория](https://github.com/tinderad/clever_cam_calibration/archive/master.zip), распакуйте его и установите: + +```bash +cd path\to\archive\clever_cam_calibration\ +python setup.py build +python setup.py install +``` + +> path\to\archive - путь до распакованного архива. + +## Подготовка к калибровке + +Вам необходимо подготовить калибровочную мишень. Она представляет собой «шахматную доску». Файл можно взять [отсюда](https://www.oreilly.com/library/view/learning-opencv-3/9781491937983/assets/lcv3_ac01.png). +Наклейте распечатанную мишень на любую твердую поверхность. Посчитайте количество пересечений в длину и в ширину доски, измерьте размер клетки (в мм). + +![img](../assets/chessboard.jpg) + +Включите Клевер и подключитесь к его Wifi. + +> Перейдите на 192.168.11.1:8080 и проверьте, получает ли компьютер изображения из топика image_raw. + +## Калибровка + +Запустите скрипт **_calibrate_cam_**: + +**Windows:** + + ```bash +>path\to\python\Scripts\calibrate_cam.exe +``` + +> path\to\Python - путь до директории Python + +**Linux:** + +```bash +>calibrate_cam +``` + +Задайте параметры доски: + +```bash +>calibrate_cam +Chessboard width: # Перекрестий в ширину +Chessboard height: # Перекрестий в длину +Square size: # Длина ребра клетки (в мм) +Saving mode (YES - on): # Режим сохранения +``` + +> Режим сохранения: если включен, то все полученные фотографии будут сохраняться в нынешней директории. + +Скрипт начнет свою работу: + + ... + Calibration started! + Commands: + help, catch (key: Enter), delete, restart, stop, finish + +Чтобы откалибровать камеру, вам требуется сделать как минимум 25 фото шахматной доски с различных ракурсов. + +![img](../assets/calibration.jpg) + +Чтобы сделать фото, введите команду **_catch_**. + +```bash +>catch +``` + +Программа будет информировать вас о состоянии калибровки. + +```bash +... +Chessboard not found, now 0 (25 required) +> # Enter +--- +Image added, now 1 (25 required) +``` + +> Вместо того, чтобы каждый раз вводить команду **_catch_**, Вы можете просто нажимать клавишу **_Enter_** (вводить пустую строку). + +После того, как будет набрано достаточное количество изображений, введите команду **_finish_**. + +```bash +... +>finish +Calibration successful! +``` + +**Калибровка по существующим изображениям:** + +Если же у вас уже есть изображения, то вы можете откалибровать камеру по ним при помощи скрипта **_calibrate_cam_ex_**. + +```bash +>calibrate_cam_ex +``` + +Указываем характеристики мишени, а так же путь до папки с изображениями: + +```bash +>calibrate_cam_ex +Chessboard width: # Перекрестий в ширину +Chessboard height: # Перекрестий в длину +Square size: # Длина ребра клетки (в мм) +Path: # Путь до папки с изображениями +``` + +В остальном этот скрипт работает аналогично **_calibrate_cam_**. + +Программа обработает все полученные фотографии, и создаст файл **_camera_info_****_._****_yaml_** в нынешней директории. При помощи этого файла можно будет выравнивать искажения на изображениях, полученных с этой камеры. + +> Если вы поменяете разрешение получаемого изображения, вам нужно будет снова калибровать камеру. + +## Исправление искажений + +За получение исправленного изображения отвечает функция **_get_undistorted_image(cv2_image, camera_info)_**: + +* **_cv2_image_**: Закодированное в массив cv2 изображение. +* **_camera_****_­__****_info_**: Путь до файла калибровки. + +Функция возвращает массив cv2, в котором закодировано исправленное изображение. + +> Если вы используете fisheye-камеру, поставляемую вместе с Клевером, то для обработки изображений разрешением 320x240 или 640x480 вы можете использовать уже существующие параметры калибровки. Для этого в качестве аргумента **_camera_info_** передайте параметры **_clever_cam_calibration.clevercamcalib.CLEVER_FISHEYE_CAM_320_** или **_clever_cam_calibration.clevercamcalib.CLEVER_FISHEYE_CAM_640_** соответственно. + +## Примеры работы + +Изначальные изображения: + +![img](../assets/img1.jpg) + +![img](../assets/img2.jpg) + +Иcправленные изображения: + +![img](../assets/calibresult.jpg) + +![img](../assets/calibresult1.jpg) + +## Пример использования + +**Обработка потока изображений с камеры**. + +Данная программа получает изображения с камеры Клевера и выводит их на экран в исправленном виде, используя существующий калибровочный файл. + +```python +import clevercamcalib.clevercamcalib as ccc +import cv2 +import urllib.request +import numpy as np +while True: + req = urllib.request.urlopen('http://192.168.11.1:8080/snapshot?topic=/main_camera/image_raw') + arr = np.asarray(bytearray(req.read()), dtype=np.uint8) + image = cv2.imdecode(arr, -1) + undistorted_img = ccc.get_undistorted_image(image, ccc.CLEVER_FISHEYE_CAM_640) + cv2.imshow("undistort", undistorted_img) + cv2.waitKey(33) +cv2.destroyAllWindows() +``` + +## Использование для ArUco + +Чтобы применить параметры калибровки к системе ArUco-навигации, требуется перенести калибровочный .yaml файл на Raspberry Pi Клевера и инициализировать его. + +> Не забудьте подключиться к WiFI Клевера. + +Для передачи файла используется протокол SFTP. В данном примере используется программа WinSCP. + +Подключимся к Raspberry Pi по SFTP: + +> Пароль: _**raspberry**_ + +![img](../assets/wcp1.png) + +Нажимаем “Войти”. Переходим в _**/home/pi/catkin_ws/src/clever/clever/camera_info/**_ и копируем туда калибровочный .yaml файл: + +![img](../assets/wcp2.jpg) + +Теперь мы должны выбрать этот файл в конфигурации ArUco. Для этого используется связь по протоколу SSH. В данном примере используется программа PuTTY. + +Подключимся к Raspberry Pi по SSH: + +![img](../assets/pty1.jpg) + +Войдем под логином _**pi**_ и паролем _**raspberry**_, перейдем в директорию _**/home/pi/catkin_ws/src/clever/clever/launch**_ и начнем редактировать конфигурацию _**main_camera.launch**_: + +![img](../assets/pty2.jpg) + +В строке _**camera node**_ заменим параметр _**camera_info**_ на _**camera_info.yaml**_: + +![img](../assets/pty3.jpg) + +> Не забудьте изменить разрешение камеры. diff --git a/docs/ru/camera_frame.md b/docs/ru/camera_frame.md index d1f442fc..01bc6626 100644 --- a/docs/ru/camera_frame.md +++ b/docs/ru/camera_frame.md @@ -1,12 +1,14 @@ # Настройка расположения основной камеры +> **Note** Документация для версий [образа](microsd_images.md), начиная с **0.15**. Для более ранних версий см. [документацию для версии **0.14**](https://github.com/CopterExpress/clever/blob/v0.14/docs/ru/camera_frame.md). + Расположение и ориентация основной камеры задается в файле `~/catkin_ws/src/clever/clever/launch/main_camera.launch`: ```xml - + ``` -Эта строка задает статическую трансформацию между фреймом `fcu` ([соответствует корпусу полетного контроллера](frames.md)) и камерой (`main_camera_optical`) в формате: +Эта строка задает статическую трансформацию между фреймом `base_link` ([соответствует корпусу полетного контроллера](frames.md)) и камерой (`main_camera_optical`) в формате: ```txt сдвиг_x сдвиг_y сдвиг_z угол_рысканье угол_тангаж угол_крен @@ -22,40 +24,40 @@ ## Настройки для Клевера -> Первое изображение - как выглядит модель коптера в Rviz при указанных настройках, второе - как выглядит Клевер при тех же настройках. +Первое изображение – как выглядит модель коптера в rviz при указанных настройках, второе – как выглядит Клевер при тех же настройках. ### 1. Камера направлена вниз, шлейф назад ```xml - + ``` -![](../assets/camera_option_1_rviz.png) -![](../assets/camera_option_1_clever.jpg) + + ### 2. Камера направлена вниз, шлейф вперёд ```xml - + ``` -![](../assets/camera_option_2_rviz.png) -![](../assets/camera_option_2_clever.jpg) + + ### 3. Камера направлена вверх, шлейф назад ```xml - + ``` -![](../assets/camera_option_3_rviz.png) -![](../assets/camera_option_3_clever.jpg) + + ### 4. Камера направлена вверх, шлейф вперёд ```xml - + ``` -![](../assets/camera_option_4_rviz.png) -![](../assets/camera_option_4_clever.jpg) + + diff --git a/docs/ru/deck.md b/docs/ru/deck.md index a10508d4..d898f668 100644 --- a/docs/ru/deck.md +++ b/docs/ru/deck.md @@ -1,3 +1,5 @@ +# Комплектация + В составе набора имеется 4 дополнительных рамы (поз. 2). Они абсолютно одинаковые. Поэтому для дальнейшего удобства понимания инструкции условно разделим их на верхнюю и нижнюю дополнительные рамы diff --git a/docs/ru/face_recognition.md b/docs/ru/face_recognition.md new file mode 100644 index 00000000..02598ab1 --- /dev/null +++ b/docs/ru/face_recognition.md @@ -0,0 +1,163 @@ +# Система распознавания лиц + +## Введение + +В последнее время системы распознавания лиц используются все шире, область применения этой технологии поистине огромна: от обычных селфи-дронов до дронов-полицейских. Ее интеграция в различные устройства проводится повсеместно. Сам процесс распознавания реально завораживает, и это сподвигло меня сделать проект связанный именно с этим. Целью моего стажерского проекта является создание простой open source-ной системы распознавания лиц с квадрокоптера Клевер. Данная программа берет изображения с камеры квадрокоптера, а его обработка происходит уже на компьютере. Поэтому все оставшиеся инструкции выполняются на ПК. + +## Разработка + +Первой задачей было найти алгоритм самого распознавания. В качестве пути решения проблемы было выбрано использовать [готовое API для Python](https://github.com/ageitgey/face_recognition). Данное API сочетает в себе ряд преимуществ: скорость и точность распознавания, а также простота использования. + +## Установка + +Для начала нужно установить все необходимые библиотеки: + +```bash +pip install face_recognition +pip install opencv-python +``` + +Затем скачать сам скрипт из репозитория: + +```bash +git clone https://github.com/mmkuznecov/face_recognition_from_clever.git +``` + +## Объяснение кода + +Подключаем библиотеки: + +```python +import face_recognition +import cv2 +import os +import urllib.request +import numpy as np +``` + +***Данный кусок кода предназначен для Python 3. В Python 2.7 подключаем urllib2 вместо urllib:*** + +```python +import urllib2 +``` + +Создаем список кодировок изображений и список имен: + +```python +faces_images=[] +for i in os.listdir('faces/'): + faces_images.append(face_recognition.load_image_file('faces/'+i)) +known_face_encodings=[] +for i in faces_images: + known_face_encodings.append(face_recognition.face_encodings(i)[0]) +known_face_names=[]url +for i in os.listdir('faces/'): + i=i.split('.')[0] + known_face_names.append(i) +``` + +***Дополнение: все изображения хранятся в папке faces в формате name.jpg*** + + + + + + + +Инициализируем некоторые переменные: + +```python +face_locations = [] +face_encodings = [] +face_names = [] +process_this_frame = True +``` + +Берем изображение с сервера и преобразуем его в cv2 формат: + +```python +req = urllib.request.urlopen('http://192.168.11.1:8080/snapshot?topic=/main_camera/image_raw') +arr = np.asarray(bytearray(req.read()), dtype=np.uint8) +frame = cv2.imdecode(arr, -1) +``` + +***Для Python 2.7:*** + +```python +req = urllib2.urlopen('http://192.168.11.1:8080/snapshot?topic=/main_camera/image_raw') +arr = np.asarray(bytearray(req.read()), dtype=np.uint8) +frame = cv2.imdecode(arr, -1) +``` + +Объяснение дальнейшего кода можно найти на github’е используемого API в комментариях к [следующему скрипту](https://github.com/ageitgey/face_recognition/blob/master/examples/facerec_from_webcam_faster.py) + +## Использование + +Достаточно подключиться к "Клеверу" через Wi-Fi и проверить, корректно ли работает видеострим с камеры. + +Затем просто запускаем скрипт: + +```bash +python recog.py +``` + +И на выходе: + + + + + +## Возможные трудности + +При запуске скрипта может выскочить следующая ошибка: + +```python + known_face_encodings.append(face_recognition.face_encodings(i)[0]) +IndexError: list index out of range +``` + +В этом случае постарайтесь переделать изображения в папке faces, возможно из-за плохого качества программа не распознает лиц на изображениях. + +## Использование калибровки + +Для повышения точности распознавания можно использовать калибровку камеры. Модуль для калибровки можно установить, используя [специальный пакет](https://github.com/tinderad/clever_cam_calibration). Инструкцию по установке и использованию можно найти в файле calibration.md. Программа с использованием калибровочного пакета называется recog_undist.py + +**Краткое пояснение кода:** + +Подключаем установленный пакет: + +```python +import clever_cam_calibration.clevercamcalib as ccc +``` + +Добавляем следующие строки: + +```python +height_or, width_or, depth_or = frame.shape +``` + +Таким образом получаем информацию о размере изображения, где height_or-это высота оригинального изображения в пикселях, а width_or-ширина. +Затем исправляем искажения оригинального изображения и получаем уже его параметры: + +```python +if height_or==240 and width_or==320: + frame=ccc.get_undistorted_image(frame,ccc.CLEVER_FISHEYE_CAM_320) +elif height_or==480 and width_or==640: + frame=ccc.get_undistorted_image(frame,ccc.CLEVER_FISHEYE_CAM_640) +else: + frame=ccc.get_undistorted_image(frame,input("Input your path to the .yaml file: ")) +height_unz, width_unz, depth_unz = frame.shape +``` + +***В данном случае мы передаем аргумент ссс.CLEVER_FISHEYE_CAM_640, т.к. разрешение изображения в приведенном примере составляет 640x480, также можно использовать ссс.CLEVER_FISHEYE_CAM_320 для разрешения 320x240, в противном случае необходимо в качестве второго аргумента передать путь до калибровочного .yaml файла.*** + +И, наконец, возвращаем изображение к изначальному размеру: + +```python +frame=cv2.resize(frame,(0,0), fx=(width_or/width_unz),fy=(height_or/height_unz)) +``` + +Благодаря этому можно значительно повысить точность распознавания, т.к. обрабатываемое изображение будет уже не так сильно искажено. + + + diff --git a/docs/ru/frames.md b/docs/ru/frames.md index 93a00526..52893ef3 100644 --- a/docs/ru/frames.md +++ b/docs/ru/frames.md @@ -1,13 +1,15 @@ Системы координат (фреймы) === +> **Note** Документация для версий [образа](microsd_images.md), начиная с **0.15**. Для более ранних версий см. [документацию для версии **0.14**](https://github.com/CopterExpress/clever/blob/v0.14/docs/ru/frames.md). + ![Системы координаты Клевера (TF2)](../assets/frames.png) Основные фреймы в пакете `clever`: -* `local_origin` — координаты относительно точки инициализации полетного контроллера: белая сетка на иллюстрации; -* `fcu` — координаты относительно квадрокоптера: схематичное изображение квадрокоптера на иллюстрации; -* `fcu_horiz` — координаты относительно квадрокоптера без учета наклонов по тангажу и крену: красная, синия и зеленая линии на иллюстрации. +* `map` — координаты относительно точки инициализации полетного контроллера: белая сетка на иллюстрации; +* `base_link` — координаты относительно квадрокоптера: схематичное изображение квадрокоптера на иллюстрации; +* `body` — координаты относительно квадрокоптера без учета наклонов по тангажу и крену: красная, синия и зеленая линии на иллюстрации. > **Hint** В соответствии с [соглашением](http://www.ros.org/reps/rep-0103.html), для фреймов, связанных с коптером, ось X направлена вперед, Y – налево и Z – вверх. diff --git a/docs/ru/lessons.md b/docs/ru/lessons.md index 69acae8e..763fee59 100644 --- a/docs/ru/lessons.md +++ b/docs/ru/lessons.md @@ -1,24 +1,24 @@ # Теория -[**Урок №1** «Знакомство. Принципы проектирования и строение мультикоптеров»](https://github.com/CopterExpress/clever/blob/master/docs/lesson1.md) +[**Урок №1** «Знакомство. Принципы проектирования и строение мультикоптеров»](https://github.com/CopterExpress/clever/blob/master/docs/ru/lesson1.md) -[**Урок №2** «Основы электричества»](https://github.com/CopterExpress/clever/blob/master/docs/lesson2.md) +[**Урок №2** «Основы электричества»](https://github.com/CopterExpress/clever/blob/master/docs/ru/lesson2.md) -[**Урок №3** «Теория пайки»](https://github.com/CopterExpress/clever/blob/master/docs/lesson3.md) +[**Урок №3** «Теория пайки»](https://github.com/CopterExpress/clever/blob/master/docs/ru/lesson3.md) -[**Урок №4** «Аэродинамика полета. Пропеллер»](https://github.com/CopterExpress/clever/blob/master/docs/lesson4.md) +[**Урок №4** «Аэродинамика полета. Пропеллер»](https://github.com/CopterExpress/clever/blob/master/docs/ru/lesson4.md) -[**Урок №5** «Бесколлекторные двигатели и регуляторы их хода»](https://github.com/CopterExpress/clever/blob/master/docs/lesson5.md) +[**Урок №5** «Бесколлекторные двигатели и регуляторы их хода»](https://github.com/CopterExpress/clever/blob/master/docs/ru/lesson5.md) -[**Урок №6** «Основы электромагнетизма. Типы двигателей»](https://github.com/CopterExpress/clever/blob/master/docs/lesson6.md) +[**Урок №6** «Основы электромагнетизма. Типы двигателей»](https://github.com/CopterExpress/clever/blob/master/docs/ru/lesson6.md) -[**Урок №7** «Принцип работы, типы и устройство аккумуляторов»](https://github.com/CopterExpress/clever/blob/master/docs/lesson7.md) +[**Урок №7** «Принцип работы, типы и устройство аккумуляторов»](https://github.com/CopterExpress/clever/blob/master/docs/ru/lesson7.md) -[**Урок №8** «Управление полётом мультикоптера. Принцип функционирования полётного контроллера. ПИД регуляторы»](https://github.com/CopterExpress/clever/blob/master/docs/lesson8.md) +[**Урок №8** «Управление полётом мультикоптера. Принцип функционирования полётного контроллера. ПИД регуляторы»](https://github.com/CopterExpress/clever/blob/master/docs/ru/lesson8.md) -[**Урок №9** «Основы радиосвязи. Принцип работы радиоаппаратуры управления»](https://github.com/CopterExpress/clever/blob/master/docs/lesson9.md) +[**Урок №9** «Основы радиосвязи. Принцип работы радиоаппаратуры управления»](https://github.com/CopterExpress/clever/blob/master/docs/ru/lesson9.md) -[**Урок №10** «Аналоговая и цифровая видеотрансляция. Применяемые камеры, радиопередатчики иприёмники»](https://github.com/CopterExpress/clever/blob/master/docs/lesson10.md) +[**Урок №10** «Аналоговая и цифровая видеотрансляция. Применяемые камеры, радиопередатчики иприёмники»](https://github.com/CopterExpress/clever/blob/master/docs/ru/lesson10.md) ## Видеоуроки diff --git a/docs/ru/microsd_images.md b/docs/ru/microsd_images.md index d0853b5a..d292973e 100644 --- a/docs/ru/microsd_images.md +++ b/docs/ru/microsd_images.md @@ -18,6 +18,8 @@ [![Etcher](../assets/etcher.gif)](https://etcher.io) +После записи образа на SD-карту, вы можете подключаться к [Клеверу по Wi-Fi](wifi.md), получать [доступ по SSH](ssh.md) и использовать остальные функции. + ## Версия образа Версию установленного образа можно узнать в файле `/etc/clever_version`: diff --git a/docs/ru/rc.md b/docs/ru/rc.md index 89cca134..608bd0a1 100644 --- a/docs/ru/rc.md +++ b/docs/ru/rc.md @@ -1,9 +1,9 @@ Управление Клевером со смартфона === - + -Для управления Клевером со смартфона через Wi-Fi необходимо установить приложение – [iOS](https://itunes.apple.com/ru/app/clever-rc/id1396166572?mt=8), Android (*work-in-progress*). +Для управления Клевером со смартфона через Wi-Fi необходимо установить приложение – [iOS](https://itunes.apple.com/ru/app/clever-rc/id1396166572?mt=8), [Android](https://play.google.com/store/apps/details?id=express.copter.cleverrc). ![CLEVER RC](../assets/IMG_4397.PNG) @@ -46,7 +46,7 @@ sudo systemctl restart clever Подключите смартфон к [Wi-Fi](wifi.md) сети Клевера (`CLEVER-xxxx`). Приложение должно подключиться с коптеру автоматически. При успешном подключении должны отобразиться текущий [режим](modes.md) и заряд батареи. -Стики на экране приложения работают так же, как и реальные стики. Для арма коптера подержите левый стик в правом нижнем углу на протяжении нескольких секунд. Для дизарма – в левом нижнем углу. +Стики на экране приложения работают так же, как и реальные стики. Для арма коптера подержите левый стик в правом нижнем углу на протяжении нескольких секунд. Для дизарма – в левом нижнем углу. Неисправности --- diff --git a/docs/ru/rviz.md b/docs/ru/rviz.md index 9056946d..7a26ef89 100644 --- a/docs/ru/rviz.md +++ b/docs/ru/rviz.md @@ -31,7 +31,7 @@ export ROS_IP=192.168.11.1 ### Визуализация положения коптера -В качестве reference frame рекомендуется установить фрейм `local_origin`. Для визуализации коптера добавьте визуализационные маркеры из топика `/vehicle_markers`. Для визуализации камеры коптера добавьте визуализационные маркеры из топика `/main_camera/camera_markers`. +В качестве reference frame рекомендуется установить фрейм `map`. Для визуализации коптера добавьте визуализационные маркеры из топика `/vehicle_markers`. Для визуализации камеры коптера добавьте визуализационные маркеры из топика `/main_camera/camera_markers`. Результат визуализации коптера и камеры представлен ниже: @@ -43,6 +43,14 @@ export ROS_IP=192.168.11.1 Axis или Grid настроенный на фрейм `aruco_map` будут визуализировать расположение [карты ArUco-меток](aruco.md). +### jsk_rviz_plugins + +Рекомендуется также установка набора дополнительных полезных плагинов для rviz [jsk_rviz_plugins](https://jsk-docs.readthedocs.io/en/latest/jsk_visualization/doc/jsk_rviz_plugins/index.html). Это набор позволяет визуализировать топики типа `TwistStamped` (скорость), `CameraInfo`, `PolygonArray` и многое другое. Для установки используйте команду: + +```bash +sudo apt-get install ros-kinetic-jsk-visualization +``` + Запуск инструментов rqt --- @@ -60,11 +68,8 @@ ROS_MASTER_URI=http://192.168.11.1:11311 rqt ROS_MASTER_URI=http://192.168.11.1:11311 rqt_image_view ``` -jsk_rviz_plugins ---- +Краткое описание полезных rqt-плагинов: -Рекомендуется также установка набора дополнительных полезных плагинов для rviz [jsk_rviz_plugins](https://jsk-docs.readthedocs.io/en/latest/jsk_visualization/doc/jsk_rviz_plugins/index.html). Это набор позволяет визуализировать топики типа `TwistStamped` (скорость), `CameraInfo`, `PolygonArray` и многое другое. Для установки используйте команду: - -```bash -sudo apt-get install ros-kinetic-jsk-visualization -``` +* `rqt_image_view` – просмотр изображений из топиков типа `sensor_msgs/Image`; +* `rqt_multiplot` – построение графиков по данным из произвольным топиков (установка: `sudo apt-get install ros-kinetic-rqt-multiplot`); +* Bag – работа с [Bag-файлами](http://wiki.ros.org/rosbag). diff --git a/docs/ru/setup.md b/docs/ru/setup.md index 439e3f44..4c4a42bb 100644 --- a/docs/ru/setup.md +++ b/docs/ru/setup.md @@ -144,7 +144,7 @@ 2. Устанавливаем БПЛА на ровную поверхность и кликаем OK. 3. Ждем окончания калибровки. -![Калибровка компаса](../assets/calibrategyro.jpg) +![Калибровка гироскопа](../assets/calibrategyro.jpg) > *Warning* Во время калибровки БПЛА не должен менять своего положения, шататься и т.д. diff --git a/docs/ru/simple_offboard.md b/docs/ru/simple_offboard.md index a272555a..4f1d0274 100644 --- a/docs/ru/simple_offboard.md +++ b/docs/ru/simple_offboard.md @@ -1,26 +1,14 @@ Simple offboard === +> **Note** Документация для версий [образа](microsd_images.md), начиная с **0.15**. Для более ранних версий см. [документацию для версии **0.14**](https://github.com/CopterExpress/clever/blob/v0.14/docs/ru/simple_offboard.md). + Модуль `simple_offboard` пакета `clever` предназначен для упрощенного программирования автономного дрона ([режим](modes.md) `OFFBOARD`). Он позволяет устанавливать желаемые полетные задачи и автоматически трансформирует [систему координат](frames.md). `simple_offboard` является высокоуровневым способом взаимодействия с полетным контроллером. Для более низкоуровневой работы см. [mavros](mavros.md). Основные сервисы – `get_telemetry` (получение всей телеметрии), `navigate` (полет в заданную точку по прямой), `navigate_global` (полет в глобальную точку по прямой), `land` (переход в режим посадки). -Общие для сервисов параметры: - -* `auto_arm` = `true`/`false` – перевести коптер в `OFFBOARD` и заармить автоматически (**коптер взлетит**); -* `frame_id` — система координат в TF2, в которой заданы координаты и рысканье (yaw), [описание систем координат](frames.md); -* `update_frame` — считать ли систему координат изменяющейся (например, `false` для `local_origin`, `fcu`, `fcu_horiz`, `true` для `marker_map`); -* `x`, `y` – горизонтальные координаты в системе координат `frame_id` *(м)*; -* `z` — высота в системе координат `frame_id` *(м)*; -* `lat`, `lon` – широта и долгота *(градусы)*; -* `yaw` — рысканье в радианах в системе координат `frame_id` (0 – коптер смотрит по оси X); -* `yaw_rate` — угловая скорость по рысканью в радианах в секунду (против часовой), `yaw` должен быть установлен в NaN; -* `thrust` — уровень газа от 0 (нет газа) до 1 (полный газ). - -> **Warning** API модуля `simple_offboard` на данный момент нестабилен и может измениться. - Использование из языка Python --- @@ -59,11 +47,11 @@ release = rospy.ServiceProxy('release', Trigger) Параметры: -* `frame_id` – [фрейм](frames.md) для значений `x`, `y`, `z`, `vx`, `vy`, `vz`. Пример: `local_origin`, `fcu_horiz`, `aruco_map`. +* `frame_id` – [система координат](frames.md) для значений `x`, `y`, `z`, `vx`, `vy`, `vz`. Пример: `map`, `body`, `aruco_map`. Значение по умолчанию: `map`. Формат ответа: -* `frame_id` – фрейм; +* `frame_id` – система координат; * `connected` – есть ли подключение к FCU; * `armed` – состояние `armed` винтов (винты включены, если true); * `mode` – текущий [полетный режим](modes.md); @@ -73,7 +61,7 @@ release = rospy.ServiceProxy('release', Trigger) * `vx, vy, vz` – скорость коптера *(м/с)*; * `pitch` – угол по тангажу *(радианы)*; * `roll` – угол по крену *(радианы)*; -* `yaw` – угол по рысканью в фрейме `frame_id`; +* `yaw` – угол по рысканью *(радианы)*; * `pitch_rate` – угловая скорость по тангажу *(рад/с)*; * `roll_rate` – угловая скорость по крену *(рад/с)*; * `yaw_rate` – угловая скорость по рысканью *(рад/с)*; @@ -118,19 +106,19 @@ rosservice call /get_telemetry "{frame_id: ''}" Параметры: -* `x`, `y`, `z` – координаты в системе `frame_id` *(м)*; +* `x`, `y`, `z` – координаты *(м)*; * `yaw` – угол по рысканью *(радианы)*; * `yaw_rate` – угловая скорость по рысканью (применяется при установке yaw в `NaN`) *(рад/с)*; * `speed` – скорость полета (скорость движения setpoint) *(м/с)*; * `auto_arm` – перевести коптер в `OFFBOARD` и заармить автоматически (**коптер взлетит**); -* `frame_id`, `update_frame`. +* `frame_id` – [система координат](frames.md), в которой заданы `x`, `y`, `z` и `yaw` (по умолчанию: `map`). > **Note** Для полета без изменения угла по рыскаью достаточно установить `yaw` в `NaN` (значение угловой скорости по-умолчанию – 0). Взлет на высоту 1.5 м со скоростью взлета 0.5 м/с: ```python -navigate(x=0, y=0, z=1.5, speed=0.5, frame_id='fcu_horiz', auto_arm=True) +navigate(x=0, y=0, z=1.5, speed=0.5, frame_id='body', auto_arm=True) ``` Полет по прямой в точку 5:0 (высота 2) в локальной системе координат со скоростью 0.8 м/с (рысканье установится в 0): @@ -148,37 +136,37 @@ navigate(x=5, y=0, z=3, speed=0.8, yaw=float('nan')) Полет вправо относительно коптера на 3 м: ```python -navigate(x=0, y=-3, z=0, speed=1, frame_id='fcu_horiz') +navigate(x=0, y=-3, z=0, speed=1, frame_id='body') ``` Повернуться на 90 градусов против часовой: ```python -navigate(yaw=math.radians(-90), frame_id='fcu_horiz') +navigate(yaw=math.radians(-90), frame_id='body') ``` Полет в точку 3:2 (высота 2) в системе координат [маркерного поля](aruco.md) со скоростью 1 м/с: ```python -navigate(x=3, y=2, z=2, speed=1, frame_id='aruco_map', update_frame=True) +navigate(x=3, y=2, z=2, speed=1, frame_id='aruco_map') ``` Вращение на месте со скоростью 0.5 рад/c (против часовой): ```python -navigate(x=0, y=0, z=0, yaw=float('nan'), yaw_rate=0.5, frame_id='fcu_horiz') +navigate(x=0, y=0, z=0, yaw=float('nan'), yaw_rate=0.5, frame_id='body') ``` Полет вперед 3 метра со скоростью 0.5 м/с, вращаясь по рысканью со скоростью 0.2 рад/с: ```python -navigate(x=3, y=0, z=0, speed=0.5, yaw=float('nan'), yaw_rate=0.2, frame_id='fcu_horiz') +navigate(x=3, y=0, z=0, speed=0.5, yaw=float('nan'), yaw_rate=0.2, frame_id='body') ``` Взлет на высоту 2 м (командная строка): ```bash -rosservice call /navigate "{x: 0.0, y: 0.0, z: 2, yaw: 0.0, yaw_rate: 0.0, speed: 0.5, frame_id: 'fcu_horiz', update_frame: false, auto_arm: true}" +rosservice call /navigate "{x: 0.0, y: 0.0, z: 2, yaw: 0.0, yaw_rate: 0.0, speed: 0.5, frame_id: 'body', auto_arm: true}" ``` ### navigate_global @@ -188,31 +176,31 @@ rosservice call /navigate "{x: 0.0, y: 0.0, z: 2, yaw: 0.0, yaw_rate: 0.0, speed Параметры: * `lat`, `lon` – широта и долгота *(градусы)*; -* `z` – высота в системе координат `frame_id` *(м)*; +* `z` – высота *(м)*; * `yaw` – угол по рысканью *(радианы)*; * `yaw_rate` – угловая скорость по рысканью (при установке yaw в `NaN`) *(рад/с)*; * `speed` – скорость полета (скорость движения setpoint) *(м/с)*; * `auto_arm` – перевести коптер в `OFFBOARD` и заармить автоматически (**коптер взлетит**); -* `frame_id`, `update_frame`. +* `frame_id` – [система координат](frames.md), в которой заданы `z` и `yaw` (по умолчанию: `map`). > **Note** Для полета без изменения угла по рыскаью достаточно установить `yaw` в `NaN` (значение угловой скорости по-умолчанию – 0). Полет в глобальную точку со скоростью 5 м/с, оставаясь на текущей высоте (`yaw` установится в 0, коптер сориентируется передом на восток): ```python -navigate_global(lat=55.707033, lon=37.725010, z=0, speed=5, frame_id='fcu_horiz') +navigate_global(lat=55.707033, lon=37.725010, z=0, speed=5, frame_id='body') ``` Полет в глобальную точку без изменения угла по рысканью (`yaw` = `NaN`, `yaw_rate` = 0): ```python -navigate_global(lat=55.707033, lon=37.725010, z=0, speed=5, yaw=float('nan'), frame_id='fcu_horiz') +navigate_global(lat=55.707033, lon=37.725010, z=0, speed=5, yaw=float('nan'), frame_id='body') ``` Полет в глобальную точку (командная строка): ```bash -rosservice call /navigate_global "{lat: 55.707033, lon: 37.725010, z: 0.0, yaw: 0.0, yaw_rate: 0.0, speed: 5.0, frame_id: 'fcu_horiz', update_frame: false, auto_arm: false}" +rosservice call /navigate_global "{lat: 55.707033, lon: 37.725010, z: 0.0, yaw: 0.0, yaw_rate: 0.0, speed: 5.0, frame_id: 'body', auto_arm: false}" ``` ### set_position @@ -223,34 +211,34 @@ rosservice call /navigate_global "{lat: 55.707033, lon: 37.725010, z: 0.0, yaw: Параметры: -* `x`, `y`, `z` – координаты точки в системе координат `frame_id` *(м)*; +* `x`, `y`, `z` – координаты точки *(м)*; * `yaw` – угол по рысканью *(радианы)*; * `yaw_rate` – угловая скорость по рысканью (при установке yaw в NaN) *(рад/с)*; * `auto_arm` – перевести коптер в `OFFBOARD` и заармить автоматически (**коптер взлетит**); -* `frame_id`, `update_frame`. +* `frame_id` – [система координат](frames.md), в которой заданы `x`, `y`, `z` и `yaw` (по умолчанию: `map`). Зависнуть на месте: ```python -set_position(frame_id='fcu_horiz') +set_position(frame_id='body') ``` Назначить целевую точку на 3 м выше текущей позиции: ```python -set_position(x=0, y=0, z=3, frame_id='fcu_horiz') +set_position(x=0, y=0, z=3, frame_id='body') ``` Назначить целевую точку на 1 м впереди текущей позиции: ```python -set_position(x=1, y=0, z=0, frame_id='fcu_horiz') +set_position(x=1, y=0, z=0, frame_id='body') ``` Вращение на месте со скоростью 0.5 рад/c: ```python -set_position(x=0, y=0, z=0, frame_id='fcu_horiz', yaw=float('nan'), yaw_rate=0.5) +set_position(x=0, y=0, z=0, frame_id='body', yaw=float('nan'), yaw_rate=0.5) ``` ### set_velocity @@ -261,34 +249,32 @@ set_position(x=0, y=0, z=0, frame_id='fcu_horiz', yaw=float('nan'), yaw_rate=0.5 * `yaw` – угол по рысканью *(радианы)*; * `yaw_rate` – угловая скорость по рысканью (при установке yaw в NaN) *(рад/с)*; * `auto_arm` – перевести коптер в `OFFBOARD` и заармить автоматически (**коптер взлетит**); -* `frame_id`, `update_frame`. +* `frame_id` – [система координат](frames.md), в которой заданы `vx`, `vy`, `vz` и `yaw` (по умолчанию: `map`). > **Note** Параметр `frame_id` определяет только ориентацию результирующего вектора скорости, но не его длину. Полет вперед (относительно коптера) со скоростью 1 м/с: ```python -set_velocity(vx=1, vy=0.0, vz=0, frame_id='fcu_horiz') +set_velocity(vx=1, vy=0.0, vz=0, frame_id='body') ``` Один из вариантов полета по кругу: ```python -set_velocity(vx=0.4, vy=0.0, vz=0, yaw=float('nan'), yaw_rate=0.4, frame_id='fcu_horiz', update_frame=True) +set_velocity(vx=0.4, vy=0.0, vz=0, yaw=float('nan'), yaw_rate=0.4, frame_id='body') ``` ### set_attitude Установить тангаж, крен, рысканье и уровень газа (примерный аналог управления в [режиме `STABILIZED`](modes.md)). Данный сервис может быть использован для более низкоуровнего контроля поведения коптера либо для управления коптером при отсутствии источника достоверных данных о его позиции. -> **Note** Параметр `frame_id` определяет только систему координат, в которой задается рысканье (`yaw`). - Параметры: * `pitch`, `roll`, `yaw` – необходимый угол по тангажу, крену и рысканью *(радианы)*; -* `thrust` – уровень газа от 0 (нет газа) до 1 (полный газ); +* `thrust` – уровень газа от 0 (нет газа, пропеллеры остановлены) до 1 (полный газ); * `auto_arm` – перевести коптер в `OFFBOARD` и заармить автоматически (**коптер взлетит**); -* `frame_id`, `update_frame`. +* `frame_id` – [система координат](frames.md), в которой задан `yaw` (по умолчанию: `map`). ### set_rates @@ -297,14 +283,14 @@ set_velocity(vx=0.4, vy=0.0, vz=0, yaw=float('nan'), yaw_rate=0.4, frame_id='fcu Параметры: * `pitch_rate`, `roll_rate`, `yaw_rate` – угловая скорость по танажу, крену и рыканью *(рад/с)*; -* `thrust` – уровень газа от 0 (нет газа) до 1 (полный газ). +* `thrust` – уровень газа от 0 (нет газа, пропеллеры остановлены) до 1 (полный газ). * `auto_arm` – перевести коптер в `OFFBOARD` и заармить автоматически (**коптер взлетит**); ### land Перевести коптер в [режим](modes.md) посадки (`AUTO.LAND` или аналогичный). -> **Note** Для автоматического отключения винтов после посадки PX4-параметр `COM_DISARM_LAND` должен быть установлен в значение > 0. +> **Note** Для автоматического отключения винтов после посадки [параметр PX4](px4_parameters.md) `COM_DISARM_LAND` должен быть установлен в значение > 0. Посадка коптера: @@ -321,9 +307,11 @@ if res.success: rosservice call /land "{}" ``` + Дополнительные материалы ------------------------ diff --git a/docs/ru/snippets.md b/docs/ru/snippets.md index efa9db5d..dc9e4a61 100644 --- a/docs/ru/snippets.md +++ b/docs/ru/snippets.md @@ -39,7 +39,7 @@ tolerance = 0.2 # точность проверки высоты (м) start = get_telemetry() # Взлетаем на 2 м -print navigate(z=z, speed=0.5, frame_id='fcu_horiz', auto_arm=True) +print navigate(z=z, speed=0.5, frame_id='body', auto_arm=True) # Ожидаем взлета while True: @@ -100,14 +100,14 @@ tf_listener = tf2_ros.TransformListener(tf_buffer) # Создаем объект PoseStamped (либо получаем из топика): pose = PoseStamped() -pose.header.frame_id = 'local_origin' # фрейм, в котором задана позиция +pose.header.frame_id = 'map' # фрейм, в котором задана позиция pose.header.stamp = rospy.get_rostime() # момент времени, для которого задана позиция (текущее время) pose.pose.position.x = 1 pose.pose.position.y = 2 pose.pose.position.z = 3 pose.pose.orientation.w = 1 -frame_id = 'fcu' # целевой фрейм +frame_id = 'base_link' # целевой фрейм transform_timeout = rospy.Duration(0.2) # таймаут ожидания транформации # Преобразовываем позицию из старого фрейма в новый: @@ -252,6 +252,22 @@ rospy.Subscriber('mavros/rc/in', RCIn, rc_callback) rospy.spin() ``` +### # {#set_mode} + +Сменить [режим полета](modes.md) на произвольный: + +```python +from mavros_msgs.srv import SetMode + +# ... + +set_mode = rospy.ServiceProxy('mavros/set_mode', SetMode) + +# ... + +set_mode(custom_mode='STABILIZED') +``` + ### # {#flip} Флип: diff --git a/docs/ru/sonar.md b/docs/ru/sonar.md index 500f8e03..0339abd9 100644 --- a/docs/ru/sonar.md +++ b/docs/ru/sonar.md @@ -1,11 +1,15 @@ # Работа с ультразвуковым дальномером -Ультразвуковой дальномер (*«сонар»*) — это датчик расстояния, принцип действия которого основан на измерении времени распространения звуковой волны (с частотой около 40 Гц) до препятствия и обратно. Сонар может измерять расстояние до 1,5–3 м с точностью до нескольких сантиметров. +Ультразвуковой дальномер (*«сонар»*) — это датчик расстояния, принцип действия которого основан на измерении времени распространения звуковой волны (с частотой около 40 кГц) до препятствия и обратно. Сонар может измерять расстояние до 1,5–3 м с точностью до нескольких сантиметров. -## HC-SR04 +## Дальномер HC-SR04 hc-sr04 +## Установка + +Дальномер закрепляется к корпусу с помощью двухстороннего скотча. Для получения приемлемых результатов необходимо использование виброразвязки. В качестве виброразвязки можно использовать кусок поролона. + ### Подключение Подключите HC-SR04 к Raspberry Pi согласно схеме подключения. Используйте резисторы на 1,0 и 2,2 кОм и любые свободные GPIO-пины, например 23 и 24: @@ -20,7 +24,7 @@ ### Чтение данных -Чтобы считать данных с дальномера HC-SR04 используется библиотека для работы с GPIO – [`pigpio`](http://abyz.me.uk/rpi/pigpio/index.html). Эта библиотека предустановлена на [образе Клевера](microsd_images.md), начиная с версии **v0.14**. Для более старых версий образа используйте [инструкцию по установке](http://abyz.me.uk/rpi/pigpio/download.html). +Чтобы считать данные с дальномера HC-SR04, используется библиотека для работы с GPIO – [`pigpio`](http://abyz.me.uk/rpi/pigpio/index.html). Эта библиотека предустановлена на [образе Клевера](microsd_images.md), начиная с версии **v0.14**. Для более старых версий образа используйте [инструкцию по установке](http://abyz.me.uk/rpi/pigpio/download.html). Для работы с `pigpio` необходимо запустить соответствующий демон: @@ -108,7 +112,7 @@ while True: Исходный код ROS-ноды, использовавшейся для построения графика можно найти [на Gist](https://gist.github.com/okalachev/feb2d7235f5c9636802c3cda43add253). -## RCW-0001 +## Дальномер RCW-0001 @@ -119,11 +123,11 @@ while True: Пример полетной программы с использованием [simple_offboard](simple_offboard.md), которая заставляет коптер лететь вперед, пока подключенный ультразвуковой дальномер не задетектирует препятствие: ```python -set_velocity(x=0.5, frame_id='fcu_horiz', auto_arm=True) # полет вперед со скоростью 0.5 мс +set_velocity(x=0.5, frame_id='body', auto_arm=True) # полет вперед со скоростью 0.5 мс while True: if read_distance_filtered() < 1: # если препятствие ближе, чем в 1 м, зависаем в точке - set_position(x=0, y=0, z=0, frame_id='fcu_horiz') + set_position(x=0, y=0, z=0, frame_id='body') rospy.sleep(0.1) ``` diff --git a/docs/ru/web_video_server.md b/docs/ru/web_video_server.md index 9d404725..e6b5822e 100644 --- a/docs/ru/web_video_server.md +++ b/docs/ru/web_video_server.md @@ -1,6 +1,6 @@ # Просмотр изображений с камер -Для просмотра изображений с камер можно воспользовться [rviz](rviz.md), rqt, или смотреть их через браузер, используя web\_video\_server. +Для просмотра изображений с камер (или других ROS-топиков) можно воспользовться [rviz](rviz.md), rqt, или смотреть их через браузер, используя web\_video\_server. См. подробнее про [использование rqt](rviz.md). @@ -22,7 +22,7 @@ sudo systemctl restart clever ### Просмотр -Для просмотра видеострима нужно [подключиться к Wi-Fi](wifi.md) Клевера \(`CLEVER-xxxx`\), перейти на страницу [http://192.168.11.1:8080/](http://192.168.11.1:8080/) и выбрать топик камеры. +Для просмотра видеострима нужно [подключиться к Wi-Fi](wifi.md) Клевера \(`CLEVER-xxxx`\), перейти на страницу [http://192.168.11.1:8080/](http://192.168.11.1:8080/) и выбрать топик. ![Просмотр web_video_server](../assets/web_video_server.png) @@ -33,3 +33,25 @@ http://192.168.11.1:8080/stream_viewer?topic=/main_camera/image_raw&quality=1 По URL выше будет доступен стрим с основной камеры в минимальном возможном качестве. Также доступны параметры `width`, `height` и другие. Подробнее о `web_video_server`: http://wiki.ros.org/web_video_server. + +## Просмотр через rqt_image_view + +Для просмотра изображений через инструменты rqt необходим компьютер с установленной Ubuntu 16.04 и [ROS Kinetic](http://wiki.ros.org/kinetic/Installation/Ubuntu). + +[Подключитесь к Wi-Fi сети Клевера](wifi.md) и запустите `rqt_image_view` с указанием его IP-адреса: + +```bash +ROS_MASTER_URI=http://192.168.11.1:11311 rqt_image_view +``` + +Выберите топик для просмотра, например `/main_camera/image_raw`: + +![rqt_image_view](../assets/rqt_image_view.jpg) + +Для снижения нагрузки на сеть и уменьшения задержки используйте сжатый вариант топика – `/main_camera/image_raw/compressed`. + +Для изменения настроек сжатия используйте rqt-плагин Dynamic Reconfigure: + +![rqt_image_view+rqt_dynamic_reconfigure](../assets/rqt_image_view_dyn_rec.jpg) + +См. [подробнее об rviz и rqt](rviz.md). diff --git a/docs/ru/wifi.md b/docs/ru/wifi.md index 60f60e49..8151ce42 100644 --- a/docs/ru/wifi.md +++ b/docs/ru/wifi.md @@ -8,3 +8,9 @@ Wi-Fi SSID Для изменения настроек Wi-Fi или получения более детальной информации о устройстве сети на Raspberry Pi прочитайте эту [статью](network.md). + +## Веб-интерфейс + +После подключения к Клеверу по адресу http://192.168.11.1 будет доступен веб-интерфейс. В нем доступны основные веб-инструменты Клевера: просмотр топиков с изображениями, веб-терминал (Butterfly) а также полная копия данной документации. + +![web interface](../assets/web_interface.png)