aruco_pose: improve processing of axis visualization in aruco_map/image (#167)

* aruco_pose: improved processing of axis visualization in the topic /aruco_map/image

* aruco_pose: reworked method for drawing axis arrow

* aruco_pose: fix indentation error

* Fix style

.editorconfig

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

.gitattributes

@@ -1,3 +1,5 @@
-apps/ios/cleverrc/roslib.js linguist-vendored
 apps/ios/cleverrc/BinUtils.swift linguist-vendored
-apps/android/app/src/main/assets/roslib.js linguist-vendored
+roslib.js linguist-vendored
+eventemitter2.js linguist-vendored
+ros3d.js linguist-vendored
+three.min.js linguist-vendored

.markdownlint.json

@@ -13,17 +13,98 @@
     "MD040": false,
     "MD044": {
         "names": [
+            "COEX",
+            "Copter Express",
+            "Коптер Экспресс",
+            "Клевер",
             "MAVLink",
             "ROS",
+            "ROS Kinetic",
+            "OpenCV",
+            "Gazebo",
+            "GitHub",
+            "FPV",
+            "PPM",
+            "PWM",
             "Python",
             "C++",
+            "JavaScript",
+            "Node.js",
+            "Django",
+            "Flask",
+            "HTTP",
+            "HTTPS",
+            "WebSocket",
+            "RPC",
             "PX4",
+            "ArduPilot",
+            "jMAVSim",
+            "px4.io",
+            "logs.px4.io",
+            "QGroundControl",
+            "QGC",
             "WireShark",
+            "FlightPlot",
+            "OFFBOARD",
+            "ACRO",
+            "RPY",
+            "LPE",
+            "EKF2",
+            "IMU",
+            "VPE",
+            "SITL",
+            "PID",
             "Wi-Fi",
             "Raspberry Pi",
+            "RPi",
+            "Linux",
+            "GNU",
+            "GNU/Linux",
+            "Windows",
+            "Docker",
+            "RFC",
+            "Travis",
+            "travis-ci.org",
+            "travis-ci.com",
+            "macOS",
+            "iOS",
+            "Android",
+            "Bluetooth",
+            "Raspbian",
+            "Raspbian Jesse",
+            "Raspbian Stretch",
+            "Raspbian Buster",
             "Pixhawk",
             "Pixracer",
-            "ArUco"
+            "Arduino",
+            "GPS",
+            "ArUco",
+            "LIRC",
+            "GPIO",
+            "HC-SR04",
+            "RCW-0001",
+            "RealSense",
+            "NUC",
+            "NVIDIA",
+            "Jetson",
+            "Jetson Nano",
+            "STM",
+            "LED",
+            "USB",
+            "FAT32",
+            "uORB",
+            "SSH",
+            "PuTTY",
+            "API",
+            "UART",
+            "GND",
+            "VCC",
+            "SCL",
+            "SDA",
+            "TCP",
+            "UDP",
+            "QR",
+            "Li-ion"
         ],
         "code_blocks": false
     },

.travis.yml

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

README.md

@@ -1,16 +1,16 @@
 # CLEVER
 
-<img src="docs/assets/clever3.png" align="right" width="300px" alt="CLEVER drone">
+<img src="docs/assets/clever4-front-white.png" align="right" width="400px" alt="CLEVER drone">
 
 CLEVER (Russian: *"Клевер"*, meaning *"Clover"*) is an educational programmable drone kit consisting of an unassembled quadcopter, open source software and documentation. The kit includes Pixhawk/Pixracer autopilot running PX4 firmware, Raspberry Pi 3 as companion computer, a camera for computer vision navigation as well as additional sensors and peripheral devices.
 
 Copter Express has implemented a large number of different autonomous drone projects using exactly the same platform: [automated pizza delivery](https://www.youtube.com/watch?v=hmkAoZOtF58) in Samara and Kazan, coffee delivery in Skolkovo Innovation Center, [autonomous quadcopter with charging station](https://www.youtube.com/watch?v=RjX6nUqw1mI) for site monitoring and security, winning drones on [Robocross-2016](https://www.youtube.com/watch?v=dGbDaz_VmYU) and [Robocross-2017](https://youtu.be/AQnd2CRczbQ) competitions and many others.
 
-**The main documentation in Russian is available [on our Gitbook](https://clever.copterexpress.com/).**
+**The main documentation is available [on Gitbook](https://clever.copterexpress.com/).**
 
 Use it to learn how to assemble, configure, pilot and program autonomous CLEVER drone.
 
-## Preconfigured RPi 3 image
+## Raspberry Pi image
 
 **Preconfigured image for Raspberry Pi 3 with installed and configured software, ready to fly, is available [in the Releases section](https://github.com/CopterExpress/clever/releases).**
 
@@ -30,49 +30,74 @@ API description (in Russian) for autonomous flights is available [on GitBook](ht
 
 ## Manual installation
 
-Install ROS Kinetic according to the [documentation](http://wiki.ros.org/kinetic/Installation).
+Install ROS Kinetic according to the [documentation](http://wiki.ros.org/kinetic/Installation), then [create a Catkin workspace](http://wiki.ros.org/catkin/Tutorials/create_a_workspace).
 
-Clone repo to directory `/home/pi/catkin_ws/src/clever`:
+Clone this repo to directory `~/catkin_ws/src/clever`:
 
 ```bash
 cd ~/catkin_ws/src
 git clone https://github.com/CopterExpress/clever.git clever
 ```
 
-Build ROS packages:
+All the required ROS packages (including `mavros` and `opencv`) can be installed using `rosdep`:
+
+```bash
+cd ~/catkin_ws/
+rosdep install -y --from-paths src --ignore-src
+```
+
+Build ROS packages (on memory constrained platforms you might be going to need to use `-j1` key):
 
 ```bash
 cd ~/catkin_ws
 catkin_make -j1
 ```
 
-Enable systemd service `roscore` (if not enabled):
+To complete `mavros` install you'll need to install `geographiclib` datasets:
 
 ```bash
-sudo systemctl enable /home/pi/catkin_ws/src/clever/deploy/roscore.service
+curl https://raw.githubusercontent.com/mavlink/mavros/master/mavros/scripts/install_geographiclib_datasets.sh | sudo bash
+```
+
+You may optionally install udev rules to provide `/dev/px4fmu` symlink to your PX4-based flight controller connected over USB. Copy `99-px4fmu.rules` to your `/lib/udev/rules.d` folder:
+
+```bash
+cd ~/catkin_ws/src/clever/clever/config
+sudo cp 99-px4fmu.rules /lib/udev/rules.d
+```
+
+Alternatively you may change the `fcu_url` property in `mavros.launch` file to point to your flight controller device.
+
+## Running
+
+Enable systemd service `roscore` (if not running):
+
+```bash
+sudo systemctl enable /home/<username>/catkin_ws/src/clever/builder/assets/roscore.service
 sudo systemctl start roscore
 ```
 
-Enable systemd service `clever`:
+To start connection to SITL, use:
 
 ```bash
-sudo systemctl enable /home/pi/catkin_ws/src/clever/deploy/clever.service
+roslaunch clever sitl.launch
+```
+
+To start connection to the flight controller, use:
+
+```bash
+roslaunch clever clever.launch
+```
+
+> Note that the package is configured to connect to `/dev/px4fmu` by default (see [previous section](#manual-installation)). Install udev rules or specify path to your FCU device in `mavros.launch`.
+
+Also, you can enable and start the systemd service:
+
+```bash
+sudo systemctl enable /home/<username>/catkin_ws/src/clever/deploy/clever.service
 sudo systemctl start clever
 ```
 
-### Dependencies
+## License
 
-[ROS Kinetic](http://wiki.ros.org/kinetic).
-
-Necessary ROS packages:
-
-* `opencv3`
-* `mavros`
-* `rosbridge_suite`
-* `web_video_server`
-* `cv_camera`
-* `nodelet`
-* `dynamic_reconfigure`
-* `bondcpp`, branch `master`
-* `roslint`
-* `rosserial`
+While the Clever platform source code is available under the MIT License, note, that the [documentation](docs/) is licensed under the Creative Commons Attribution-NonCommercial-ShareAlike 4.0 International License.

apps/android/app/src/main/assets/main.js

@@ -23,19 +23,18 @@ function throttle(func, ms) {
 }
 
 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));
-    }
+	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);
+		postAppMessage(msg);
 		return true;
 	} catch(err) {
 		console.warn('The native context does not exist yet');
@@ -109,12 +108,12 @@ function stickTouchStart(e) {
 
 function stickTouchMove(e) {
 	for (touch of e.changedTouches) {
-            onStickTouchMove(touch);
-        }
-    	//onStickTouchMove(e.changedTouches[0]);
-    	rcPublishThrottled();
-    	e.stopPropagation();
-    	e.preventDefault();
+		onStickTouchMove(touch);
+	}
+	//onStickTouchMove(e.changedTouches[0]);
+	rcPublishThrottled();
+	e.stopPropagation();
+	e.preventDefault();
 }
 
 function stickTouchEnd(e) {
@@ -136,4 +135,4 @@ stickRight.addEventListener('touchmove', stickTouchMove);
 stickLeft.addEventListener('touchstart', stickTouchStart);
 stickRight.addEventListener('touchstart', stickTouchStart);
 stickLeft.addEventListener('touchend', stickTouchEnd);
-stickRight.addEventListener('touchend', stickTouchEnd);
+stickRight.addEventListener('touchend', stickTouchEnd);

apps/ios/README.md

@@ -1,5 +1,4 @@
-iOS-приложение для управления Клевером
---------------------------------------
+# iOS-приложение для управления Клевером
 
 Для установки зависимостей необходим [CocoaPods](https://cocoapods.org):
 
@@ -8,3 +7,11 @@ pod install
 ```
 
 Для разработки и сборки откройте в XCode файл `cleverrc.xcworkspace`.
+
+## Политика конфиденциальности
+
+App Store приложение CLEVER RC не собирает и не хранит каких-либо личных данных пользователя.
+
+## Privacy policy
+
+The App Store app CLEVER RC does not collect and store any personal user data.

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

@@ -145,8 +145,15 @@
       "size" : "44x44",
       "idiom" : "watch",
       "scale" : "2x",
-      "role" : "longLook",
-      "subtype" : "42mm"
+      "role" : "appLauncher",
+      "subtype" : "40mm"
+    },
+    {
+      "size" : "50x50",
+      "idiom" : "watch",
+      "scale" : "2x",
+      "role" : "appLauncher",
+      "subtype" : "44mm"
     },
     {
       "size" : "86x86",
@@ -162,10 +169,24 @@
       "role" : "quickLook",
       "subtype" : "42mm"
     },
+    {
+      "size" : "108x108",
+      "idiom" : "watch",
+      "scale" : "2x",
+      "role" : "quickLook",
+      "subtype" : "44mm"
+    },
     {
       "idiom" : "watch-marketing",
       "size" : "1024x1024",
       "scale" : "1x"
+    },
+    {
+      "size" : "44x44",
+      "idiom" : "watch",
+      "scale" : "2x",
+      "role" : "longLook",
+      "subtype" : "42mm"
     }
   ],
   "info" : {

apps/ios/cleverrc/Info.plist

@@ -17,9 +17,9 @@
 	<key>CFBundlePackageType</key>
 	<string>APPL</string>
 	<key>CFBundleShortVersionString</key>
-	<string>1.1</string>
+	<string>1.2</string>
 	<key>CFBundleVersion</key>
-	<string>6</string>
+	<string>7</string>
 	<key>LSRequiresIPhoneOS</key>
 	<true/>
 	<key>UILaunchStoryboardName</key>

aruco_pose/CMakeLists.txt

@@ -16,9 +16,11 @@ find_package(catkin REQUIRED COMPONENTS
   image_transport
   cv_bridge
   tf
-  #tf2
-  #tf2_ros
-  #aruco_msgs
+  tf2
+  tf2_ros
+  tf2_geometry_msgs
+  sensor_msgs
+  message_generation
 )
 
 find_package(OpenCV 3 REQUIRED)
@@ -57,11 +59,12 @@ find_package(OpenCV 3 REQUIRED)
 ##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
 
 ## Generate messages in the 'msg' folder
-#add_message_files(
-#  FILES
-#  Marker.msg
-#  MarkerArray.msg
-#)
+add_message_files(
+  FILES
+  Point2D.msg
+  Marker.msg
+  MarkerArray.msg
+)
 
 ## Generate services in the 'srv' folder
 # add_service_files(
@@ -78,10 +81,11 @@ find_package(OpenCV 3 REQUIRED)
 # )
 
 ## Generate added messages and services with any dependencies listed here
-#generate_messages(
-#  DEPENDENCIES
-#  std_msgs  # Or other packages containing msgs
-#)
+generate_messages(
+  DEPENDENCIES
+  std_msgs
+  geometry_msgs
+)
 
 ################################################
 ## Declare ROS dynamic reconfigure parameters ##
@@ -113,9 +117,9 @@ find_package(OpenCV 3 REQUIRED)
 ## CATKIN_DEPENDS: catkin_packages dependent projects also need
 ## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
-#  INCLUDE_DIRS include
+  INCLUDE_DIRS DEPENDS OpenCV
   LIBRARIES aruco_pose
-#  CATKIN_DEPENDS other_catkin_pkg
+  CATKIN_DEPENDS message_runtime
 #  DEPENDS system_lib
 )
 
@@ -128,17 +132,17 @@ catkin_package(
 include_directories(
 # include
   ${catkin_INCLUDE_DIRS}
+  ${OpenCV_INCLUDE_DIRS}
 )
 
 ## Declare a C++ library
-add_library(${PROJECT_NAME}
-  src/aruco_pose.cpp
+add_library(aruco_pose
+  src/aruco_detect.cpp
+  src/aruco_map.cpp
+  src/draw.cpp
 )
 
-## Add cmake target dependencies of the library
-## as an example, code may need to be generated before libraries
-## either from message generation or dynamic reconfigure
-# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(${PROJECT_NAME} aruco_pose_generate_messages_cpp)
 
 ## Declare a C++ executable
 ## With catkin_make all packages are built within a single CMake context
@@ -156,9 +160,7 @@ add_library(${PROJECT_NAME}
 # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
 ## Specify libraries to link a library or executable target against
-link_directories(/opt/ros/kinetic/lib)
-
-target_link_libraries(${PROJECT_NAME}
+target_link_libraries(aruco_pose
   ${catkin_LIBRARIES}
   ${OpenCV_LIBRARIES}
 )
@@ -171,7 +173,7 @@ target_link_libraries(${PROJECT_NAME}
 # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
 
 ## Mark executable scripts (Python etc.) for installation
- ## in contrast to setup.py, you can choose the destination
+## in contrast to setup.py, you can choose the destination
 # install(PROGRAMS
 #   scripts/my_python_script
 #   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
@@ -210,3 +212,12 @@ target_link_libraries(${PROJECT_NAME}
 
 ## Add folders to be run by python nosetests
 # catkin_add_nosetests(test)
+
+if (CATKIN_ENABLE_TESTING)
+  find_package(rostest REQUIRED)
+  add_rostest(test/basic.test)
+  add_rostest(test/test_parser_pass.test)
+  add_rostest(test/test_parser_empty_map.test)
+  add_rostest(test/test_node_failure.test)
+  add_rostest(test/largemap.test)
+endif()

aruco_pose/README.md

@@ -0,0 +1,120 @@
+# Positioning with ArUco markers
+
+`aruco_pose` package consists of two nodelets: `aruco_detect` detects individual ArUco-markers and estimates their poses, `aruco_map` detects maps of markers using `aruco_detect` output.
+
+## Quick start
+
+To run a camera nodelet, markers and maps detector:
+
+```bash
+roslaunch aruco_pose sample.launch
+```
+
+You're going to need [`cv_camera`](http://wiki.ros.org/cv_camera) package installed.
+
+## aruco_detect nodelet
+
+`aruco_detect` detects ArUco markers on the image, publishes list of them (with poses), TF transformations, visualization markers and processed image for debugging.
+
+It's recommended to run it within the same nodelet manager with the camera nodelet (e. g. [`cv_camera`](http://wiki.ros.org/cv_camera)).
+
+### Parameters
+
+* `~dictionary` (*int*) – ArUco dictionary (default: 2)
+  * 0 = DICT_4X4_50
+  * 1 = DICT_4X4_100,
+  * 2 = DICT_4X4_250,
+  * 3 = DICT_4X4_1000,
+  * 4 = DICT_5X5_50,
+  * 5 = DICT_5X5_100,
+  * 6 = DICT_5X5_250,
+  * 7 = DICT_5X5_1000,
+  * 8 = DICT_6X6_50,
+  * 9 = DICT_6X6_100,
+  * 10 = DICT_6X6_250,
+  * 11 = DICT_6X6_1000,
+  * 12 = DICT_7X7_50,
+  * 13 = DICT_7X7_100,
+  * 14 = DICT_7X7_250,
+  * 15 = DICT_7X7_1000,
+  * 16 = DICT_ARUCO_ORIGINAL
+* `~estimate_poses` (*bool*) – estimate single markers' poses (default: true)
+* `~send_tf` (*bool*) – send TF transforms (default: true)
+* `~frame_id_prefix` (*string*) – prefix for TF transforms names, marker's ID is appended (default: `aruco_`)
+* `~length` (*double*) – markers' sides length
+* `~length_override` (*map*) – lengths of markers with specified ids
+* `~known_tilt` (*string*) – known tilt (pitch and roll) of all the markers as a frame
+
+### Topics
+
+#### Subscribed
+
+* `image_raw` (*sensor_msgs/Image*) – camera image
+* `camera_info` (*sensor_msgs/CameraInfo*) – camera calibration info
+
+#### Published
+
+* `~markers` (*aruco_pose/MarkerArray*) – list of detected markers with their corners and poses
+* `~visualization` (*visualization_msgs/MarkerArray*) – visualization markers for rviz
+* `~debug` (*sensor_msgs/Image*) – debug image with detected markers
+
+### Published transforms
+
+* `<camera_frame>` => `<frame_id_prefix><id>` – markers' poses
+
+## aruco_map nodelet
+
+`aruco_map` nodelet estimates position of markers map.
+
+### Parameters
+
+* `~map` – path to text file with markers list
+* `~frame_id` – published frame id (default: `aruco_map`)
+* `~known_tilt` – debug image width
+* `~image_width` – debug image width (default: 2000)
+* `~image_height` – debug image height (default: 2000)
+* `~image_margin` – debug image margin (default: 200)
+* `~dictionary` (*int*) – ArUco dictionary (default: 2) - should be the same as `dictionary` parameter of `aruco_detect` nodelet
+
+Map file has one marker per line with the following line format:
+
+```
+marker_id marker_length x y z yaw pitch roll
+```
+
+Where yaw, pitch and roll are extrinsic rotation around Z, Y, X axis, respectively.
+
+See examples in [`map`](map/) directory.
+
+### Topics
+
+#### Subscribed
+
+* `image_raw` (*sensor_msgs/Image*) – camera image (used for debug image)
+* `camera_info` (*sensor_msgs/CameraInfo*) – camera calibration info (used for debug image)
+* `markers` (*aruco_pose/MarkerArray*) – list of markers detected by `aruco_pose` nodelet
+
+#### Published
+
+* `~pose` (*geometry_msgs/PoseWithCovarianceStamped*) – estimated map pose
+* `~image` (*sensor_msgs/Image*) – planarized map image
+* `~visualization` (*visualization_msgs/MarkerArray*) – markers map visualization for rviz
+* `~debug` (*sensor_msgs/Image*) – debug image with detected markers and map axis
+
+### Published transforms
+
+* `<camera_frame>` => `<map_name>` – markers map pose
+
+## Running tests
+
+Command for running tests:
+
+```bash
+catkin_make run_tests && catkin_test_results
+```
+
+## Copyright
+
+Copyright © 2018 Copter Express Technologies. Author: Oleg Kalachev.
+
+Distributed under MIT License (https://opensource.org/licenses/MIT).

aruco_pose/launch/sample.launch

@@ -0,0 +1,26 @@
+<launch>
+    <node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager"/>
+
+    <!-- camera node -->
+    <node pkg="nodelet" type="nodelet" name="main_camera" args="load cv_camera/CvCameraNodelet nodelet_manager">
+        <param name="frame_id" value="main_camera_optical"/>
+        <param name="camera_info_url" value="file://$(find aruco_pose)/test/camera_info.yaml" />
+        <param name="image_width" value="640"/>
+        <param name="image_height" value="480"/>
+    </node>
+
+    <!-- detect aruco markers -->
+    <node pkg="nodelet" clear_params="true" type="nodelet" name="aruco_detect" args="load aruco_pose/aruco_detect nodelet_manager">
+        <remap from="image_raw" to="main_camera/image_raw"/>
+        <remap from="camera_info" to="main_camera/camera_info"/>
+        <param name="length" value="0.33"/>
+    </node>
+
+    <!-- aruco map -->
+    <node pkg="nodelet" clear_params="true" type="nodelet" name="aruco_map" args="load aruco_pose/aruco_map nodelet_manager">
+        <remap from="image_raw" to="main_camera/image_raw"/>
+        <remap from="camera_info" to="main_camera/camera_info"/>
+        <remap from="markers" to="aruco_detect/markers"/>
+        <param name="map" value="$(find aruco_pose)/map/map.txt"/>
+    </node>
+</launch>

aruco_pose/map/cmit.txt

@@ -0,0 +1,100 @@
+0	0.33	0.0	9.0	0	0	0	0	
+1	0.33	1.0	9.0	0	0	0	0	
+2	0.33	2.0	9.0	0	0	0	0	
+3	0.33	3.0	9.0	0	0	0	0	
+4	0.33	4.0	9.0	0	0	0	0	
+5	0.33	5.0	9.0	0	0	0	0	
+6	0.33	6.0	9.0	0	0	0	0	
+7	0.33	7.0	9.0	0	0	0	0	
+8	0.33	8.0	9.0	0	0	0	0	
+9	0.33	9.0	9.0	0	0	0	0	
+10	0.33	0.0	8.0	0	0	0	0	
+11	0.33	1.0	8.0	0	0	0	0	
+12	0.33	2.0	8.0	0	0	0	0	
+13	0.33	3.0	8.0	0	0	0	0	
+14	0.33	4.0	8.0	0	0	0	0	
+15	0.33	5.0	8.0	0	0	0	0	
+16	0.33	6.0	8.0	0	0	0	0	
+#17	0.33	7.0	8.0	0	0	0	0	
+18	0.33	8.0	8.0	0	0	0	0	
+19	0.33	9.0	8.0	0	0	0	0	
+20	0.33	0.0	7.0	0	0	0	0	
+21	0.33	1.0	7.0	0	0	0	0	
+22	0.33	2.0	7.0	0	0	0	0	
+23	0.33	3.0	7.0	0	0	0	0	
+24	0.33	4.0	7.0	0	0	0	0	
+25	0.33	5.0	7.0	0	0	0	0	
+26	0.33	6.0	7.0	0	0	0	0	
+27	0.33	7.0	7.0	0	0	0	0	
+28	0.33	8.0	7.0	0	0	0	0	
+29	0.33	9.0	7.0	0	0	0	0	
+30	0.33	0.0	6.0	0	0	0	0	
+31	0.33	1.0	6.0	0	0	0	0	
+32	0.33	2.0	6.0	0	0	0	0	
+33	0.33	3.0	6.0	0	0	0	0	
+34	0.33	4.0	6.0	0	0	0	0	
+35	0.33	5.0	6.0	0	0	0	0	
+36	0.33	6.0	6.0	0	0	0	0	
+37	0.33	7.0	6.0	0	0	0	0	
+38	0.33	8.0	6.0	0	0	0	0	
+39	0.33	9.0	6.0	0	0	0	0	
+40	0.33	0.0	5.0	0	0	0	0	
+41	0.33	1.0	5.0	0	0	0	0	
+42	0.33	2.0	5.0	0	0	0	0	
+43	0.33	3.0	5.0	0	0	0	0	
+44	0.33	4.0	5.0	0	0	0	0	
+45	0.33	5.0	5.0	0	0	0	0	
+46	0.33	6.0	5.0	0	0	0	0	
+47	0.33	7.0	5.0	0	0	0	0	
+48	0.33	8.0	5.0	0	0	0	0	
+49	0.33	9.0	5.0	0	0	0	0	
+50	0.33	0.0	4.0	0	0	0	0	
+51	0.33	1.0	4.0	0	0	0	0	
+52	0.33	2.0	4.0	0	0	0	0	
+53	0.33	3.0	4.0	0	0	0	0	
+54	0.33	4.0	4.0	0	0	0	0	
+55	0.33	5.0	4.0	0	0	0	0	
+56	0.33	6.0	4.0	0	0	0	0	
+57	0.33	7.0	4.0	0	0	0	0	
+58	0.33	8.0	4.0	0	0	0	0	
+59	0.33	9.0	4.0	0	0	0	0	
+60	0.33	0.0	3.0	0	0	0	0	
+61	0.33	1.0	3.0	0	0	0	0	
+62	0.33	2.0	3.0	0	0	0	0	
+63	0.33	3.0	3.0	0	0	0	0	
+64	0.33	4.0	3.0	0	0	0	0	
+65	0.33	5.0	3.0	0	0	0	0	
+66	0.33	6.0	3.0	0	0	0	0	
+67	0.33	7.0	3.0	0	0	0	0	
+68	0.33	8.0	3.0	0	0	0	0	
+69	0.33	9.0	3.0	0	0	0	0	
+70	0.33	0.0	2.0	0	0	0	0	
+71	0.33	1.0	2.0	0	0	0	0	
+72	0.33	2.0	2.0	0	0	0	0	
+73	0.33	3.0	2.0	0	0	0	0	
+74	0.33	4.0	2.0	0	0	0	0	
+75	0.33	5.0	2.0	0	0	0	0	
+76	0.33	6.0	2.0	0	0	0	0	
+77	0.33	7.0	2.0	0	0	0	0	
+78	0.33	8.0	2.0	0	0	0	0	
+79	0.33	9.0	2.0	0	0	0	0	
+80	0.33	0.0	1.0	0	0	0	0	
+81	0.33	1.0	1.0	0	0	0	0	
+82	0.33	2.0	1.0	0	0	0	0	
+83	0.33	3.0	1.0	0	0	0	0	
+84	0.33	4.0	1.0	0	0	0	0	
+85	0.33	5.0	1.0	0	0	0	0	
+86	0.33	6.0	1.0	0	0	0	0	
+87	0.33	7.0	1.0	0	0	0	0	
+88	0.33	8.0	1.0	0	0	0	0	
+89	0.33	9.0	1.0	0	0	0	0	
+90	0.33	0.0	0.0	0	0	0	0	
+91	0.33	1.0	0.0	0	0	0	0	
+92	0.33	2.0	0.0	0	0	0	0	
+93	0.33	3.0	0.0	0	0	0	0	
+94	0.33	4.0	0.0	0	0	0	0	
+95	0.33	5.0	0.0	0	0	0	0	
+96	0.33	6.0	0.0	0	0	0	0	
+97	0.33	7.0	0.0	0	0	0	0	
+98	0.33	8.0	0.0	0	0	0	0	
+99	0.33	9.0	0.0	0	0	0	0	

aruco_pose/map/map.txt

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

aruco_pose/map/office.txt

@@ -0,0 +1,8 @@
+107	0.33	0	0	0	0	0	0
+106	0.33	0.77	0	0	0	0	0
+105	0.33	0	0.77	0	0	0	0
+104	0.33	0.77	0.77	0	0	0	0
+103	0.33	0	1.54	0	0	0	0
+102	0.33	0.77	1.54	0	0	0	0
+101	0.33	0	2.31	0	0	0	0
+100	0.33	0.77	2.31	0	0	0	0

aruco_pose/map/office_ceiling.txt

@@ -0,0 +1,31 @@
+14	0.365	0.000	0.0	0	0	0	0
+15	0.365	1.335	0.0	0	0	0	0
+30	0.365	2.865	0.0	0	0	0	0
+31	0.365	4.200	0.0	0	0	0	0
+12	0.365	0.000	1.8	0	0	0	0
+13	0.365	1.335	1.8	0	0	0	0
+28	0.365	2.865	1.8	0	0	0	0
+29	0.365	4.200	1.8	0	0	0	0
+10	0.365	0.000	3.6	0	0	0	0
+11	0.365	1.335	3.6	0	0	0	0
+26	0.365	2.865	3.6	0	0	0	0
+27	0.365	4.200	3.6	0	0	0	0
+8	0.365	0.000	5.4	0	0	0	0
+9	0.365	1.335	5.4	0	0	0	0
+24	0.365	2.865	5.4	0	0	0	0
+25	0.365	4.200	5.4	0	0	0	0
+6	0.365	0.000	7.2	0	0	0	0
+7	0.365	1.335	7.2	0	0	0	0
+22	0.365	2.865	7.2	0	0	0	0
+23	0.365	4.200	7.2	0	0	0	0
+4	0.365	0.000	9.0	0	0	0	0
+5	0.365	1.335	9.0	0	0	0	0
+20	0.365	2.865	9.0	0	0	0	0
+21	0.365	4.200	9.0	0	0	0	0
+2	0.365	0.000	10.8	0	0	0	0
+3	0.365	1.335	10.8	0	0	0	0
+18	0.365	2.865	10.8	0	0	0	0
+19	0.365	4.200	10.8	0	0	0	0
+1	0.365	0.000	12.6	0	0	0	0
+0	0.365	1.335	12.6	0	0	0	0
+16	0.365	2.865	12.6	0	0	0	0

aruco_pose/msg/Marker.msg

@@ -0,0 +1,7 @@
+uint32 id
+float32 length
+geometry_msgs/Pose pose
+Point2D c1
+Point2D c2
+Point2D c3
+Point2D c4

aruco_pose/msg/MarkerArray.msg

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

aruco_pose/msg/Point2D.msg

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

aruco_pose/nodelet_plugins.xml

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

aruco_pose/package.xml

@@ -1,31 +1,36 @@
 <?xml version="1.0"?>
-<package>
+<package format="2">
   <name>aruco_pose</name>
-  <version>0.0.0</version>
-  <description>ArUco maps precise pose estimation nodelet</description>
+  <version>0.0.1</version>
+  <description>Positioning with ArUco markers</description>
 
   <maintainer email="okalachev@gmail.com">Oleg Kalachev</maintainer>
   <license>MIT</license>
 
   <!--url type="website">http://wiki.ros.org/aruco_pose</url-->
   <author email="okalachev@gmail.com">Oleg Kalachev</author>
-  <author email="urpylka@gmail.com">Artem Smirnov</author>
 
-  <!-- Use build_depend for packages you need at compile time: -->
-  <build_depend>nodelet</build_depend>
-  <build_depend>roscpp</build_depend>
-  <build_depend>image_transport</build_depend>
-  <build_depend>cv_bridge</build_depend>
-  <build_depend>tf</build_depend>
-  <!-- Use buildtool_depend for build tool packages: -->
   <buildtool_depend>catkin</buildtool_depend>
-  <!-- Use run_depend for packages you need at runtime: -->
-  <run_depend>nodelet</run_depend>
-  <run_depend>roscpp</run_depend>
-  <run_depend>image_transport</run_depend>
-  <run_depend>cv_bridge</run_depend>
-  <!-- Use test_depend for packages you need only for testing: -->
-  <!--   <test_depend>gtest</test_depend> -->
+
+  <depend>roscpp</depend>
+  <depend>nodelet</depend>
+  <depend>tf</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>opencv3</depend>
+  <depend>cv_bridge</depend>
+  <depend>image_transport</depend>
+  <depend>message_generation</depend>
+  <depend>message_runtime</depend>
+  <depend>std_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>visualization_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>rostest</depend>
+
+  <test_depend>image_publisher</test_depend>
+  <test_depend>ros_pytest</test_depend>
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>

aruco_pose/src/aruco_detect.cpp

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

aruco_pose/src/aruco_map.cpp

@@ -0,0 +1,516 @@
+/*
+ * Detecting and pose estimation of ArUco markers maps
+ * Copyright (C) 2018 Copter Express Technologies
+ *
+ * Author: Oleg Kalachev <okalachev@gmail.com>
+ *
+ * Distributed under MIT License (available at https://opensource.org/licenses/MIT).
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ */
+
+/*
+ * Code is based on https://github.com/UbiquityRobotics/fiducials, which is distributed
+ * under the BSD license.
+ */
+
+#include <math.h>
+#include <string>
+#include <vector>
+#include <fstream>
+#include <algorithm>
+#include <ros/ros.h>
+#include <nodelet/nodelet.h>
+#include <pluginlib/class_list_macros.h>
+#include <image_transport/image_transport.h>
+#include <cv_bridge/cv_bridge.h>
+#include <tf/transform_datatypes.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include <tf2_ros/static_transform_broadcaster.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <message_filters/subscriber.h>
+#include <message_filters/synchronizer.h>
+#include <message_filters/sync_policies/exact_time.h>
+#include <geometry_msgs/TransformStamped.h>
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
+#include <sensor_msgs/Image.h>
+#include <visualization_msgs/Marker.h>
+#include <visualization_msgs/MarkerArray.h>
+
+#include <aruco_pose/MarkerArray.h>
+#include <aruco_pose/Marker.h>
+
+#include <opencv2/opencv.hpp>
+#include <opencv2/aruco.hpp>
+
+#include "draw.h"
+#include "utils.h"
+
+using std::vector;
+using cv::Mat;
+using sensor_msgs::Image;
+using sensor_msgs::CameraInfo;
+using aruco_pose::MarkerArray;
+
+typedef message_filters::sync_policies::ExactTime<Image, CameraInfo, MarkerArray> SyncPolicy;
+
+class ArucoMap : public nodelet::Nodelet {
+private:
+	ros::NodeHandle nh_, nh_priv_;
+	ros::Publisher img_pub_, pose_pub_, markers_pub_, vis_markers_pub_;
+	image_transport::Publisher debug_pub_;
+	message_filters::Subscriber<Image> image_sub_;
+	message_filters::Subscriber<CameraInfo> info_sub_;
+	message_filters::Subscriber<MarkerArray> markers_sub_;
+	boost::shared_ptr<message_filters::Synchronizer<SyncPolicy> > sync_;
+	cv::Ptr<cv::aruco::Board> board_;
+	Mat camera_matrix_, dist_coeffs_;
+	geometry_msgs::TransformStamped transform_;
+	geometry_msgs::PoseWithCovarianceStamped pose_;
+	vector<geometry_msgs::TransformStamped> markers_transforms_;
+	aruco_pose::MarkerArray markers_;
+	tf2_ros::TransformBroadcaster br_;
+	tf2_ros::StaticTransformBroadcaster static_br_;
+	tf2_ros::Buffer tf_buffer_;
+	tf2_ros::TransformListener tf_listener_{tf_buffer_};
+	visualization_msgs::MarkerArray vis_array_;
+	std::string known_tilt_, map_, markers_frame_, markers_parent_frame_;
+	int image_width_, image_height_, image_margin_;
+	bool auto_flip_, image_axis_;
+
+public:
+	virtual void onInit()
+	{
+		nh_ = getNodeHandle();
+		nh_priv_ = getPrivateNodeHandle();
+
+		image_transport::ImageTransport it_priv(nh_priv_);
+
+		// TODO: why image_transport doesn't work here?
+		img_pub_ = nh_priv_.advertise<sensor_msgs::Image>("image", 1, true);
+		markers_pub_ = nh_priv_.advertise<aruco_pose::MarkerArray>("markers", 1, true);
+
+		board_ = cv::makePtr<cv::aruco::Board>();
+		board_->dictionary = cv::aruco::getPredefinedDictionary(
+			                 static_cast<cv::aruco::PREDEFINED_DICTIONARY_NAME>(nh_priv_.param("dictionary", 2)));
+		camera_matrix_ = cv::Mat::zeros(3, 3, CV_64F);
+		dist_coeffs_ = cv::Mat::zeros(8, 1, CV_64F);
+
+		std::string type, map;
+		nh_priv_.param<std::string>("type", type, "map");
+		nh_priv_.param<std::string>("frame_id", transform_.child_frame_id, "aruco_map");
+		nh_priv_.param<std::string>("known_tilt", known_tilt_, "");
+		nh_priv_.param("auto_flip", auto_flip_, false);
+		nh_priv_.param("image_width", image_width_, 2000);
+		nh_priv_.param("image_height", image_height_, 2000);
+		nh_priv_.param("image_margin", image_margin_, 200);
+		nh_priv_.param("image_axis", image_axis_, true);
+		nh_priv_.param<std::string>("markers/frame_id", markers_parent_frame_, transform_.child_frame_id);
+		nh_priv_.param<std::string>("markers/child_frame_id_prefix", markers_frame_, "");
+
+		// createStripLine();
+
+		if (type == "map") {
+			param(nh_priv_, "map", map);
+			loadMap(map);
+		} else if (type == "gridboard") {
+			createGridBoard();
+		} else {
+			NODELET_FATAL("unknown type: %s", type.c_str());
+			ros::shutdown();
+		}
+
+		pose_pub_ = nh_priv_.advertise<geometry_msgs::PoseWithCovarianceStamped>("pose", 1);
+		vis_markers_pub_ = nh_priv_.advertise<visualization_msgs::MarkerArray>("visualization", 1, true);
+		debug_pub_ = it_priv.advertise("debug", 1);
+
+		image_sub_.subscribe(nh_, "image_raw", 1);
+		info_sub_.subscribe(nh_, "camera_info", 1);
+		markers_sub_.subscribe(nh_, "markers", 1);
+
+		sync_.reset(new message_filters::Synchronizer<SyncPolicy>(SyncPolicy(10), image_sub_, info_sub_, markers_sub_));
+		sync_->registerCallback(boost::bind(&ArucoMap::callback, this, _1, _2, _3));
+
+		publishMarkersFrames();
+		publishMarkers();
+		publishMapImage();
+		vis_markers_pub_.publish(vis_array_);
+
+		NODELET_INFO("ready");
+	}
+
+	void callback(const sensor_msgs::ImageConstPtr& image,
+	              const sensor_msgs::CameraInfoConstPtr& cinfo,
+	              const aruco_pose::MarkerArrayConstPtr& markers)
+	{
+		int valid = 0;
+		int count = markers->markers.size();
+		std::vector<int> ids;
+		std::vector<std::vector<cv::Point2f>> corners;
+		cv::Vec3d rvec, tvec;
+
+		parseCameraInfo(cinfo, camera_matrix_, dist_coeffs_);
+		if (markers->markers.empty()) goto publish_debug;
+
+		ids.reserve(count);
+		corners.reserve(count);
+
+		for(auto const &marker : markers->markers) {
+			ids.push_back(marker.id);
+			std::vector<cv::Point2f> marker_corners = {
+				cv::Point2f(marker.c1.x, marker.c1.y),
+				cv::Point2f(marker.c2.x, marker.c2.y),
+				cv::Point2f(marker.c3.x, marker.c3.y),
+				cv::Point2f(marker.c4.x, marker.c4.y)
+			};
+			corners.push_back(marker_corners);
+		}
+
+		if (known_tilt_.empty()) {
+			// simple estimation
+			valid = cv::aruco::estimatePoseBoard(corners, ids, board_, camera_matrix_, dist_coeffs_,
+			                                     rvec, tvec, false);
+			if (!valid) goto publish_debug;
+
+			transform_.header.stamp = markers->header.stamp;
+			transform_.header.frame_id = markers->header.frame_id;
+			pose_.header = transform_.header;
+			fillPose(pose_.pose.pose, rvec, tvec);
+			fillTransform(transform_.transform, rvec, tvec);
+
+		} else {
+			Mat obj_points, img_points;
+			// estimation with "snapping"
+			cv::aruco::getBoardObjectAndImagePoints(board_, corners, ids, obj_points, img_points);
+			if (obj_points.empty()) goto publish_debug;
+
+			double center_x = 0, center_y = 0, center_z = 0;
+			alignObjPointsToCenter(obj_points, center_x, center_y, center_z);
+
+			valid = solvePnP(obj_points, img_points, camera_matrix_, dist_coeffs_, rvec, tvec, false);
+			if (!valid) goto publish_debug;
+
+			fillTransform(transform_.transform, rvec, tvec);
+			try {
+				geometry_msgs::TransformStamped snap_to = tf_buffer_.lookupTransform(markers->header.frame_id,
+				                                          known_tilt_, markers->header.stamp, ros::Duration(0.02));
+				snapOrientation(transform_.transform.rotation, snap_to.transform.rotation, auto_flip_);
+			} catch (const tf2::TransformException& e) {
+				NODELET_WARN_THROTTLE(1, "can't snap: %s", e.what());
+			}
+
+			geometry_msgs::TransformStamped shift;
+			shift.transform.translation.x = -center_x;
+			shift.transform.translation.y = -center_y;
+			shift.transform.translation.z = -center_z;
+			shift.transform.rotation.w = 1;
+			tf2::doTransform(shift, transform_, transform_);
+
+			// for debug topic
+			tvec[0] = transform_.transform.translation.x;
+			tvec[1] = transform_.transform.translation.y;
+			tvec[2] = transform_.transform.translation.z;
+
+			transform_.header.stamp = markers->header.stamp;
+			transform_.header.frame_id = markers->header.frame_id;
+			pose_.header = transform_.header;
+			transformToPose(transform_.transform, pose_.pose.pose);
+		}
+
+		if (!transform_.child_frame_id.empty()) {
+			br_.sendTransform(transform_);
+		}
+		pose_pub_.publish(pose_);
+
+publish_debug:
+		// publish debug image (even if no map detected)
+		if (debug_pub_.getNumSubscribers() > 0) {
+			Mat mat = cv_bridge::toCvCopy(image, "bgr8")->image; // copy image as we're planning to modify it
+			cv::aruco::drawDetectedMarkers(mat, corners, ids); // draw detected markers
+			if (valid) {
+				_drawAxis(mat, camera_matrix_, dist_coeffs_, rvec, tvec, 1.0); // draw board axis
+			}
+			cv_bridge::CvImage out_msg;
+			out_msg.header.frame_id = image->header.frame_id;
+			out_msg.header.stamp = image->header.stamp;
+			out_msg.encoding = sensor_msgs::image_encodings::BGR8;
+			out_msg.image = mat;
+			debug_pub_.publish(out_msg.toImageMsg());
+		}
+	}
+
+	void alignObjPointsToCenter(Mat &obj_points, double &center_x, double &center_y, double &center_z) const
+	{
+		// Align object points to the center of mass
+		double sum_x = 0;
+		double sum_y = 0;
+		double sum_z = 0;
+
+		for (int i = 0; i < obj_points.rows; i++) {
+			sum_x += obj_points.at<float>(i, 0);
+			sum_y += obj_points.at<float>(i, 1);
+			sum_z += obj_points.at<float>(i, 2);
+		}
+
+		center_x = sum_x / obj_points.rows;
+		center_y = sum_y / obj_points.rows;
+		center_z = sum_z / obj_points.rows;
+
+		for (int i = 0; i < obj_points.rows; i++) {
+			obj_points.at<float>(i, 0) -= center_x;
+			obj_points.at<float>(i, 1) -= center_y;
+			obj_points.at<float>(i, 2) -= center_z;
+		}
+	}
+
+	void loadMap(std::string filename)
+	{
+		std::ifstream f(filename);
+		std::string line;
+
+		if (!f.good()) {
+			NODELET_FATAL("%s - %s", strerror(errno), filename.c_str());
+			ros::shutdown();
+		}
+
+		while (std::getline(f, line)) {
+			int id;
+			double length, x, y, z, yaw, pitch, roll;
+
+			std::istringstream s(line);
+
+			// Read first character to see whether it's a comment
+			char first = 0;
+			if (!(s >> first)) {
+				// No non-whitespace characters, must be a blank line
+				continue;
+			}
+
+			if (first == '#') {
+				NODELET_DEBUG("Skipping line as a comment: %s", line.c_str());
+				continue;
+			} else if (isdigit(first)) {
+				// Put the digit back into the stream
+				// Note that this is a non-modifying putback, so this should work with istreams
+				// (see https://en.cppreference.com/w/cpp/io/basic_istream/putback)
+				s.putback(first);
+			} else {
+				// Probably garbage data; inform user and throw an exception, possibly killing nodelet
+				NODELET_FATAL("Malformed input: %s", line.c_str());
+				ros::shutdown();
+				throw std::runtime_error("Malformed input");
+			}
+
+			if (!(s >> id >> length >> x >> y)) {
+				NODELET_ERROR("Not enough data in line: %s; "
+				          "Each marker must have at least id, length, x, y fields", line.c_str());
+				continue;
+			}
+			// Be less strict about z, yaw, pitch roll
+			if (!(s >> z)) {
+				NODELET_DEBUG("No z coordinate provided for marker %d, assuming 0", id);
+				z = 0;
+			}
+			if (!(s >> yaw)) {
+				NODELET_DEBUG("No yaw provided for marker %d, assuming 0", id);
+				yaw = 0;
+			}
+			if (!(s >> pitch)) {
+				NODELET_DEBUG("No pitch provided for marker %d, assuming 0", id);
+				pitch = 0;
+			}
+			if (!(s >> roll)) {
+				NODELET_DEBUG("No roll provided for marker %d, assuming 0", id);
+				roll = 0;
+			}
+			addMarker(id, length, x, y, z, yaw, pitch, roll);
+		}
+
+		NODELET_INFO("loading %s complete (%d markers)", filename.c_str(), static_cast<int>(board_->ids.size()));
+	}
+
+	void createGridBoard()
+	{
+		NODELET_INFO("generate gridboard");
+		NODELET_WARN("gridboard maps are deprecated");
+
+		int markers_x, markers_y, first_marker;
+		double markers_side, markers_sep_x, markers_sep_y;
+		std::vector<int> marker_ids;
+		nh_priv_.param<int>("markers_x", markers_x, 10);
+		nh_priv_.param<int>("markers_y", markers_y, 10);
+		nh_priv_.param<int>("first_marker", first_marker, 0);
+
+		param(nh_priv_, "markers_side", markers_side);
+		param(nh_priv_, "markers_sep_x", markers_sep_x);
+		param(nh_priv_, "markers_sep_y", markers_sep_y);
+
+		if (nh_priv_.getParam("marker_ids", marker_ids)) {
+			if ((unsigned int)(markers_x * markers_y) != marker_ids.size()) {
+				NODELET_FATAL("~marker_ids length should be equal to ~markers_x * ~markers_y");
+				ros::shutdown();
+			}
+		} else {
+			// Fill marker_ids automatically
+			marker_ids.resize(markers_x * markers_y);
+			for (int i = 0; i < markers_x * markers_y; i++)
+			{
+				marker_ids.at(i) = first_marker++;
+			}
+		}
+
+		double max_y = markers_y * markers_side + (markers_y - 1) * markers_sep_y;
+		for(int y = 0; y < markers_y; y++) {
+			for(int x = 0; x < markers_x; x++) {
+				double x_pos = x * (markers_side + markers_sep_x);
+				double y_pos = max_y - y * (markers_side + markers_sep_y) - markers_side;
+				NODELET_INFO("add marker %d %g %g", marker_ids[y * markers_y + x], x_pos, y_pos);
+				addMarker(marker_ids[y * markers_y + x], markers_side, x_pos, y_pos, 0, 0, 0, 0);
+			}
+		}
+	}
+
+	// void createStripLine()
+	// {
+	// 	visualization_msgs::Marker marker;
+	// 	marker.header.frame_id = transform_.child_frame_id;
+	// 	marker.action = visualization_msgs::Marker::ADD;
+	// 	marker.ns = "aruco_map_link";
+	// 	marker.type = visualization_msgs::Marker::LINE_STRIP;
+	// 	marker.scale.x = 0.02;
+	// 	marker.color.g = 1;
+	// 	marker.color.a = 0.8;
+	// 	marker.frame_locked = true;
+	// 	marker.pose.orientation.w = 1;
+	// 	vis_array_.markers.push_back(marker);
+	// }
+
+	void addMarker(int id, double length, double x, double y, double z,
+				   double yaw, double pitch, double roll)
+	{
+		// Check whether the id is in range for current dictionary
+		int num_markers = board_->dictionary->bytesList.rows;
+		if (num_markers <= id) {
+			NODELET_ERROR("Marker id %d is not in dictionary; current dictionary contains %d markers. "
+			              "Please see https://github.com/CopterExpress/clever/blob/master/aruco_pose/README.md#parameters for details",
+					  id, num_markers);
+			return;
+		}
+		// Check if marker is already in the board
+		if (std::count(board_->ids.begin(), board_->ids.end(), id) > 0) {
+			NODELET_ERROR("Marker id %d is already in the map", id);
+			return;
+		}
+		// Create transform
+		tf::Quaternion q;
+		q.setRPY(roll, pitch, yaw);
+		tf::Transform transform(q, tf::Vector3(x, y, z));
+
+		/* marker's corners:
+			0    1
+			3    2
+		*/
+		double halflen = length / 2;
+		tf::Point p0(-halflen, halflen, 0);
+		tf::Point p1(halflen, halflen, 0);
+		tf::Point p2(halflen, -halflen, 0);
+		tf::Point p3(-halflen, -halflen, 0);
+		p0 = transform * p0;
+		p1 = transform * p1;
+		p2 = transform * p2;
+		p3 = transform * p3;
+
+		vector<cv::Point3f> obj_points = {
+			cv::Point3f(p0.x(), p0.y(), p0.z()),
+			cv::Point3f(p1.x(), p1.y(), p1.z()),
+			cv::Point3f(p2.x(), p2.y(), p2.z()),
+			cv::Point3f(p3.x(), p3.y(), p3.z())
+		};
+
+		board_->ids.push_back(id);
+		board_->objPoints.push_back(obj_points);
+
+		// Add marker's static transform
+		if (!markers_frame_.empty()) {
+			geometry_msgs::TransformStamped marker_transform;
+			marker_transform.header.frame_id = markers_parent_frame_;
+			marker_transform.child_frame_id = markers_frame_ + std::to_string(id);
+			tf::transformTFToMsg(transform, marker_transform.transform);
+			markers_transforms_.push_back(marker_transform);
+		}
+
+		// Add marker to array
+		aruco_pose::Marker marker;
+		marker.id = id;
+		marker.length = length;
+		marker.pose.position.x = x;
+		marker.pose.position.y = y;
+		marker.pose.position.z = z;
+		tf::quaternionTFToMsg(q, marker.pose.orientation);
+		markers_.markers.push_back(marker);
+
+		// Add visualization marker
+		visualization_msgs::Marker vis_marker;
+		vis_marker.header.frame_id = transform_.child_frame_id;
+		vis_marker.action = visualization_msgs::Marker::ADD;
+		vis_marker.id = vis_array_.markers.size();
+		vis_marker.ns = "aruco_map_marker";
+		vis_marker.type = visualization_msgs::Marker::CUBE;
+		vis_marker.scale.x = length;
+		vis_marker.scale.y = length;
+		vis_marker.scale.z = 0.001;
+		vis_marker.color.r = 1;
+		vis_marker.color.g = 0.5;
+		vis_marker.color.b = 0.5;
+		vis_marker.color.a = 0.8;
+		vis_marker.pose.position.x = x;
+		vis_marker.pose.position.y = y;
+		vis_marker.pose.position.z = z;
+		tf::quaternionTFToMsg(q, marker.pose.orientation);
+		vis_marker.frame_locked = true;
+		vis_array_.markers.push_back(vis_marker);
+
+		// Add linking line
+		// geometry_msgs::Point p;
+		// p.x = x;
+		// p.y = y;
+		// p.z = z;
+		// vis_array_.markers.at(0).points.push_back(p);
+	}
+
+	void publishMarkersFrames()
+	{
+		if (!markers_transforms_.empty()) {
+			static_br_.sendTransform(markers_transforms_);
+		}
+	}
+
+	void publishMarkers()
+	{
+		markers_pub_.publish(markers_);
+	}
+
+	void publishMapImage()
+	{
+		cv::Size size(image_width_, image_height_);
+		cv::Mat image;
+		cv_bridge::CvImage msg;
+
+		if (!board_->ids.empty()) {
+			_drawPlanarBoard(board_, size, image, image_margin_, 1, image_axis_);
+			msg.encoding = image_axis_ ? sensor_msgs::image_encodings::RGB8 : sensor_msgs::image_encodings::MONO8;
+		} else {
+			// empty map
+			image.create(size, CV_8UC1);
+			image.setTo(cv::Scalar::all(255));
+			msg.encoding = sensor_msgs::image_encodings::MONO8;
+		}
+
+		msg.image = image;
+		img_pub_.publish(msg.toImageMsg());
+	}
+};
+
+PLUGINLIB_EXPORT_CLASS(ArucoMap, nodelet::Nodelet)

aruco_pose/src/aruco_pose.cpp

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

aruco_pose/src/draw.cpp

@@ -0,0 +1,832 @@
+// This code is basically taken from https://github.com/opencv/opencv_contrib/blob/master/modules/aruco/src/aruco.cpp
+// with some improvements and fixes
+
+#include "draw.h"
+#include <math.h>
+
+using namespace cv;
+using namespace cv::aruco;
+
+static void _cvProjectPoints2( const CvMat* object_points, const CvMat* rotation_vector,
+                       const CvMat* translation_vector, const CvMat* camera_matrix,
+                       const CvMat* distortion_coeffs, CvMat* image_points,
+                       CvMat* dpdrot CV_DEFAULT(NULL), CvMat* dpdt CV_DEFAULT(NULL),
+                       CvMat* dpdf CV_DEFAULT(NULL), CvMat* dpdc CV_DEFAULT(NULL),
+                       CvMat* dpddist CV_DEFAULT(NULL),
+                       double aspect_ratio CV_DEFAULT(0));
+
+static void _projectPoints( InputArray objectPoints,
+                    InputArray rvec, InputArray tvec,
+                    InputArray cameraMatrix, InputArray distCoeffs,
+                    OutputArray imagePoints,
+                    OutputArray jacobian = noArray(),
+                    double aspectRatio = 0 );
+
+
+void _drawPlanarBoard(Board *_board, Size outSize, OutputArray _img, int marginSize,
+                      int borderBits, bool drawAxis) {
+
+	CV_Assert(outSize.area() > 0);
+	CV_Assert(marginSize >= 0);
+
+	_img.create(outSize, drawAxis ? CV_8UC3 : CV_8UC1);
+	Mat out = _img.getMat();
+	out.setTo(Scalar::all(255));
+	out.adjustROI(-marginSize, -marginSize, -marginSize, -marginSize);
+
+	// calculate max and min values in XY plane
+	CV_Assert(_board->objPoints.size() > 0);
+	float minX, maxX, minY, maxY;
+	minX = maxX = _board->objPoints[0][0].x;
+	minY = maxY = _board->objPoints[0][0].y;
+
+	for(unsigned int i = 0; i < _board->objPoints.size(); i++) {
+		for(int j = 0; j < 4; j++) {
+			minX = min(minX, _board->objPoints[i][j].x);
+			maxX = max(maxX, _board->objPoints[i][j].x);
+			minY = min(minY, _board->objPoints[i][j].y);
+			maxY = max(maxY, _board->objPoints[i][j].y);
+		}
+	}
+
+	float sizeX = maxX - minX;
+	float sizeY = maxY - minY;
+
+	// proportion transformations
+	float xReduction = sizeX / float(out.cols);
+	float yReduction = sizeY / float(out.rows);
+
+	// determine the zone where the markers are placed
+	if(xReduction > yReduction) {
+		int nRows = int(sizeY / xReduction);
+		int rowsMargins = (out.rows - nRows) / 2;
+		out.adjustROI(-rowsMargins, -rowsMargins, 0, 0);
+	} else {
+		int nCols = int(sizeX / yReduction);
+		int colsMargins = (out.cols - nCols) / 2;
+		out.adjustROI(0, 0, -colsMargins, -colsMargins);
+	}
+
+	// now paint each marker
+	Dictionary &dictionary = *(_board->dictionary);
+	Mat marker;
+	Point2f outCorners[3];
+	Point2f inCorners[3];
+	for(unsigned int m = 0; m < _board->objPoints.size(); m++) {
+		// transform corners to markerZone coordinates
+		for(int j = 0; j < 3; j++) {
+			Point2f pf = Point2f(_board->objPoints[m][j].x, _board->objPoints[m][j].y);
+			// move top left to 0, 0
+			pf -= Point2f(minX, minY);
+			pf.x = pf.x / sizeX * float(out.cols);
+			pf.y = (1.0f - pf.y / sizeY) * float(out.rows);
+			outCorners[j] = pf;
+		}
+
+		// get marker
+		Size dst_sz(outCorners[2] - outCorners[0]); // assuming CCW order
+		// dst_sz.width = dst_sz.height = std::min(dst_sz.width, dst_sz.height); //marker should be square
+		double diag = std::round(std::hypot(dst_sz.width, dst_sz.height));
+		int side = std::round(diag / std::sqrt(2));
+		side = std::max(side, 10);
+
+		dictionary.drawMarker(_board->ids[m], side, marker, borderBits);
+		if (drawAxis) {
+			cvtColor(marker, marker, COLOR_GRAY2RGB);
+		}
+
+		// interpolate tiny marker to marker position in markerZone
+		inCorners[0] = Point2f(-0.5f, -0.5f);
+		inCorners[1] = Point2f(marker.cols - 0.5f, -0.5f);
+		inCorners[2] = Point2f(marker.cols - 0.5f, marker.rows - 0.5f);
+
+		// remove perspective
+		Mat transformation = getAffineTransform(inCorners, outCorners);
+		warpAffine(marker, out, transformation, out.size(), INTER_LINEAR,
+						BORDER_TRANSPARENT);
+	}
+
+	// draw axis
+	if (drawAxis) {
+		Size wholeSize; Point ofs;
+		out.locateROI(wholeSize, ofs);
+		auto out_copy = _img.getMat();
+
+		cv::Point center(ofs.x - minX / sizeX * float(out.cols), ofs.y + out.rows + minY / sizeY * float(out.rows));
+
+		int axis_points[3][2] = {{300, 0}, {0, -300}, {-150, 150}};
+		Point axis_names[3] = {Point(270, 50), Point(25, -270), Point(-160, 115)};
+		Scalar colors[] = {Scalar(255, 0, 0), Scalar(0, 255, 0), Scalar(0, 0, 255)};
+		String names[] = {"X", "Y", "Z"};
+
+		int r_half = 14;
+		int height = 55;
+
+		for(int poly = 2; poly >= 0; poly--){
+			double alpha = atan2(0 - axis_points[poly][0], 0 - axis_points[poly][1]);
+			float x_delta = r_half * cos(alpha);
+			float y_delta = r_half * sin(alpha);
+
+			Point polygon_vertices[1][3];
+			polygon_vertices[0][0] = center + Point(axis_points[poly][0] + x_delta, axis_points[poly][1] - y_delta);
+			polygon_vertices[0][1] = center + Point(axis_points[poly][0] - x_delta, axis_points[poly][1] + y_delta);
+			polygon_vertices[0][2] = center + Point(axis_points[poly][0] - sin(alpha) * height, axis_points[poly][1] - cos(alpha) * height);
+
+			const Point* ppt[1] = {polygon_vertices[0]};
+			int npt[] = {3};
+
+			fillPoly(out_copy, ppt, npt, 1, colors[poly]);
+			putText(out_copy, names[poly], center + axis_names[poly], FONT_HERSHEY_SIMPLEX, 1.2, colors[poly], 7);
+			line(out_copy, center, center + Point(axis_points[poly][0], axis_points[poly][1]), colors[poly], 10);
+		}
+	}
+}
+
+/* Draw a (potentially partially visible) line. */
+static void linePartial(InputOutputArray image, Point3f p1, Point3f p2, const Scalar& color,
+                        int thickness = 1, int lineType = LINE_8, int shift = 0)
+{
+	// If both points are behind the screen, don't draw anything
+	if (p1.z <= 0 && p2.z <= 0) {
+		return;
+	}
+	Point2f p1p{p1.x, p1.y};
+	Point2f p2p{p2.x, p2.y};
+	// If points are on the different sides of the plane, compute intersection point
+	if (p1.z * p2.z < 0) {
+		// Compute intersection point with the screen
+		// We denote alpha as such:
+		// xi = (1 - alpha) * x1 + alpha * x2
+		// yi = (1 - alpha) * y1 + alpha * y2
+		// zi = (1 - alpha) * z1 + alpha * z2 = 0
+		// Thus, alpha can be expressed as
+		// alpha = z1 / (z1 - z2)
+		float alpha = p1.z / (p1.z - p2.z);
+		Point2f pi{(1 - alpha) * p1.x + alpha * p2.x, (1 - alpha) * p1.y + alpha * p2.y};
+		// Now, if z1 is negative, we draw the line from (xi, yi) to (x2, y2), else we draw from (x1, y1) to (xi, yi)
+		if (p1.z < 0) {
+			p1p = pi;
+		} else {
+			p2p = pi;
+		}
+	}
+	line(image, p1p, p2p, color, thickness, lineType, shift);
+}
+
+void _drawAxis(InputOutputArray _image, InputArray _cameraMatrix, InputArray _distCoeffs,
+              InputArray _rvec, InputArray _tvec, float length) {
+
+	CV_Assert(_image.getMat().total() != 0 &&
+			  (_image.getMat().channels() == 1 || _image.getMat().channels() == 3));
+	CV_Assert(length > 0);
+
+	// project axis points
+	std::vector<Point3f> axisPoints;
+	axisPoints.push_back(Point3f(0, 0, 0));
+	axisPoints.push_back(Point3f(length, 0, 0));
+	axisPoints.push_back(Point3f(0, length, 0));
+	axisPoints.push_back(Point3f(0, 0, length));
+	std::vector<Point3f> imagePointsZ;
+	_projectPoints(axisPoints, _rvec, _tvec, _cameraMatrix, _distCoeffs, imagePointsZ);
+
+	// draw axis lines
+	linePartial(_image, imagePointsZ[0], imagePointsZ[1], Scalar(0, 0, 255), 3);
+	linePartial(_image, imagePointsZ[0], imagePointsZ[2], Scalar(0, 255, 0), 3);
+	linePartial(_image, imagePointsZ[0], imagePointsZ[3], Scalar(255, 0, 0), 3);
+}
+
+static CvMat _cvMat(const cv::Mat& m)
+{
+	CvMat self;
+	CV_DbgAssert(m.dims <= 2);
+	self = cvMat(m.rows, m.dims == 1 ? 1 : m.cols, m.type(), m.data);
+	self.step = (int)m.step[0];
+	self.type = (self.type & ~cv::Mat::CONTINUOUS_FLAG) | (m.flags & cv::Mat::CONTINUOUS_FLAG);
+	return self;
+}
+
+static void _projectPoints( InputArray _opoints,
+                        InputArray _rvec,
+                        InputArray _tvec,
+                        InputArray _cameraMatrix,
+                        InputArray _distCoeffs,
+                        OutputArray _ipoints,
+                        OutputArray _jacobian,
+                        double aspectRatio )
+{
+	Mat opoints = _opoints.getMat();
+	int npoints = opoints.checkVector(3), depth = opoints.depth();
+	CV_Assert(npoints >= 0 && (depth == CV_32F || depth == CV_64F));
+
+	CvMat dpdrot, dpdt, dpdf, dpdc, dpddist;
+	CvMat *pdpdrot = 0, *pdpdt = 0, *pdpdf = 0, *pdpdc = 0, *pdpddist = 0;
+
+	CV_Assert(_ipoints.needed());
+
+	_ipoints.create(npoints, 1, CV_MAKETYPE(depth, 3), -1, true);
+	Mat imagePoints = _ipoints.getMat();
+	CvMat c_imagePoints = _cvMat(imagePoints);
+	CvMat c_objectPoints = _cvMat(opoints);
+	Mat cameraMatrix = _cameraMatrix.getMat();
+
+	Mat rvec = _rvec.getMat(), tvec = _tvec.getMat();
+	CvMat c_cameraMatrix = _cvMat(cameraMatrix);
+	CvMat c_rvec = _cvMat(rvec), c_tvec = _cvMat(tvec);
+
+	double dc0buf[5] = {0};
+	Mat dc0(5, 1, CV_64F, dc0buf);
+	Mat distCoeffs = _distCoeffs.getMat();
+	if (distCoeffs.empty())
+		distCoeffs = dc0;
+	CvMat c_distCoeffs = _cvMat(distCoeffs);
+	int ndistCoeffs = distCoeffs.rows + distCoeffs.cols - 1;
+
+	Mat jacobian;
+	if (_jacobian.needed())
+	{
+		_jacobian.create(npoints * 2, 3 + 3 + 2 + 2 + ndistCoeffs, CV_64F);
+		jacobian = _jacobian.getMat();
+		pdpdrot = &(dpdrot = _cvMat(jacobian.colRange(0, 3)));
+		pdpdt = &(dpdt = _cvMat(jacobian.colRange(3, 6)));
+		pdpdf = &(dpdf = _cvMat(jacobian.colRange(6, 8)));
+		pdpdc = &(dpdc = _cvMat(jacobian.colRange(8, 10)));
+		pdpddist = &(dpddist = _cvMat(jacobian.colRange(10, 10 + ndistCoeffs)));
+	}
+
+	_cvProjectPoints2(&c_objectPoints, &c_rvec, &c_tvec, &c_cameraMatrix, &c_distCoeffs,
+					  &c_imagePoints, pdpdrot, pdpdt, pdpdf, pdpdc, pdpddist, aspectRatio);
+}
+
+namespace _detail
+{
+    template <typename FLOAT>
+    void computeTiltProjectionMatrix(FLOAT tauX,
+                                     FLOAT tauY,
+                                     Matx<FLOAT, 3, 3>* matTilt = 0,
+                                     Matx<FLOAT, 3, 3>* dMatTiltdTauX = 0,
+                                     Matx<FLOAT, 3, 3>* dMatTiltdTauY = 0,
+                                     Matx<FLOAT, 3, 3>* invMatTilt = 0)
+    {
+        FLOAT cTauX = cos(tauX);
+        FLOAT sTauX = sin(tauX);
+        FLOAT cTauY = cos(tauY);
+        FLOAT sTauY = sin(tauY);
+        Matx<FLOAT, 3, 3> matRotX = Matx<FLOAT, 3, 3>(1,0,0,0,cTauX,sTauX,0,-sTauX,cTauX);
+        Matx<FLOAT, 3, 3> matRotY = Matx<FLOAT, 3, 3>(cTauY,0,-sTauY,0,1,0,sTauY,0,cTauY);
+        Matx<FLOAT, 3, 3> matRotXY = matRotY * matRotX;
+        Matx<FLOAT, 3, 3> matProjZ = Matx<FLOAT, 3, 3>(matRotXY(2,2),0,-matRotXY(0,2),0,matRotXY(2,2),-matRotXY(1,2),0,0,1);
+        if (matTilt)
+        {
+            // Matrix for trapezoidal distortion of tilted image sensor
+            *matTilt = matProjZ * matRotXY;
+        }
+        if (dMatTiltdTauX)
+        {
+            // Derivative with respect to tauX
+            Matx<FLOAT, 3, 3> dMatRotXYdTauX = matRotY * Matx<FLOAT, 3, 3>(0,0,0,0,-sTauX,cTauX,0,-cTauX,-sTauX);
+            Matx<FLOAT, 3, 3> dMatProjZdTauX = Matx<FLOAT, 3, 3>(dMatRotXYdTauX(2,2),0,-dMatRotXYdTauX(0,2),
+                                                                 0,dMatRotXYdTauX(2,2),-dMatRotXYdTauX(1,2),0,0,0);
+            *dMatTiltdTauX = (matProjZ * dMatRotXYdTauX) + (dMatProjZdTauX * matRotXY);
+        }
+        if (dMatTiltdTauY)
+        {
+            // Derivative with respect to tauY
+            Matx<FLOAT, 3, 3> dMatRotXYdTauY = Matx<FLOAT, 3, 3>(-sTauY,0,-cTauY,0,0,0,cTauY,0,-sTauY) * matRotX;
+            Matx<FLOAT, 3, 3> dMatProjZdTauY = Matx<FLOAT, 3, 3>(dMatRotXYdTauY(2,2),0,-dMatRotXYdTauY(0,2),
+                                                                 0,dMatRotXYdTauY(2,2),-dMatRotXYdTauY(1,2),0,0,0);
+            *dMatTiltdTauY = (matProjZ * dMatRotXYdTauY) + (dMatProjZdTauY * matRotXY);
+        }
+        if (invMatTilt)
+        {
+            FLOAT inv = 1./matRotXY(2,2);
+            Matx<FLOAT, 3, 3> invMatProjZ = Matx<FLOAT, 3, 3>(inv,0,inv*matRotXY(0,2),0,inv,inv*matRotXY(1,2),0,0,1);
+            *invMatTilt = matRotXY.t()*invMatProjZ;
+        }
+    }
+}
+
+static const char* cvDistCoeffErr = "Distortion coefficients must be 1x4, 4x1, 1x5, 5x1, 1x8, 8x1, 1x12, 12x1, 1x14 or 14x1 floating-point vector";
+
+static void _cvProjectPoints2Internal( const CvMat* objectPoints,
+                                      const CvMat* r_vec,
+                                      const CvMat* t_vec,
+                                      const CvMat* A,
+                                      const CvMat* distCoeffs,
+                                      CvMat* imagePoints, CvMat* dpdr CV_DEFAULT(NULL),
+                                      CvMat* dpdt CV_DEFAULT(NULL), CvMat* dpdf CV_DEFAULT(NULL),
+                                      CvMat* dpdc CV_DEFAULT(NULL), CvMat* dpdk CV_DEFAULT(NULL),
+                                      CvMat* dpdo CV_DEFAULT(NULL),
+                                      double aspectRatio CV_DEFAULT(0) )
+{
+    Ptr<CvMat> matM, _m;
+    Ptr<CvMat> _dpdr, _dpdt, _dpdc, _dpdf, _dpdk;
+    Ptr<CvMat> _dpdo;
+
+    int i, j, count;
+    int calc_derivatives;
+    const CvPoint3D64f* M;
+    CvPoint3D64f* m;
+    double r[3], R[9], dRdr[27], t[3], a[9], k[14] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, fx, fy, cx, cy;
+    Matx33d matTilt = Matx33d::eye();
+    Matx33d dMatTiltdTauX(0,0,0,0,0,0,0,-1,0);
+    Matx33d dMatTiltdTauY(0,0,0,0,0,0,1,0,0);
+    CvMat _r, _t, _a = cvMat( 3, 3, CV_64F, a ), _k;
+    CvMat matR = cvMat( 3, 3, CV_64F, R ), _dRdr = cvMat( 3, 9, CV_64F, dRdr );
+    double *dpdr_p = 0, *dpdt_p = 0, *dpdk_p = 0, *dpdf_p = 0, *dpdc_p = 0;
+    double* dpdo_p = 0;
+    int dpdr_step = 0, dpdt_step = 0, dpdk_step = 0, dpdf_step = 0, dpdc_step = 0;
+    int dpdo_step = 0;
+    bool fixedAspectRatio = aspectRatio > FLT_EPSILON;
+
+    if( !CV_IS_MAT(objectPoints) || !CV_IS_MAT(r_vec) ||
+        !CV_IS_MAT(t_vec) || !CV_IS_MAT(A) ||
+        /*!CV_IS_MAT(distCoeffs) ||*/ !CV_IS_MAT(imagePoints) )
+        CV_Error( CV_StsBadArg, "One of required arguments is not a valid matrix" );
+
+    int total = objectPoints->rows * objectPoints->cols * CV_MAT_CN(objectPoints->type);
+    if(total % 3 != 0)
+    {
+        //we have stopped support of homogeneous coordinates because it cause ambiguity in interpretation of the input data
+        CV_Error( CV_StsBadArg, "Homogeneous coordinates are not supported" );
+    }
+    count = total / 3;
+
+    if( CV_IS_CONT_MAT(objectPoints->type) &&
+        (CV_MAT_DEPTH(objectPoints->type) == CV_32F || CV_MAT_DEPTH(objectPoints->type) == CV_64F)&&
+        ((objectPoints->rows == 1 && CV_MAT_CN(objectPoints->type) == 3) ||
+         (objectPoints->rows == count && CV_MAT_CN(objectPoints->type)*objectPoints->cols == 3) ||
+         (objectPoints->rows == 3 && CV_MAT_CN(objectPoints->type) == 1 && objectPoints->cols == count)))
+    {
+        matM.reset(cvCreateMat( objectPoints->rows, objectPoints->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(objectPoints->type)) ));
+        cvConvert(objectPoints, matM);
+    }
+    else
+    {
+//        matM = cvCreateMat( 1, count, CV_64FC3 );
+//        cvConvertPointsHomogeneous( objectPoints, matM );
+        CV_Error( CV_StsBadArg, "Homogeneous coordinates are not supported" );
+    }
+
+    if( CV_IS_CONT_MAT(imagePoints->type) &&
+        (CV_MAT_DEPTH(imagePoints->type) == CV_32F || CV_MAT_DEPTH(imagePoints->type) == CV_64F) &&
+        ((imagePoints->rows == 1 && CV_MAT_CN(imagePoints->type) == 3) ||
+         (imagePoints->rows == count && CV_MAT_CN(imagePoints->type)*imagePoints->cols == 3) ||
+         (imagePoints->rows == 3 && CV_MAT_CN(imagePoints->type) == 1 && imagePoints->cols == count)))
+    {
+        _m.reset(cvCreateMat( imagePoints->rows, imagePoints->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(imagePoints->type)) ));
+        cvConvert(imagePoints, _m);
+    }
+    else
+    {
+//        _m = cvCreateMat( 1, count, CV_64FC2 );
+        CV_Error( CV_StsBadArg, "Homogeneous coordinates are not supported" );
+    }
+
+    M = (CvPoint3D64f*)matM->data.db;
+    m = (CvPoint3D64f*)_m->data.db;
+
+    if( (CV_MAT_DEPTH(r_vec->type) != CV_64F && CV_MAT_DEPTH(r_vec->type) != CV_32F) ||
+        (((r_vec->rows != 1 && r_vec->cols != 1) ||
+          r_vec->rows*r_vec->cols*CV_MAT_CN(r_vec->type) != 3) &&
+         ((r_vec->rows != 3 && r_vec->cols != 3) || CV_MAT_CN(r_vec->type) != 1)))
+        CV_Error( CV_StsBadArg, "Rotation must be represented by 1x3 or 3x1 "
+                                "floating-point rotation vector, or 3x3 rotation matrix" );
+
+    if( r_vec->rows == 3 && r_vec->cols == 3 )
+    {
+        _r = cvMat( 3, 1, CV_64FC1, r );
+        cvRodrigues2( r_vec, &_r );
+        cvRodrigues2( &_r, &matR, &_dRdr );
+        cvCopy( r_vec, &matR );
+    }
+    else
+    {
+        _r = cvMat( r_vec->rows, r_vec->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(r_vec->type)), r );
+        cvConvert( r_vec, &_r );
+        cvRodrigues2( &_r, &matR, &_dRdr );
+    }
+
+    if( (CV_MAT_DEPTH(t_vec->type) != CV_64F && CV_MAT_DEPTH(t_vec->type) != CV_32F) ||
+        (t_vec->rows != 1 && t_vec->cols != 1) ||
+        t_vec->rows*t_vec->cols*CV_MAT_CN(t_vec->type) != 3 )
+        CV_Error( CV_StsBadArg,
+                  "Translation vector must be 1x3 or 3x1 floating-point vector" );
+
+    _t = cvMat( t_vec->rows, t_vec->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(t_vec->type)), t );
+    cvConvert( t_vec, &_t );
+
+    if( (CV_MAT_TYPE(A->type) != CV_64FC1 && CV_MAT_TYPE(A->type) != CV_32FC1) ||
+        A->rows != 3 || A->cols != 3 )
+        CV_Error( CV_StsBadArg, "Instrinsic parameters must be 3x3 floating-point matrix" );
+
+    cvConvert( A, &_a );
+    fx = a[0]; fy = a[4];
+    cx = a[2]; cy = a[5];
+
+    if( fixedAspectRatio )
+        fx = fy*aspectRatio;
+
+    if( distCoeffs )
+    {
+        if( !CV_IS_MAT(distCoeffs) ||
+            (CV_MAT_DEPTH(distCoeffs->type) != CV_64F &&
+             CV_MAT_DEPTH(distCoeffs->type) != CV_32F) ||
+            (distCoeffs->rows != 1 && distCoeffs->cols != 1) ||
+            (distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 4 &&
+             distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 5 &&
+             distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 8 &&
+             distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 12 &&
+             distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 14) )
+            CV_Error( CV_StsBadArg, cvDistCoeffErr );
+
+        _k = cvMat( distCoeffs->rows, distCoeffs->cols,
+                    CV_MAKETYPE(CV_64F,CV_MAT_CN(distCoeffs->type)), k );
+        cvConvert( distCoeffs, &_k );
+        if(k[12] != 0 || k[13] != 0)
+        {
+            _detail::computeTiltProjectionMatrix(k[12], k[13],
+                                                &matTilt, &dMatTiltdTauX, &dMatTiltdTauY);
+        }
+    }
+
+    if( dpdr )
+    {
+        if( !CV_IS_MAT(dpdr) ||
+            (CV_MAT_TYPE(dpdr->type) != CV_32FC1 &&
+             CV_MAT_TYPE(dpdr->type) != CV_64FC1) ||
+            dpdr->rows != count*2 || dpdr->cols != 3 )
+            CV_Error( CV_StsBadArg, "dp/drot must be 2Nx3 floating-point matrix" );
+
+        if( CV_MAT_TYPE(dpdr->type) == CV_64FC1 )
+        {
+            _dpdr.reset(cvCloneMat(dpdr));
+        }
+        else
+            _dpdr.reset(cvCreateMat( 2*count, 3, CV_64FC1 ));
+        dpdr_p = _dpdr->data.db;
+        dpdr_step = _dpdr->step/sizeof(dpdr_p[0]);
+    }
+
+    if( dpdt )
+    {
+        if( !CV_IS_MAT(dpdt) ||
+            (CV_MAT_TYPE(dpdt->type) != CV_32FC1 &&
+             CV_MAT_TYPE(dpdt->type) != CV_64FC1) ||
+            dpdt->rows != count*2 || dpdt->cols != 3 )
+            CV_Error( CV_StsBadArg, "dp/dT must be 2Nx3 floating-point matrix" );
+
+        if( CV_MAT_TYPE(dpdt->type) == CV_64FC1 )
+        {
+            _dpdt.reset(cvCloneMat(dpdt));
+        }
+        else
+            _dpdt.reset(cvCreateMat( 2*count, 3, CV_64FC1 ));
+        dpdt_p = _dpdt->data.db;
+        dpdt_step = _dpdt->step/sizeof(dpdt_p[0]);
+    }
+
+    if( dpdf )
+    {
+        if( !CV_IS_MAT(dpdf) ||
+            (CV_MAT_TYPE(dpdf->type) != CV_32FC1 && CV_MAT_TYPE(dpdf->type) != CV_64FC1) ||
+            dpdf->rows != count*2 || dpdf->cols != 2 )
+            CV_Error( CV_StsBadArg, "dp/df must be 2Nx2 floating-point matrix" );
+
+        if( CV_MAT_TYPE(dpdf->type) == CV_64FC1 )
+        {
+            _dpdf.reset(cvCloneMat(dpdf));
+        }
+        else
+            _dpdf.reset(cvCreateMat( 2*count, 2, CV_64FC1 ));
+        dpdf_p = _dpdf->data.db;
+        dpdf_step = _dpdf->step/sizeof(dpdf_p[0]);
+    }
+
+    if( dpdc )
+    {
+        if( !CV_IS_MAT(dpdc) ||
+            (CV_MAT_TYPE(dpdc->type) != CV_32FC1 && CV_MAT_TYPE(dpdc->type) != CV_64FC1) ||
+            dpdc->rows != count*2 || dpdc->cols != 2 )
+            CV_Error( CV_StsBadArg, "dp/dc must be 2Nx2 floating-point matrix" );
+
+        if( CV_MAT_TYPE(dpdc->type) == CV_64FC1 )
+        {
+            _dpdc.reset(cvCloneMat(dpdc));
+        }
+        else
+            _dpdc.reset(cvCreateMat( 2*count, 2, CV_64FC1 ));
+        dpdc_p = _dpdc->data.db;
+        dpdc_step = _dpdc->step/sizeof(dpdc_p[0]);
+    }
+
+    if( dpdk )
+    {
+        if( !CV_IS_MAT(dpdk) ||
+            (CV_MAT_TYPE(dpdk->type) != CV_32FC1 && CV_MAT_TYPE(dpdk->type) != CV_64FC1) ||
+            dpdk->rows != count*2 || (dpdk->cols != 14 && dpdk->cols != 12 && dpdk->cols != 8 && dpdk->cols != 5 && dpdk->cols != 4 && dpdk->cols != 2) )
+            CV_Error( CV_StsBadArg, "dp/df must be 2Nx14, 2Nx12, 2Nx8, 2Nx5, 2Nx4 or 2Nx2 floating-point matrix" );
+
+        if( !distCoeffs )
+            CV_Error( CV_StsNullPtr, "distCoeffs is NULL while dpdk is not" );
+
+        if( CV_MAT_TYPE(dpdk->type) == CV_64FC1 )
+        {
+            _dpdk.reset(cvCloneMat(dpdk));
+        }
+        else
+            _dpdk.reset(cvCreateMat( dpdk->rows, dpdk->cols, CV_64FC1 ));
+        dpdk_p = _dpdk->data.db;
+        dpdk_step = _dpdk->step/sizeof(dpdk_p[0]);
+    }
+
+    if( dpdo )
+    {
+        if( !CV_IS_MAT( dpdo ) || ( CV_MAT_TYPE( dpdo->type ) != CV_32FC1
+                                    && CV_MAT_TYPE( dpdo->type ) != CV_64FC1 )
+            || dpdo->rows != count * 2 || dpdo->cols != count * 3 )
+            CV_Error( CV_StsBadArg, "dp/do must be 2Nx3N floating-point matrix" );
+
+        if( CV_MAT_TYPE( dpdo->type ) == CV_64FC1 )
+        {
+            _dpdo.reset( cvCloneMat( dpdo ) );
+        }
+        else
+            _dpdo.reset( cvCreateMat( 2 * count, 3 * count, CV_64FC1 ) );
+        cvZero(_dpdo);
+        dpdo_p = _dpdo->data.db;
+        dpdo_step = _dpdo->step / sizeof( dpdo_p[0] );
+    }
+
+    calc_derivatives = dpdr || dpdt || dpdf || dpdc || dpdk || dpdo;
+
+    for( i = 0; i < count; i++ )
+    {
+        double X = M[i].x, Y = M[i].y, Z = M[i].z;
+        double x = R[0]*X + R[1]*Y + R[2]*Z + t[0];
+        double y = R[3]*X + R[4]*Y + R[5]*Z + t[1];
+        double z = R[6]*X + R[7]*Y + R[8]*Z + t[2];
+        double r2, r4, r6, a1, a2, a3, cdist, icdist2;
+        double xd, yd, xd0, yd0, invProj;
+        Vec3d vecTilt;
+        Vec3d dVecTilt;
+        Matx22d dMatTilt;
+        Vec2d dXdYd;
+
+        double z0 = z;
+        z = z ? 1./z : 1;
+        x *= z; y *= z;
+
+        r2 = x*x + y*y;
+        r4 = r2*r2;
+        r6 = r4*r2;
+        a1 = 2*x*y;
+        a2 = r2 + 2*x*x;
+        a3 = r2 + 2*y*y;
+        cdist = 1 + k[0]*r2 + k[1]*r4 + k[4]*r6;
+        icdist2 = 1./(1 + k[5]*r2 + k[6]*r4 + k[7]*r6);
+        xd0 = x*cdist*icdist2 + k[2]*a1 + k[3]*a2 + k[8]*r2+k[9]*r4;
+        yd0 = y*cdist*icdist2 + k[2]*a3 + k[3]*a1 + k[10]*r2+k[11]*r4;
+
+        // additional distortion by projecting onto a tilt plane
+        vecTilt = matTilt*Vec3d(xd0, yd0, 1);
+        invProj = vecTilt(2) ? 1./vecTilt(2) : 1;
+        xd = invProj * vecTilt(0);
+        yd = invProj * vecTilt(1);
+
+        m[i].x = xd*fx + cx;
+        m[i].y = yd*fy + cy;
+        m[i].z = z; // Just put the projected Z coordinate here, we mainly care about the sign
+
+        if( calc_derivatives )
+        {
+            if( dpdc_p )
+            {
+                dpdc_p[0] = 1; dpdc_p[1] = 0; // dp_xdc_x; dp_xdc_y
+                dpdc_p[dpdc_step] = 0;
+                dpdc_p[dpdc_step+1] = 1;
+                dpdc_p += dpdc_step*2;
+            }
+
+            if( dpdf_p )
+            {
+                if( fixedAspectRatio )
+                {
+                    dpdf_p[0] = 0; dpdf_p[1] = xd*aspectRatio; // dp_xdf_x; dp_xdf_y
+                    dpdf_p[dpdf_step] = 0;
+                    dpdf_p[dpdf_step+1] = yd;
+                }
+                else
+                {
+                    dpdf_p[0] = xd; dpdf_p[1] = 0;
+                    dpdf_p[dpdf_step] = 0;
+                    dpdf_p[dpdf_step+1] = yd;
+                }
+                dpdf_p += dpdf_step*2;
+            }
+            for (int row = 0; row < 2; ++row)
+                for (int col = 0; col < 2; ++col)
+                    dMatTilt(row,col) = matTilt(row,col)*vecTilt(2)
+                                        - matTilt(2,col)*vecTilt(row);
+            double invProjSquare = (invProj*invProj);
+            dMatTilt *= invProjSquare;
+            if( dpdk_p )
+            {
+                dXdYd = dMatTilt*Vec2d(x*icdist2*r2, y*icdist2*r2);
+                dpdk_p[0] = fx*dXdYd(0);
+                dpdk_p[dpdk_step] = fy*dXdYd(1);
+                dXdYd = dMatTilt*Vec2d(x*icdist2*r4, y*icdist2*r4);
+                dpdk_p[1] = fx*dXdYd(0);
+                dpdk_p[dpdk_step+1] = fy*dXdYd(1);
+                if( _dpdk->cols > 2 )
+                {
+                    dXdYd = dMatTilt*Vec2d(a1, a3);
+                    dpdk_p[2] = fx*dXdYd(0);
+                    dpdk_p[dpdk_step+2] = fy*dXdYd(1);
+                    dXdYd = dMatTilt*Vec2d(a2, a1);
+                    dpdk_p[3] = fx*dXdYd(0);
+                    dpdk_p[dpdk_step+3] = fy*dXdYd(1);
+                    if( _dpdk->cols > 4 )
+                    {
+                        dXdYd = dMatTilt*Vec2d(x*icdist2*r6, y*icdist2*r6);
+                        dpdk_p[4] = fx*dXdYd(0);
+                        dpdk_p[dpdk_step+4] = fy*dXdYd(1);
+
+                        if( _dpdk->cols > 5 )
+                        {
+                            dXdYd = dMatTilt*Vec2d(
+                                    x*cdist*(-icdist2)*icdist2*r2, y*cdist*(-icdist2)*icdist2*r2);
+                            dpdk_p[5] = fx*dXdYd(0);
+                            dpdk_p[dpdk_step+5] = fy*dXdYd(1);
+                            dXdYd = dMatTilt*Vec2d(
+                                    x*cdist*(-icdist2)*icdist2*r4, y*cdist*(-icdist2)*icdist2*r4);
+                            dpdk_p[6] = fx*dXdYd(0);
+                            dpdk_p[dpdk_step+6] = fy*dXdYd(1);
+                            dXdYd = dMatTilt*Vec2d(
+                                    x*cdist*(-icdist2)*icdist2*r6, y*cdist*(-icdist2)*icdist2*r6);
+                            dpdk_p[7] = fx*dXdYd(0);
+                            dpdk_p[dpdk_step+7] = fy*dXdYd(1);
+                            if( _dpdk->cols > 8 )
+                            {
+                                dXdYd = dMatTilt*Vec2d(r2, 0);
+                                dpdk_p[8] = fx*dXdYd(0); //s1
+                                dpdk_p[dpdk_step+8] = fy*dXdYd(1); //s1
+                                dXdYd = dMatTilt*Vec2d(r4, 0);
+                                dpdk_p[9] = fx*dXdYd(0); //s2
+                                dpdk_p[dpdk_step+9] = fy*dXdYd(1); //s2
+                                dXdYd = dMatTilt*Vec2d(0, r2);
+                                dpdk_p[10] = fx*dXdYd(0);//s3
+                                dpdk_p[dpdk_step+10] = fy*dXdYd(1); //s3
+                                dXdYd = dMatTilt*Vec2d(0, r4);
+                                dpdk_p[11] = fx*dXdYd(0);//s4
+                                dpdk_p[dpdk_step+11] = fy*dXdYd(1); //s4
+                                if( _dpdk->cols > 12 )
+                                {
+                                    dVecTilt = dMatTiltdTauX * Vec3d(xd0, yd0, 1);
+                                    dpdk_p[12] = fx * invProjSquare * (
+                                            dVecTilt(0) * vecTilt(2) - dVecTilt(2) * vecTilt(0));
+                                    dpdk_p[dpdk_step+12] = fy*invProjSquare * (
+                                            dVecTilt(1) * vecTilt(2) - dVecTilt(2) * vecTilt(1));
+                                    dVecTilt = dMatTiltdTauY * Vec3d(xd0, yd0, 1);
+                                    dpdk_p[13] = fx * invProjSquare * (
+                                            dVecTilt(0) * vecTilt(2) - dVecTilt(2) * vecTilt(0));
+                                    dpdk_p[dpdk_step+13] = fy * invProjSquare * (
+                                            dVecTilt(1) * vecTilt(2) - dVecTilt(2) * vecTilt(1));
+                                }
+                            }
+                        }
+                    }
+                }
+                dpdk_p += dpdk_step*2;
+            }
+
+            if( dpdt_p )
+            {
+                double dxdt[] = { z, 0, -x*z }, dydt[] = { 0, z, -y*z };
+                for( j = 0; j < 3; j++ )
+                {
+                    double dr2dt = 2*x*dxdt[j] + 2*y*dydt[j];
+                    double dcdist_dt = k[0]*dr2dt + 2*k[1]*r2*dr2dt + 3*k[4]*r4*dr2dt;
+                    double dicdist2_dt = -icdist2*icdist2*(k[5]*dr2dt + 2*k[6]*r2*dr2dt + 3*k[7]*r4*dr2dt);
+                    double da1dt = 2*(x*dydt[j] + y*dxdt[j]);
+                    double dmxdt = (dxdt[j]*cdist*icdist2 + x*dcdist_dt*icdist2 + x*cdist*dicdist2_dt +
+                                    k[2]*da1dt + k[3]*(dr2dt + 4*x*dxdt[j]) + k[8]*dr2dt + 2*r2*k[9]*dr2dt);
+                    double dmydt = (dydt[j]*cdist*icdist2 + y*dcdist_dt*icdist2 + y*cdist*dicdist2_dt +
+                                    k[2]*(dr2dt + 4*y*dydt[j]) + k[3]*da1dt + k[10]*dr2dt + 2*r2*k[11]*dr2dt);
+                    dXdYd = dMatTilt*Vec2d(dmxdt, dmydt);
+                    dpdt_p[j] = fx*dXdYd(0);
+                    dpdt_p[dpdt_step+j] = fy*dXdYd(1);
+                }
+                dpdt_p += dpdt_step*2;
+            }
+
+            if( dpdr_p )
+            {
+                double dx0dr[] =
+                        {
+                                X*dRdr[0] + Y*dRdr[1] + Z*dRdr[2],
+                                X*dRdr[9] + Y*dRdr[10] + Z*dRdr[11],
+                                X*dRdr[18] + Y*dRdr[19] + Z*dRdr[20]
+                        };
+                double dy0dr[] =
+                        {
+                                X*dRdr[3] + Y*dRdr[4] + Z*dRdr[5],
+                                X*dRdr[12] + Y*dRdr[13] + Z*dRdr[14],
+                                X*dRdr[21] + Y*dRdr[22] + Z*dRdr[23]
+                        };
+                double dz0dr[] =
+                        {
+                                X*dRdr[6] + Y*dRdr[7] + Z*dRdr[8],
+                                X*dRdr[15] + Y*dRdr[16] + Z*dRdr[17],
+                                X*dRdr[24] + Y*dRdr[25] + Z*dRdr[26]
+                        };
+                for( j = 0; j < 3; j++ )
+                {
+                    double dxdr = z*(dx0dr[j] - x*dz0dr[j]);
+                    double dydr = z*(dy0dr[j] - y*dz0dr[j]);
+                    double dr2dr = 2*x*dxdr + 2*y*dydr;
+                    double dcdist_dr = (k[0] + 2*k[1]*r2 + 3*k[4]*r4)*dr2dr;
+                    double dicdist2_dr = -icdist2*icdist2*(k[5] + 2*k[6]*r2 + 3*k[7]*r4)*dr2dr;
+                    double da1dr = 2*(x*dydr + y*dxdr);
+                    double dmxdr = (dxdr*cdist*icdist2 + x*dcdist_dr*icdist2 + x*cdist*dicdist2_dr +
+                                    k[2]*da1dr + k[3]*(dr2dr + 4*x*dxdr) + (k[8] + 2*r2*k[9])*dr2dr);
+                    double dmydr = (dydr*cdist*icdist2 + y*dcdist_dr*icdist2 + y*cdist*dicdist2_dr +
+                                    k[2]*(dr2dr + 4*y*dydr) + k[3]*da1dr + (k[10] + 2*r2*k[11])*dr2dr);
+                    dXdYd = dMatTilt*Vec2d(dmxdr, dmydr);
+                    dpdr_p[j] = fx*dXdYd(0);
+                    dpdr_p[dpdr_step+j] = fy*dXdYd(1);
+                }
+                dpdr_p += dpdr_step*2;
+            }
+
+            if( dpdo_p )
+            {
+                double dxdo[] = { z * ( R[0] - x * z * z0 * R[6] ),
+                                  z * ( R[1] - x * z * z0 * R[7] ),
+                                  z * ( R[2] - x * z * z0 * R[8] ) };
+                double dydo[] = { z * ( R[3] - y * z * z0 * R[6] ),
+                                  z * ( R[4] - y * z * z0 * R[7] ),
+                                  z * ( R[5] - y * z * z0 * R[8] ) };
+                for( j = 0; j < 3; j++ )
+                {
+                    double dr2do = 2 * x * dxdo[j] + 2 * y * dydo[j];
+                    double dr4do = 2 * r2 * dr2do;
+                    double dr6do = 3 * r4 * dr2do;
+                    double da1do = 2 * y * dxdo[j] + 2 * x * dydo[j];
+                    double da2do = dr2do + 4 * x * dxdo[j];
+                    double da3do = dr2do + 4 * y * dydo[j];
+                    double dcdist_do
+                            = k[0] * dr2do + k[1] * dr4do + k[4] * dr6do;
+                    double dicdist2_do = -icdist2 * icdist2
+                                         * ( k[5] * dr2do + k[6] * dr4do + k[7] * dr6do );
+                    double dxd0_do = cdist * icdist2 * dxdo[j]
+                                     + x * icdist2 * dcdist_do + x * cdist * dicdist2_do
+                                     + k[2] * da1do + k[3] * da2do + k[8] * dr2do
+                                     + k[9] * dr4do;
+                    double dyd0_do = cdist * icdist2 * dydo[j]
+                                     + y * icdist2 * dcdist_do + y * cdist * dicdist2_do
+                                     + k[2] * da3do + k[3] * da1do + k[10] * dr2do
+                                     + k[11] * dr4do;
+                    dXdYd = dMatTilt * Vec2d( dxd0_do, dyd0_do );
+                    dpdo_p[i * 3 + j] = fx * dXdYd( 0 );
+                    dpdo_p[dpdo_step + i * 3 + j] = fy * dXdYd( 1 );
+                }
+                dpdo_p += dpdo_step * 2;
+            }
+        }
+    }
+
+    if( _m != imagePoints )
+        cvConvert( _m, imagePoints );
+
+    if( _dpdr != dpdr )
+        cvConvert( _dpdr, dpdr );
+
+    if( _dpdt != dpdt )
+        cvConvert( _dpdt, dpdt );
+
+    if( _dpdf != dpdf )
+        cvConvert( _dpdf, dpdf );
+
+    if( _dpdc != dpdc )
+        cvConvert( _dpdc, dpdc );
+
+    if( _dpdk != dpdk )
+        cvConvert( _dpdk, dpdk );
+
+    if( _dpdo != dpdo )
+        cvConvert( _dpdo, dpdo );
+}
+
+static void _cvProjectPoints2( const CvMat* objectPoints,
+                        const CvMat* r_vec,
+                        const CvMat* t_vec,
+                        const CvMat* A,
+                        const CvMat* distCoeffs,
+                        CvMat* imagePoints, CvMat* dpdr,
+                        CvMat* dpdt, CvMat* dpdf,
+                        CvMat* dpdc, CvMat* dpdk,
+                        double aspectRatio )
+{
+    _cvProjectPoints2Internal( objectPoints, r_vec, t_vec, A, distCoeffs, imagePoints, dpdr, dpdt,
+                               dpdf, dpdc, dpdk, NULL, aspectRatio );
+}

aruco_pose/src/draw.h

@@ -0,0 +1,9 @@
+#include <cmath>
+#include <ros/ros.h>
+#include <opencv2/opencv.hpp>
+#include <opencv2/aruco.hpp>
+
+void _drawPlanarBoard(cv::aruco::Board *_board, cv::Size outSize, cv::OutputArray _img,
+                      int marginSize, int borderBits, bool drawAxis); // editorconfig-checker-disable-line
+void _drawAxis(cv::InputOutputArray image, cv::InputArray cameraMatrix, cv::InputArray distCoeffs,
+               cv::InputArray rvec, cv::InputArray tvec, float length); // editorconfig-checker-disable-line

aruco_pose/src/fix.cpp

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

aruco_pose/src/genmap.py

@@ -0,0 +1,53 @@
+#!/usr/bin/env python
+
+# Copyright (C) 2018 Copter Express Technologies
+#
+# Author: Oleg Kalachev <okalachev@gmail.com>
+#
+# Distributed under MIT License (available at https://opensource.org/licenses/MIT).
+# The above copyright notice and this permission notice shall be included in all
+# copies or substantial portions of the Software.
+
+"""Markers map generator
+
+Generate map file for aruco_map nodelet.
+
+Usage:
+  genmap.py <length> <x> <y> <dist_x> <dist_y> <first> [--top-left]
+  genmap.py (-h | --help)
+
+Options:
+  <length>    Marker side length
+  <x>         Marker count along X axis
+  <y>         Marker count along Y axis
+  <dist_x>    Distance between markers along X axis
+  <dist_y>    Distance between markers along Y axis
+  <first>     First marker ID
+  --top-left  First marker is on top-left (not bottom-left)
+"""
+
+from __future__ import print_function
+
+from docopt import docopt
+
+
+arguments = docopt(__doc__)
+
+length = float(arguments['<length>'])
+first = int(arguments['<first>'])
+markers_x = int(arguments['<x>'])
+markers_y = int(arguments['<y>'])
+dist_x = float(arguments['<dist_x>'])
+dist_y = float(arguments['<dist_y>'])
+top_left = arguments['--top-left']
+
+max_y = (markers_y - 1) * dist_y
+
+for y in range(markers_y):
+    for x in range(markers_x):
+        pos_x = x * dist_x
+        pos_y = y * dist_y
+        if top_left:
+            pos_y = max_y - pos_y
+        print('{}\t{}\t{}\t{}\t{}\t{}\t{}\t{}\t'.format(first, length, pos_x, pos_y, 0, 0, 0, 0))
+        first += 1

aruco_pose/src/util.h

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

aruco_pose/src/utils.h

@@ -0,0 +1,139 @@
+/*
+ * Utility functions
+ * Copyright (C) 2018 Copter Express Technologies
+ *
+ * Author: Oleg Kalachev <okalachev@gmail.com>
+ *
+ * Distributed under MIT License (available at https://opensource.org/licenses/MIT).
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ */
+
+#pragma once
+
+#include <cmath>
+#include <ros/ros.h>
+#include <tf/transform_datatypes.h>
+#include <geometry_msgs/Quaternion.h>
+#include <geometry_msgs/Pose.h>
+#include <geometry_msgs/Vector3.h>
+#include <sensor_msgs/CameraInfo.h>
+#include <opencv2/opencv.hpp>
+
+// Read required param or shutdown the node
+template<typename T>
+static void param(ros::NodeHandle nh, const std::string& param_name, T& param_val)
+{
+	if (!nh.getParam(param_name, param_val)) {
+		ROS_FATAL("Required param %s is not defined", param_name.c_str());
+		ros::shutdown();
+	}
+}
+
+static void parseCameraInfo(const sensor_msgs::CameraInfoConstPtr& cinfo, cv::Mat& matrix, cv::Mat& dist)
+{
+	for (unsigned int i = 0; i < 3; ++i)
+		for (unsigned int j = 0; j < 3; ++j)
+			matrix.at<double>(i, j) = cinfo->K[3 * i + j];
+
+	for (unsigned int k = 0; k < cinfo->D.size(); k++)
+		dist.at<double>(k) = cinfo->D[k];
+}
+
+inline void rotatePoint(cv::Point3f& p, cv::Point3f origin, float angle)
+{
+	float s = sin(angle);
+	float c = cos(angle);
+
+	// translate point back to origin:
+	p.x -= origin.x;
+	p.y -= origin.y;
+
+	// rotate point
+	float xnew = p.x * c - p.y * s;
+	float ynew = p.x * s + p.y * c;
+
+	// translate point back:
+	p.x = xnew + origin.x;
+	p.y = ynew + origin.y;
+}
+
+inline void fillPose(geometry_msgs::Pose& pose, const cv::Vec3d& rvec, const cv::Vec3d& tvec)
+{
+	pose.position.x = tvec[0];
+	pose.position.y = tvec[1];
+	pose.position.z = tvec[2];
+
+	double angle = norm(rvec);
+	cv::Vec3d axis = rvec / angle;
+
+	tf2::Quaternion q;
+	q.setRotation(tf2::Vector3(axis[0], axis[1], axis[2]), angle);
+
+	pose.orientation.w = q.w();
+	pose.orientation.x = q.x();
+	pose.orientation.y = q.y();
+	pose.orientation.z = q.z();
+}
+
+inline void fillTransform(geometry_msgs::Transform& transform, const cv::Vec3d& rvec, const cv::Vec3d& tvec)
+{
+	transform.translation.x = tvec[0];
+	transform.translation.y = tvec[1];
+	transform.translation.z = tvec[2];
+
+	double angle = norm(rvec);
+	cv::Vec3d axis = rvec / angle;
+
+	tf2::Quaternion q;
+	q.setRotation(tf2::Vector3(axis[0], axis[1], axis[2]), angle);
+
+	transform.rotation.w = q.w();
+	transform.rotation.x = q.x();
+	transform.rotation.y = q.y();
+	transform.rotation.z = q.z();
+}
+
+inline void fillTranslation(geometry_msgs::Vector3& translation, const cv::Vec3d& tvec)
+{
+	translation.x = tvec[0];
+	translation.y = tvec[1];
+	translation.z = tvec[2];
+}
+
+inline bool isFlipped(tf::Quaternion& q)
+{
+	double yaw, pitch, roll;
+	tf::Matrix3x3(q).getEulerYPR(yaw, pitch, roll);
+	return (abs(pitch) > M_PI / 2) || (abs(roll) > M_PI / 2);
+}
+
+/* Set roll and pitch from "from" to "to", keeping yaw */
+inline void snapOrientation(geometry_msgs::Quaternion& to, const geometry_msgs::Quaternion& from, bool auto_flip = false)
+{
+	tf::Quaternion _from, _to;
+	tf::quaternionMsgToTF(from, _from);
+	tf::quaternionMsgToTF(to, _to);
+
+	if (auto_flip) {
+		if (!isFlipped(_from)) {
+			static const tf::Quaternion flip = tf::createQuaternionFromRPY(M_PI, 0, 0);
+			_from *= flip; // flip "from"
+		}
+	}
+
+	auto diff = tf::Matrix3x3(_to).transposeTimes(tf::Matrix3x3(_from));
+	double _, yaw;
+	diff.getRPY(_, _, yaw);
+	auto q = tf::createQuaternionFromRPY(0, 0, -yaw);
+	_from = _from * q; // set yaw from "to" to "from"
+	tf::quaternionTFToMsg(_from, to); // set "from" to "to"
+}
+
+inline void transformToPose(const geometry_msgs::Transform& transform, geometry_msgs::Pose& pose)
+{
+	pose.position.x = transform.translation.x;
+	pose.position.y = transform.translation.y;
+	pose.position.z = transform.translation.z;
+	pose.orientation = transform.rotation;
+}

aruco_pose/test/basic.py

@@ -0,0 +1,204 @@
+import rospy
+import pytest
+
+import tf2_ros
+import tf2_geometry_msgs
+from geometry_msgs.msg import PoseWithCovarianceStamped
+from sensor_msgs.msg import Image
+from aruco_pose.msg import MarkerArray
+from visualization_msgs.msg import MarkerArray as VisMarkerArray
+
+
+@pytest.fixture
+def node():
+    return rospy.init_node('aruco_pose_test', anonymous=True)
+
+@pytest.fixture
+def tf_buffer():
+    buf = tf2_ros.Buffer()
+    tf2_ros.TransformListener(buf)
+    return buf
+
+def approx(expected):
+    return pytest.approx(expected, abs=1e-4) # compare floats more roughly
+
+def test_markers(node):
+    markers = rospy.wait_for_message('aruco_detect/markers', MarkerArray, timeout=5)
+    assert len(markers.markers) == 5
+    assert markers.header.frame_id == 'main_camera_optical'
+
+    assert markers.markers[0].id == 2
+    assert markers.markers[0].length == approx(0.33)
+    assert markers.markers[0].pose.position.x == approx(0.36706567854)
+    assert markers.markers[0].pose.position.y == approx(0.290484516644)
+    assert markers.markers[0].pose.position.z == approx(2.18787602301)
+    assert markers.markers[0].pose.orientation.x == approx(0.993997406299)
+    assert markers.markers[0].pose.orientation.y == approx(-0.00532003481626)
+    assert markers.markers[0].pose.orientation.z == approx(-0.107390951553)
+    assert markers.markers[0].pose.orientation.w == approx(0.0201999263402)
+    assert markers.markers[0].c1.x == approx(415.557739258)
+    assert markers.markers[0].c1.y == approx(335.557739258)
+    assert markers.markers[0].c2.x == approx(509.442260742)
+    assert markers.markers[0].c2.y == approx(335.557739258)
+    assert markers.markers[0].c3.x == approx(509.442260742)
+    assert markers.markers[0].c3.y == approx(429.442260742)
+    assert markers.markers[0].c4.x == approx(415.557739258)
+    assert markers.markers[0].c4.y == approx(429.442260742)
+
+    assert markers.markers[4].id == 3
+    assert markers.markers[4].length == approx(0.1)
+    assert markers.markers[4].pose.position.x == approx(-0.1805169666)
+    assert markers.markers[4].pose.position.y == approx(-0.200697302327)
+    assert markers.markers[4].pose.position.z == approx(0.585767514823)
+    assert markers.markers[4].pose.orientation.x == approx(-0.961738074009)
+    assert markers.markers[4].pose.orientation.y == approx(-0.0375180244707)
+    assert markers.markers[4].pose.orientation.z == approx(-0.0115387773672)
+    assert markers.markers[4].pose.orientation.w == approx(0.271144115664)
+    assert markers.markers[4].c1.x == approx(129.557723999)
+    assert markers.markers[4].c1.y == approx(49.557723999)
+    assert markers.markers[4].c2.x == approx(223.442276001)
+    assert markers.markers[4].c2.y == approx(49.557723999)
+    assert markers.markers[4].c3.x == approx(223.442276001)
+    assert markers.markers[4].c3.y == approx(143.442276001)
+    assert markers.markers[4].c4.x == approx(129.557723999)
+    assert markers.markers[4].c4.y == approx(143.442276001)
+
+    assert markers.markers[1].id == 1
+    assert markers.markers[1].length == approx(0.33)
+    assert markers.markers[3].id == 4
+    assert markers.markers[3].length == approx(0.33)
+
+    assert markers.markers[2].id == 100
+    assert markers.markers[2].length == approx(0.33)
+    assert markers.markers[2].pose.position.x == approx(-1.37600105389)
+    assert markers.markers[2].pose.position.y == approx(-0.323028530991)
+    assert markers.markers[2].pose.position.z == approx(2.94611272668)
+    assert markers.markers[2].pose.orientation.x == approx(-0.955543925678)
+    assert markers.markers[2].pose.orientation.y == approx(0.0458801909197)
+    assert markers.markers[2].pose.orientation.z == approx(-0.249604946264)
+    assert markers.markers[2].pose.orientation.w == approx(-0.150093920537)
+    assert markers.markers[2].c1.x == approx(52.557723999)
+    assert markers.markers[2].c1.y == approx(205.557723999)
+    assert markers.markers[2].c2.x == approx(113.442276001)
+    assert markers.markers[2].c2.y == approx(205.557723999)
+    assert markers.markers[2].c3.x == approx(113.442276001)
+    assert markers.markers[2].c3.y == approx(265.442260742)
+    assert markers.markers[2].c4.x == approx(52.557723999)
+    assert markers.markers[2].c4.y == approx(265.442260742)
+
+def test_markers_frames(node, tf_buffer):
+    marker_2 = tf_buffer.lookup_transform('main_camera_optical', 'aruco_2', rospy.Time(), rospy.Duration(5))
+    assert marker_2.transform.translation.x == approx(0.36706567854)
+    assert marker_2.transform.translation.y == approx(0.290484516644)
+    assert marker_2.transform.translation.z == approx(2.18787602301)
+    assert marker_2.transform.rotation.x == approx(0.993997406299)
+    assert marker_2.transform.rotation.y == approx(-0.00532003481626)
+    assert marker_2.transform.rotation.z == approx(-0.107390951553)
+    assert marker_2.transform.rotation.w == approx(0.0201999263402)
+
+def test_map_markers_frames(node, tf_buffer):
+    stamp = rospy.get_rostime()
+    timeout = rospy.Duration(5)
+
+    marker_1 = tf_buffer.lookup_transform('aruco_map', 'aruco_in_map_1', stamp, timeout)
+    assert marker_1.transform.translation.x == approx(0)
+    assert marker_1.transform.translation.y == approx(0)
+    assert marker_1.transform.translation.z == approx(0)
+
+    marker_4 = tf_buffer.lookup_transform('aruco_map', 'aruco_in_map_4', stamp, timeout)
+    assert marker_4.transform.translation.x == approx(1)
+    assert marker_4.transform.translation.y == approx(1)
+    assert marker_4.transform.translation.z == approx(0)
+
+    marker_12 = tf_buffer.lookup_transform('aruco_map', 'aruco_in_map_12', stamp, timeout)
+    assert marker_12.transform.translation.x == approx(0.2)
+    assert marker_12.transform.translation.y == approx(0.5)
+    assert marker_12.transform.translation.z == approx(0)
+
+def test_visualization(node):
+    vis = rospy.wait_for_message('aruco_detect/visualization', VisMarkerArray, timeout=5)
+    assert len(vis.markers) == 11
+
+def test_debug(node):
+    img = rospy.wait_for_message('aruco_detect/debug', Image, timeout=5)
+    assert img.width == 640
+    assert img.height == 480
+    assert img.header.frame_id == 'main_camera_optical'
+
+def test_map(node):
+    pose = rospy.wait_for_message('aruco_map/pose', PoseWithCovarianceStamped, timeout=5)
+    assert pose.header.frame_id == 'main_camera_optical'
+    assert pose.pose.pose.position.x == approx(-0.629167753342)
+    assert pose.pose.pose.position.y == approx(0.293822650809)
+    assert pose.pose.pose.position.z == approx(2.12641343155)
+    assert pose.pose.pose.orientation.x == approx(-0.998383794799)
+    assert pose.pose.pose.orientation.y == approx(-5.20919098575e-06)
+    assert pose.pose.pose.orientation.z == approx(-0.0300861070302)
+    assert pose.pose.pose.orientation.w == approx(0.0482143590507)
+
+def test_map_image(node):
+    img = rospy.wait_for_message('aruco_map/image', Image, timeout=5)
+    assert img.width == 2000
+    assert img.height == 2000
+    assert img.encoding == 'mono8'
+
+def test_map_markers(node):
+    markers = rospy.wait_for_message('aruco_map/markers', MarkerArray, timeout=5)
+    assert markers.markers[0].id == 1
+    assert markers.markers[1].id == 2
+    assert markers.markers[2].id == 3
+    assert markers.markers[3].id == 4
+    assert markers.markers[4].id == 10
+    assert markers.markers[5].id == 11
+    assert markers.markers[6].id == 12
+
+    assert markers.markers[0].pose.position.x == 0
+    assert markers.markers[0].pose.position.y == 0
+    assert markers.markers[0].pose.position.z == 0
+    assert markers.markers[0].pose.orientation.x == 0
+    assert markers.markers[0].pose.orientation.y == 0
+    assert markers.markers[0].pose.orientation.z == 0
+    assert markers.markers[0].pose.orientation.w == 1
+    assert markers.markers[0].length == approx(0.33)
+
+    assert markers.markers[1].pose.position.x == 1
+    assert markers.markers[1].pose.position.y == 0
+    assert markers.markers[1].pose.position.z == 0
+    assert markers.markers[1].pose.orientation.x == 0
+    assert markers.markers[1].pose.orientation.y == 0
+    assert markers.markers[1].pose.orientation.z == 0
+    assert markers.markers[1].pose.orientation.w == 1
+    assert markers.markers[1].length == approx(0.33)
+
+    assert markers.markers[2].pose.position.x == 0
+    assert markers.markers[2].pose.position.y == 1
+    assert markers.markers[2].pose.position.z == 0
+    assert markers.markers[2].pose.orientation.x == 0
+    assert markers.markers[2].pose.orientation.y == 0
+    assert markers.markers[2].pose.orientation.z == 0
+    assert markers.markers[2].pose.orientation.w == 1
+    assert markers.markers[2].length == approx(0.33)
+
+    assert markers.markers[3].pose.position.x == 1
+    assert markers.markers[3].pose.position.y == 1
+    assert markers.markers[3].pose.position.z == 0
+    assert markers.markers[3].pose.orientation.x == 0
+    assert markers.markers[3].pose.orientation.y == 0
+    assert markers.markers[3].pose.orientation.z == 0
+    assert markers.markers[3].pose.orientation.w == 1
+    assert markers.markers[3].length == approx(0.33)
+
+    assert markers.markers[4].pose.position.x == approx(0.5)
+    assert markers.markers[4].pose.position.y == 2
+    assert markers.markers[4].pose.position.z == 0
+    assert markers.markers[4].pose.orientation.x == 0
+    assert markers.markers[4].pose.orientation.y == 0
+    assert markers.markers[4].pose.orientation.z == approx(0.5646424733950354)
+    assert markers.markers[4].pose.orientation.w == approx(0.8253356149096783)
+    assert markers.markers[4].length == approx(0.5)
+
+def test_map_visualization(node):
+    vis = rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5)
+
+def test_map_debug(node):
+    img = rospy.wait_for_message('aruco_map/debug', Image, timeout=5)

aruco_pose/test/basic.test

@@ -0,0 +1,31 @@
+<launch>
+    <node pkg="image_publisher" type="image_publisher" name="main_camera" args="$(find aruco_pose)/test/map.png">
+        <param name="frame_id" value="main_camera_optical"/>
+        <param name="publish_rate" value="10"/>
+        <param name="camera_info_url" value="file://$(find aruco_pose)/test/camera_info.yaml" />
+    </node>
+
+    <node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager" required="true"/>
+
+    <node pkg="nodelet" clear_params="true" type="nodelet" name="aruco_detect" args="load aruco_pose/aruco_detect nodelet_manager" required="true">
+        <remap from="image_raw" to="main_camera/image_raw"/>
+        <remap from="camera_info" to="main_camera/camera_info"/>
+        <param name="length" value="0.33"/>
+        <param name="length_override/3" value="0.1"/>
+        <param name="estimate_poses" value="true"/>
+        <param name="send_tf" value="true"/>
+    </node>
+
+    <node name="aruco_map" pkg="nodelet" type="nodelet" args="load aruco_pose/aruco_map nodelet_manager" clear_params="true" required="true">
+        <remap from="image_raw" to="main_camera/image_raw"/>
+        <remap from="camera_info" to="main_camera/camera_info"/>
+        <remap from="markers" to="aruco_detect/markers"/>
+        <param name="type" value="map"/>
+        <param name="map" value="$(find aruco_pose)/test/basic.txt"/>
+        <param name="markers/frame_id" value="aruco_map"/>
+        <param name="markers/child_frame_id_prefix" value="aruco_in_map_"/>
+    </node>
+
+    <param name="test_module" value="$(find aruco_pose)/test/basic.py"/>
+    <test test-name="aruco_pose_test" pkg="ros_pytest" type="ros_pytest_runner"/>
+</launch>

aruco_pose/test/basic.txt

@@ -0,0 +1,11 @@
+# Default markers
+1	0.33	0	0	0	0	0	0
+2	0.33	1	0	0	0	0	0
+3	0.33	0	1	0	0	0	0
+4	0.33	1	1	0	0	0	0
+# Marker with non-zero yaw rotation
+10	0.5	0.5	2	0	1.2	0	0
+# Marker with non-zero pitch and roll rotation
+11	0.2	0.5	0.5	0	0.0	-1	1
+# Marker with yaw, pitch and roll rotation
+12	0.4	0.2	0.5	0	0.1	-1.2	-0.5

aruco_pose/test/camera_info.yaml

@@ -0,0 +1,21 @@
+# some random camera calibration for testing
+image_width: 640
+image_height: 480
+camera_name: test_camera
+camera_matrix:
+  rows: 3
+  cols: 3
+  data: [643.229809, 0.000000, 356.811289, 0.000000, 644.318982, 299.150067, 0.000000, 0.000000, 1.000000]
+distortion_model: plumb_bob
+distortion_coefficients:
+  rows: 1
+  cols: 5
+  data: [-0.422907, 0.202567, 0.000781, 0.000447, 0.000000]
+rectification_matrix:
+  rows: 3
+  cols: 3
+  data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000]
+projection_matrix:
+  rows: 3
+  cols: 4
+  data: [567.010193, 0.000000, 366.677428, 0.000000, 0.000000, 594.591980, 307.043423, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000]

aruco_pose/test/largemap.py

@@ -0,0 +1,26 @@
+#!/usr/bin/env python
+import sys
+import unittest
+import json
+import rospy
+import rostest
+
+from sensor_msgs.msg import Image
+from visualization_msgs.msg import MarkerArray as VisMarkerArray
+
+
+class TestArucoPose(unittest.TestCase):
+    def setUp(self):
+        rospy.init_node('test_aruco_largemap', anonymous=True)
+
+    def test_map_image(self):
+        img = rospy.wait_for_message('aruco_map/image', Image, timeout=5)
+        self.assertEqual(img.width, 2000)
+        self.assertEqual(img.height, 2000)
+        self.assertEqual(img.encoding, 'mono8')
+
+    def test_map_visualization(self):
+        vis = rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5)
+
+
+rostest.rosrun('aruco_pose', 'test_aruco_largemap', TestArucoPose, sys.argv)

aruco_pose/test/largemap.test

@@ -0,0 +1,13 @@
+<launch>
+    <node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager" required="true"/>
+
+    <node name="aruco_map" pkg="nodelet" type="nodelet" args="load aruco_pose/aruco_map nodelet_manager" clear_params="true" required="true">
+        <remap from="image_raw" to="main_camera/image_raw"/>
+        <remap from="camera_info" to="main_camera/camera_info"/>
+        <remap from="markers" to="aruco_detect/markers"/>
+        <param name="type" value="map"/>
+        <param name="map" value="$(find aruco_pose)/test/largemap.txt"/>
+    </node>
+
+    <test test-name="test_aruco_pose_largemap" pkg="aruco_pose" type="largemap.py"/>
+</launch>

aruco_pose/test/largemap.txt

@@ -0,0 +1,11 @@
+0 0.2 0 0 0 0 0 0
+1 0.2 10 0 0 0 0 0
+2 0.2 20 0 0 0 0 0
+3 0.2 30 0 0 0 0 0
+4 0.2 40 0 0 0 0 0
+5 0.2 50 0 0 0 0 0
+6 0.2 60 0 0 0 0 0
+7 0.2 70 0 0 0 0 0
+8 0.2 80 0 0 0 0 0
+9 0.2 90 0 0 0 0 0
+10 0.2 100 0 0 0 0 0

aruco_pose/test/test_node_failure.py

@@ -0,0 +1,13 @@
+import rospy
+import pytest
+
+from visualization_msgs.msg import MarkerArray as VisMarkerArray
+
+
+@pytest.fixture
+def node():
+    return rospy.init_node('aruco_pose_test', anonymous=True)
+
+def test_node_failure(node):
+    with pytest.raises(rospy.exceptions.ROSException):
+        rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5)

aruco_pose/test/test_node_failure.test

@@ -0,0 +1,14 @@
+<launch>
+    <node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager"/>
+
+    <node name="aruco_map" pkg="nodelet" type="nodelet" args="load aruco_pose/aruco_map nodelet_manager" clear_params="true">
+        <remap from="image_raw" to="main_camera/image_raw"/>
+        <remap from="camera_info" to="main_camera/camera_info"/>
+        <remap from="markers" to="aruco_detect/markers"/>
+        <param name="type" value="map"/>
+        <param name="map" value="$(find aruco_pose)/test/test_node_failure.txt"/>
+    </node>
+
+    <param name="test_module" value="$(find aruco_pose)/test/test_node_failure.py"/>
+    <test test-name="test_node_failure" pkg="ros_pytest" type="ros_pytest_runner"/>
+</launch>

aruco_pose/test/test_node_failure.txt

@@ -0,0 +1,4 @@
+# Any garbage data (pretty much anything apart from a comment starting with a hash starting
+# with a hash sign or a number) will be interpreted as garbage data; the node should fail
+# after reading it.
+// Don't try to put your comments this way, it will kill your node!

aruco_pose/test/test_parser_empty_map.py

@@ -0,0 +1,13 @@
+import rospy
+import pytest
+
+from visualization_msgs.msg import MarkerArray as VisMarkerArray
+
+
+@pytest.fixture
+def node():
+    return rospy.init_node('aruco_pose_test_empty_map', anonymous=True)
+
+def test_empty_map(node):
+    markers = rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5)
+    assert len(markers.markers) == 0

aruco_pose/test/test_parser_empty_map.test

@@ -0,0 +1,14 @@
+<launch>
+    <node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager"/>
+
+    <node name="aruco_map" pkg="nodelet" type="nodelet" args="load aruco_pose/aruco_map nodelet_manager" clear_params="true">
+        <remap from="image_raw" to="main_camera/image_raw"/>
+        <remap from="camera_info" to="main_camera/camera_info"/>
+        <remap from="markers" to="aruco_detect/markers"/>
+        <param name="type" value="map"/>
+        <param name="map" value="$(find aruco_pose)/test/test_parser_empty_map.txt"/>
+    </node>
+
+    <param name="test_module" value="$(find aruco_pose)/test/test_parser_empty_map.py"/>
+    <test test-name="test_node_empty_map" pkg="ros_pytest" type="ros_pytest_runner"/>
+</launch>

aruco_pose/test/test_parser_empty_map.txt

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

aruco_pose/test/test_parser_pass.py

@@ -0,0 +1,61 @@
+import rospy
+import pytest
+
+from sensor_msgs.msg import Image
+from aruco_pose.msg import MarkerArray
+from visualization_msgs.msg import MarkerArray as VisMarkerArray
+
+
+@pytest.fixture
+def node():
+    return rospy.init_node('aruco_pose_test', anonymous=True)
+
+def approx(expected):
+    return pytest.approx(expected, abs=1e-4) # compare floats more roughly
+
+def test_markers(node):
+    markers = rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5)
+    assert len(markers.markers) == 6
+
+    assert markers.markers[0].pose.position.x == approx(0)
+    assert markers.markers[0].pose.position.y == approx(0)
+    assert markers.markers[0].pose.position.z == approx(0)
+
+    assert markers.markers[1].pose.position.x == approx(1)
+    assert markers.markers[1].pose.position.y == approx(1)
+    assert markers.markers[1].pose.position.z == approx(1)
+
+    assert markers.markers[2].pose.position.x == approx(1)
+    assert markers.markers[2].pose.position.y == approx(0)
+    assert markers.markers[2].pose.position.z == approx(0.5)
+
+    assert markers.markers[3].pose.position.x == approx(0)
+    assert markers.markers[3].pose.position.y == approx(1)
+    assert markers.markers[3].pose.position.z == approx(0)
+
+    assert markers.markers[4].pose.position.x == approx(1)
+    assert markers.markers[4].pose.position.y == approx(0.5)
+    assert markers.markers[4].pose.position.z == approx(0)
+
+    assert markers.markers[5].pose.position.x == approx(2.2)
+    assert markers.markers[5].pose.position.y == approx(0.2)
+    assert markers.markers[5].pose.position.z == approx(0)
+
+    assert markers.markers[0].scale.x == approx(0.33)
+    assert markers.markers[0].scale.y == approx(0.33)
+    assert markers.markers[1].scale.x == approx(0.225)
+    assert markers.markers[1].scale.y == approx(0.225)
+    assert markers.markers[2].scale.x == approx(0.45)
+    assert markers.markers[2].scale.y == approx(0.45)
+    assert markers.markers[3].scale.x == approx(0.15)
+    assert markers.markers[3].scale.y == approx(0.15)
+    assert markers.markers[4].scale.x == approx(0.25)
+    assert markers.markers[4].scale.y == approx(0.25)
+    assert markers.markers[5].scale.x == approx(0.35)
+    assert markers.markers[5].scale.y == approx(0.35)
+
+def test_map_image(node):
+    img = rospy.wait_for_message('aruco_map/image', Image, timeout=5)
+    assert img.width == 2000
+    assert img.height == 2000
+    assert img.encoding == 'mono8'

aruco_pose/test/test_parser_pass.test

@@ -0,0 +1,14 @@
+<launch>
+    <node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager" required="true"/>
+
+    <node name="aruco_map" pkg="nodelet" type="nodelet" args="load aruco_pose/aruco_map nodelet_manager" clear_params="true" required="true">
+        <remap from="image_raw" to="main_camera/image_raw"/>
+        <remap from="camera_info" to="main_camera/camera_info"/>
+        <remap from="markers" to="aruco_detect/markers"/>
+        <param name="type" value="map"/>
+        <param name="map" value="$(find aruco_pose)/test/test_parser_pass.txt"/>
+    </node>
+
+    <param name="test_module" value="$(find aruco_pose)/test/test_parser_pass.py"/>
+    <test test-name="test_node_pass" pkg="ros_pytest" type="ros_pytest_runner"/>
+</launch>

aruco_pose/test/test_parser_pass.txt

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

book.json

@@ -4,21 +4,58 @@
     "author": "Copter Express",
     "language": "ru",
     "root": "docs/",
+    "gitbook": "^3.2.2",
     "plugins": [
-        "youtube",
         "richquotes@https://github.com/okalachev/gitbook-plugin-richquotes.git",
         "yametrika",
         "anchors",
         "validate-links",
-        "bulk-redirect@https://github.com/okalachev/gitbook-plugin-bulk-redirect.git"
+        "bulk-redirect@https://github.com/okalachev/gitbook-plugin-bulk-redirect.git",
+        "sitemap@https://github.com/okalachev/plugin-sitemap.git",
+        "toolbar@https://github.com/hamishwillee/gitbook-plugin-toolbar.git",
+        "addcssjs",
+        "localized-footer@https://github.com/okalachev/gitbook-plugin-localized-footer.git",
+        "image-zoom@https://github.com/okalachev/gitbook-plugin-image-zoom.git",
+        "language-picker@https://github.com/okalachev/gitbook-plugin-language-picker.git"
     ],
     "pluginsConfig": {
         "yametrika": {
             "id": 49359238
         },
         "bulk-redirect": {
-            "basepath": "/",
+            "basepath": "",
             "redirectsFile": "redirects.json"
+        },
+        "sitemap": {
+            "hostname": "https://clever.copterexpress.com"
+        },
+        "toolbar": {
+            "buttons":
+            [
+                {
+                    "label": "Edit page on github",
+                    "icon": "fa fa-pencil-square-o",
+                    "position" : "left",
+                    "url": "https://github.com/CopterExpress/clever/edit/master/docs/{{filepath_lang}}"
+                },
+                {
+                    "label": "GitHub",
+                    "icon": "fa fa-github",
+                    "position" : "left",
+                    "url": "https://github.com/CopterExpress/clever"
+                }
+            ]
+        },
+        "addcssjs": {
+            "css": ["../clever.css"],
+            "js": ["../clever.js"]
+        },
+        "language-picker": {
+            "languages": [["ru", "Russian"], ["en", "English"]]
+        },
+        "localized-footer": {
+            "hline": false,
+            "filename": "./FOOTER.md"
         }
     }
 }

builder/assets/clever.service

@@ -1,12 +1,13 @@
 [Unit]
 Description=Clever ROS package
 Requires=roscore.service
-After=roscore.service
+After=network.target
 
 [Service]
-EnvironmentFile=/lib/systemd/system/roscore.env
-ExecStart=/opt/ros/kinetic/bin/roslaunch clever clever.launch --wait
-Restart=on-abort
+User=pi
+ExecStart=/bin/bash -c ". /home/pi/catkin_ws/devel/setup.sh; \
+                      ROS_HOSTNAME=`hostname`.local exec stdbuf -o L roslaunch clever clever.launch --wait --screen --skip-log-check \
+                      2> >(tee /tmp/clever.err)"
 
 [Install]
 WantedBy=multi-user.target

builder/assets/hardware_setup.sh

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

builder/assets/init_rpi.sh

@@ -8,6 +8,10 @@
 #
 # Author: Artem Smirnov <urpylka@gmail.com>
 #
+# Distributed under MIT License (available at https://opensource.org/licenses/MIT).
+# The above copyright notice and this permission notice shall be included in all
+# copies or substantial portions of the Software.
+#
 
 set -e # Exit immidiately on non-zero result
 
@@ -31,8 +35,15 @@ echo_stamp() {
   echo -e ${TEXT}
 }
 
-echo_stamp "Rename SSID"
-sudo sed -i.OLD "s/CLEVER/CLEVER-$(head -c 100 /dev/urandom | xxd -ps -c 100 | sed -e 's/[^0-9]//g' | cut -c 1-4)/g" /etc/wpa_supplicant/wpa_supplicant.conf
+NEW_SSID='CLEVER-'$(head -c 100 /dev/urandom | xxd -ps -c 100 | sed -e "s/[^0-9]//g" | cut -c 1-4)
+echo_stamp "Setting SSID to ${NEW_SSID}"
+sudo sed -i.OLD "s/CLEVER/${NEW_SSID}/" /etc/wpa_supplicant/wpa_supplicant.conf
+
+NEW_HOSTNAME=$(echo ${NEW_SSID} | tr '[:upper:]' '[:lower:]')
+echo_stamp "Setting hostname to $NEW_HOSTNAME"
+hostnamectl set-hostname $NEW_HOSTNAME
+sed -i 's/127\.0\.1\.1.*/127.0.1.1\t'${NEW_HOSTNAME}' '${NEW_HOSTNAME}'.local/g' /etc/hosts
+# .local (mdns) hostname added to make it accesable when wlan and ethernet interfaces are down
 
 echo_stamp "Harware setup"
 /root/hardware_setup.sh

builder/assets/kinetic-rosdep-clever.yaml

@@ -529,3 +529,33 @@ libogg:
 vl53l1x:
   debian:
     stretch: [ros-kinetic-vl53l1x]
+ws281x:
+  debian:
+    stretch: [ros-kinetic-ws281x]
+led_msgs:
+  debian:
+    stretch: [ros-kinetic-led-msgs]
+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]
+image_publisher:
+  debian:
+    stretch: [ros-kinetic-image-publisher]
+raspberry-kernel-headers:
+  debian:
+    stretch: [raspberry-kernel-headers]
+ddynamic_reconfigure:
+  debian:
+    stretch: [ros-kinetic-ddynamic-reconfigure]
+realsense2_camera:
+  debian:
+    stretch: [ros-kinetic-realsense2-camera]
+ros_pytest:
+  debian:
+    stretch: [ros-kinetic-ros-pytest]

builder/assets/launch.nanorc

@@ -0,0 +1,22 @@
+## ROS .launch files (which are actually xml files)
+
+syntax "launch" "\.(launch)$"
+header "<\?xml.*version=.*\?>"
+magic "(XML|SGML) (sub)?document"
+comment "<!--|-->"
+
+# The entire content of the tag:
+color green  start="<" end=">"
+
+# The start and the end of the tag:
+color cyan   "<[^> ]+" ">"
+
+# The strings inside the tag:
+color magenta "\"[^"]*\""
+
+# Comments:
+color yellow start="<!DOCTYPE" end="[/]?>"
+color yellow start="<!--" end="-->"
+
+# Escapes:
+color red    "&[^;]*;"

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

builder/assets/pigpiod.service

@@ -0,0 +1,8 @@
+[Unit]
+Description=Daemon required to control GPIO pins via pigpio
+[Service]
+ExecStart=/usr/bin/pigpiod -l -t 0 -x 0x0FFF3FF0
+ExecStop=/bin/systemctl kill pigpiod
+Type=forking
+[Install]
+WantedBy=multi-user.target

builder/assets/ros_python_paths

@@ -0,0 +1,8 @@
+Defaults        env_keep += "PYTHONPATH"
+Defaults        env_keep += "PATH"
+Defaults        env_keep += "ROS_ROOT"
+Defaults        env_keep += "ROS_MASTER_URI"
+Defaults        env_keep += "ROS_PACKAGE_PATH"
+Defaults        env_keep += "ROS_LOCATIONS"
+Defaults        env_keep += "ROS_HOME"
+Defaults        env_keep += "ROS_LOG_DIR"

builder/assets/roscore.env

@@ -1,10 +0,0 @@
-ROS_ROOT=/opt/ros/kinetic/share/ros
-ROS_DISTRO=kinetic
-ROS_PACKAGE_PATH=/home/pi/catkin_ws/src:/opt/ros/kinetic/share
-ROS_PORT=11311
-ROS_MASTER_URI=http://localhost:11311
-CMAKE_PREFIX_PATH=/home/pi/catkin_ws/devel:/opt/ros/kinetic
-PATH=/opt/ros/kinetic/bin:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin:/usr/games:/usr/local/games:/snap/bin
-LD_LIBRARY_PATH=/opt/ros/kinetic/lib
-PYTHONPATH=/home/pi/catkin_ws/devel/lib/python2.7/dist-packages:/opt/ros/kinetic/lib/python2.7/dist-packages
-ROS_IP=192.168.11.1

builder/assets/roscore.service

@@ -3,9 +3,10 @@ Description=Launcher for the ROS master, parameter server and rosout logging nod
 After=network.target
 
 [Service]
-EnvironmentFile=/lib/systemd/system/roscore.env
-ExecStart=/opt/ros/kinetic/bin/roscore
-Restart=on-abort
+User=pi
+ExecStart=/bin/sh -c ". /opt/ros/kinetic/setup.sh; ROS_HOSTNAME=`hostname`.local exec roscore"
+Restart=on-failure
+RestartSec=3
 
 [Install]
 WantedBy=multi-user.target

builder/image-build.sh

@@ -1,17 +1,21 @@
 #! /usr/bin/env bash
 
 #
-# Script for build the image. Used builder script of the target repo
+# Script for build the image. Used builder script of the target repo.
 # For build: docker run --privileged -it --rm -v /dev:/dev -v $(pwd):/builder/repo smirart/builder
 #
 # Copyright (C) 2018 Copter Express Technologies
 #
 # Author: Artem Smirnov <urpylka@gmail.com>
 #
+# Distributed under MIT License (available at https://opensource.org/licenses/MIT).
+# The above copyright notice and this permission notice shall be included in all
+# copies or substantial portions of the Software.
+#
 
 set -e # Exit immidiately on non-zero result
 
-SOURCE_IMAGE="http://repo.coex.space/2018-06-27-raspbian-stretch-lite.zip"
+SOURCE_IMAGE="https://downloads.raspberrypi.org/raspbian_lite/images/raspbian_lite-2019-04-09/2019-04-08-raspbian-stretch-lite.zip"
 
 export DEBIAN_FRONTEND=${DEBIAN_FRONTEND:='noninteractive'}
 export LANG=${LANG:='C.UTF-8'}
@@ -53,16 +57,15 @@ IMAGE_NAME="${REPO_NAME}_${IMAGE_VERSION}.img"
 IMAGE_PATH="${IMAGES_DIR}/${IMAGE_NAME}"
 
 get_image() {
-  # TEMPLATE: get_image <IMAGE_PATH> <RPI_DONWLOAD_URL> 
+  # TEMPLATE: get_image <IMAGE_PATH> <RPI_DONWLOAD_URL>
   local BUILD_DIR=$(dirname $1)
   local RPI_ZIP_NAME=$(basename $2)
   local RPI_IMAGE_NAME=$(echo ${RPI_ZIP_NAME} | sed 's/zip/img/')
 
   if [ ! -e "${BUILD_DIR}/${RPI_ZIP_NAME}" ]; then
-    echo_stamp "Downloading original Linux distribution" \
-    && wget -nv -O ${BUILD_DIR}/${RPI_ZIP_NAME} $2 \
-    && echo_stamp "Downloading complete" "SUCCESS" \
-    || (echo_stamp "Downloading was failed!" "ERROR"; exit 1)
+    echo_stamp "Downloading original Linux distribution"
+    wget --progress=dot:giga -O ${BUILD_DIR}/${RPI_ZIP_NAME} $2
+    echo_stamp "Downloading complete" "SUCCESS" \
   else echo_stamp "Linux distribution already donwloaded"; fi
 
   echo_stamp "Unzipping Linux distribution image" \
@@ -80,18 +83,18 @@ ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/init_rp
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/hardware_setup.sh' '/root/'
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} exec ${SCRIPTS_DIR}'/image-init.sh' ${IMAGE_VERSION} ${SOURCE_IMAGE}
 
+# Copy cloned repository to the image
+# Include dotfiles in globs (asterisks)
+shopt -s dotglob
+for dir in ${REPO_DIR}/*; do
+  # Don't try to copy image into itself
+  if [[ $dir != *"images" && $dir != *"imgcache" ]]; then
+    ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy $dir '/home/pi/catkin_ws/src/clever/'
+  fi;
+done
+
 # 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/'
@@ -106,10 +109,16 @@ ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} exec ${SCRIPTS_DIR}'/image-network.
 [[ $(arch) == 'armv7l' ]] && NUMBER_THREADS=1 || NUMBER_THREADS=$(nproc --all)
 # Clever
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/clever.service' '/lib/systemd/system/'
-${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/roscore.env' '/lib/systemd/system/'
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/roscore.service' '/lib/systemd/system/'
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/kinetic-rosdep-clever.yaml' '/etc/ros/rosdep/'
+${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/ros_python_paths' '/etc/sudoers.d/'
+${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/pigpiod.service' '/lib/systemd/system/'
+${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/launch.nanorc' '/usr/share/nano/'
 # ${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} 
+# Add PX4 udev rules
+${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${REPO_DIR}'/clever/config/99-px4fmu.rules' '/lib/udev/rules.d/'
+# Add rename script
+${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-validate.sh'
 
 ${BUILDER_DIR}/image-resize.sh ${IMAGE_PATH}

builder/image-init.sh

@@ -1,12 +1,16 @@
 #! /usr/bin/env bash
 
 #
-# Script for initialisation image
+# Script for image initialisation
 #
 # Copyright (C) 2018 Copter Express Technologies
 #
 # Author: Artem Smirnov <urpylka@gmail.com>
 #
+# Distributed under MIT License (available at https://opensource.org/licenses/MIT).
+# The above copyright notice and this permission notice shall be included in all
+# copies or substantial portions of the Software.
+#
 
 set -e # Exit immidiately on non-zero result
 

builder/image-network.sh

@@ -1,12 +1,16 @@
 #! /usr/bin/env bash
 
 #
-# Script for network configure
+# Script for network configuration
 #
 # Copyright (C) 2018 Copter Express Technologies
 #
 # Author: Artem Smirnov <urpylka@gmail.com>
 #
+# Distributed under MIT License (available at https://opensource.org/licenses/MIT).
+# The above copyright notice and this permission notice shall be included in all
+# copies or substantial portions of the Software.
+#
 
 set -e # Exit immidiately on non-zero result
 

builder/image-ros.sh

@@ -8,6 +8,10 @@
 #
 # Author: Artem Smirnov <urpylka@gmail.com>
 #
+# Distributed under MIT License (available at https://opensource.org/licenses/MIT).
+# The above copyright notice and this permission notice shall be included in all
+# copies or substantial portions of the Software.
+#
 
 set -e # Exit immidiately on non-zero result
 
@@ -42,9 +46,10 @@ echo_stamp() {
 my_travis_retry() {
   local result=0
   local count=1
-  while [ $count -le 3 ]; do
+  local max_count=50
+  while [ $count -le $max_count ]; do
     [ $result -ne 0 ] && {
-      echo -e "\n${ANSI_RED}The command \"$@\" failed. Retrying, $count of 3.${ANSI_RESET}\n" >&2
+      echo -e "\nThe command \"$@\" failed. Retrying, $count of $max_count.\n" >&2
     }
     # ! { } ignores set -e, see https://stackoverflow.com/a/4073372
     ! { "$@"; result=$?; }
@@ -53,18 +58,21 @@ my_travis_retry() {
     sleep 1
   done
 
-  [ $count -gt 3 ] && {
-    echo -e "\n${ANSI_RED}The command \"$@\" failed 3 times.${ANSI_RESET}\n" >&2
+  [ $count -gt $max_count ] && {
+    echo -e "\nThe command \"$@\" failed $max_count times.\n" >&2
   }
 
   return $result
 }
 
 # TODO: 'kinetic-rosdep-clever.yaml' should add only if we use our repo?
-echo_stamp "Init rosdep" \
-&& rosdep init \
-&& echo "yaml file:///etc/ros/rosdep/kinetic-rosdep-clever.yaml" >> /etc/ros/rosdep/sources.list.d/20-default.list \
-&& rosdep update
+echo_stamp "Init rosdep"
+my_travis_retry rosdep init
+echo "yaml file:///etc/ros/rosdep/kinetic-rosdep-clever.yaml" >> /etc/ros/rosdep/sources.list.d/20-default.list
+my_travis_retry rosdep update
+
+echo_stamp "Populate rosdep for ROS user"
+my_travis_retry sudo -u pi rosdep update
 
 resolve_rosdep() {
   # TEMPLATE: resolve_rosdep <CATKIN_PATH> <ROS_DISTRO> <OS_DISTRO> <OS_VERSION>
@@ -75,7 +83,7 @@ resolve_rosdep() {
 
   echo_stamp "Installing dependencies apps with rosdep in ${CATKIN_PATH}"
   cd ${CATKIN_PATH}
-  my_travis_retry rosdep install -y --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} -r --os=${OS_DISTRO}:${OS_VERSION}
+  my_travis_retry rosdep install -y --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} --os=${OS_DISTRO}:${OS_VERSION}
 }
 
 INSTALL_ROS_PACK_SOURCES=${INSTALL_ROS_PACK_SOURCES:='false'}
@@ -132,22 +140,32 @@ if [ "${INSTALL_ROS_PACK_SOURCES}" = "true" ]; then
   chown -Rf pi:pi /home/pi/ros_catkin_ws
 fi
 
+export ROS_IP='127.0.0.1' # needed for running tests
+
+echo_stamp "Reconfiguring Clever repository for simplier unshallowing"
+cd /home/pi/catkin_ws/src/clever
+git config remote.origin.fetch "+refs/heads/*:refs/remotes/origin/*"
+
 echo_stamp "Installing CLEVER" \
-&& git clone ${REPO} /home/pi/catkin_ws/src/clever \
 && cd /home/pi/catkin_ws/src/clever \
-&& echo "REF: ${REF}" \
-&& git checkout ${REF} \
+&& git status \
 && cd /home/pi/catkin_ws \
 && resolve_rosdep $(pwd) \
 && my_travis_retry pip install wheel \
 && my_travis_retry pip install -r /home/pi/catkin_ws/src/clever/clever/requirements.txt \
 && source /opt/ros/kinetic/setup.bash \
-&& catkin_make -j${NUMBER_THREADS} -DCMAKE_BUILD_TYPE=Release \
+&& catkin_make -j2 -DCMAKE_BUILD_TYPE=Release \
 && systemctl enable roscore \
 && systemctl enable clever \
 && echo_stamp "All CLEVER was installed!" "SUCCESS" \
 || (echo_stamp "CLEVER installation was failed!" "ERROR"; exit 1)
 
+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 \
@@ -155,12 +173,18 @@ apt-get install -y --no-install-recommends \
     ros-kinetic-rosbridge-suite \
     ros-kinetic-rosserial \
     ros-kinetic-usb-cam \
-    ros-kinetic-vl53l1x
+    ros-kinetic-vl53l1x \
+    ros-kinetic-ws281x \
+    ros-kinetic-opencv3=3.3.19-0stretch \
+    ros-kinetic-rosshow
 
 # TODO move GeographicLib datasets to Mavros debian package
-echo_stamp "Install GeographicLib datasets (needs for mavros)" \
+echo_stamp "Install GeographicLib datasets (needed for mavros)" \
 && wget -qO- https://raw.githubusercontent.com/mavlink/mavros/master/mavros/scripts/install_geographiclib_datasets.sh | bash
 
+echo_stamp "Running tests"
+catkin_make run_tests && catkin_test_results
+
 echo_stamp "Change permissions for catkin_ws"
 chown -Rf pi:pi /home/pi/catkin_ws
 
@@ -169,7 +193,7 @@ cat << EOF >> /home/pi/.bashrc
 LANG='C.UTF-8'
 LC_ALL='C.UTF-8'
 ROS_DISTRO='kinetic'
-export ROS_IP='192.168.11.1'
+export ROS_HOSTNAME=\`hostname\`.local
 source /opt/ros/kinetic/setup.bash
 source /home/pi/catkin_ws/devel/setup.bash
 EOF

builder/image-software.sh

@@ -1,12 +1,16 @@
 #! /usr/bin/env bash
 
 #
-# Script for install software to the image.
+# Script for installing software to the image.
 #
 # Copyright (C) 2018 Copter Express Technologies
 #
 # Author: Artem Smirnov <urpylka@gmail.com>
 #
+# Distributed under MIT License (available at https://opensource.org/licenses/MIT).
+# The above copyright notice and this permission notice shall be included in all
+# copies or substantial portions of the Software.
+#
 
 set -e # Exit immidiately on non-zero result
 
@@ -56,14 +60,14 @@ my_travis_retry() {
 echo_stamp "Install apt keys & repos"
 
 # TODO: This STDOUT consist 'OK'
-curl http://repo.coex.space/aptly_repo_signing.key 2> /dev/null | apt-key add -
+curl http://deb.coex.tech/aptly_repo_signing.key 2> /dev/null | apt-key add -
 apt-get update \
-&& apt-get install --no-install-recommends -y -qq dirmngr=2.1.18-8~deb9u3 > /dev/null \
-&& apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key 421C365BD9FF1F717815A3895523BAEEB01FA116
+&& apt-get install --no-install-recommends -y -qq dirmngr=2.1.18-8~deb9u4 > /dev/null \
+&& apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654
 
 echo "deb http://packages.ros.org/ros/ubuntu stretch main" > /etc/apt/sources.list.d/ros-latest.list
-echo "deb http://repo.coex.space/rpi-ros-kinetic stretch main" > /etc/apt/sources.list.d/rpi-ros-kinetic.list
-echo "deb http://repo.coex.space/clever stretch main" > /etc/apt/sources.list.d/clever.list
+echo "deb http://deb.coex.tech/rpi-ros-kinetic stretch main" > /etc/apt/sources.list.d/rpi-ros-kinetic.list
+echo "deb http://deb.coex.tech/clever stretch main" > /etc/apt/sources.list.d/clever.list
 
 echo_stamp "Update apt cache"
 
@@ -84,16 +88,14 @@ lsof=4.89+dfsg-0.1 \
 git \
 dnsmasq=2.76-5+rpt1+deb9u1  \
 tmux=2.3-4 \
-vim=2:8.0.0197-4+deb9u1 \
+vim \
 cmake=3.7.2-1 \
-python-pip=9.0.1-2+rpt2 \
-python3-pip=9.0.1-2+rpt2 \
-libjpeg8-dev=8d1-2 \
+libjpeg8=8d1-2 \
 tcpdump \
 ltrace \
 libpoco-dev=1.7.6+dfsg1-5+deb9u1 \
-python-rosdep=0.14.0-1 \
-python-rosinstall-generator=0.1.14-1 \
+python-rosdep \
+python-rosinstall-generator \
 python-wstool=0.1.17-1 \
 python-rosinstall=0.7.8-1 \
 build-essential=12.3 \
@@ -101,36 +103,63 @@ libffi-dev \
 monkey=1.6.9-1 \
 pigpio python-pigpio python3-pigpio \
 i2c-tools \
+espeak espeak-data python-espeak \
+ntpdate \
+python-dev \
+python3-dev \
+mjpg-streamer=2.0 \
 && echo_stamp "Everything was installed!" "SUCCESS" \
 || (echo_stamp "Some packages wasn't installed!" "ERROR"; exit 1)
 
+echo_stamp "Updating kernel to fix camera bug"
+apt-get install --no-install-recommends -y \
+	raspberrypi-kernel=1.20190819~stretch-1 \
+	raspberrypi-bootloader=1.20190819~stretch-1 \
+	libraspberrypi-bin=1.20190819~stretch-1 \
+	libraspberrypi-dev=1.20190819~stretch-1 \
+	libraspberrypi0=1.20190819~stretch-1 \
+	wireless-regdb=2018.05.09-0~rpt1 \
+	wpasupplicant=2:2.6-21~bpo9~rpt1
+
 # Deny byobu to check available updates
 sed -i "s/updates_available//" /usr/share/byobu/status/status
 # sed -i "s/updates_available//" /home/pi/.byobu/status
 
-#echo_stamp "Upgrade pip"
+echo_stamp "Installing pip"
+curl https://bootstrap.pypa.io/get-pip.py -o get-pip.py
+python3 get-pip.py
+python get-pip.py
+rm get-pip.py
 #my_travis_retry pip install --upgrade pip
 #my_travis_retry pip3 install --upgrade pip
 
-echo_stamp "Not upgrading system pip due to https://github.com/pypa/pip/issues/5599"
-
 echo_stamp "Make sure both pip and pip3 are installed"
 pip --version
 pip3 --version
 
 echo_stamp "Install and enable Butterfly (web terminal)"
+echo_stamp "Workaround for tornado >= 6.0 breaking butterfly"
+my_travis_retry pip3 install tornado==5.1.1
 my_travis_retry pip3 install butterfly
 my_travis_retry pip3 install butterfly[systemd]
 systemctl enable butterfly.socket
 
 echo_stamp "Install ws281x library"
-my_travis_retry pip install rpi_ws281x
+my_travis_retry pip install --prefer-binary 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/
+rm node-v10.15.0-linux-armv6l.tar.gz
+
 echo_stamp "Add .vimrc"
 cat << EOF > /home/pi/.vimrc
 set mouse-=a
@@ -138,10 +167,13 @@ syntax on
 autocmd BufNewFile,BufRead *.launch set syntax=xml
 EOF
 
+echo_stamp "Change default keyboard layout to US"
+sed -i 's/XKBLAYOUT="gb"/XKBLAYOUT="us"/g' /etc/default/keyboard
+
 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
+# We ignore pkill's exit value as well.
+pkill -9 -f dirmngr || true
 
 echo_stamp "End of software installation"

builder/image-validate.sh

@@ -0,0 +1,26 @@
+#!/usr/bin/env bash
+
+#
+# Validate built image using tests
+#
+# Copyright (C) 2018 Copter Express Technologies
+#
+# Author: Oleg Kalachev <okalachev@gmail.com>
+#
+# Distributed under MIT License (available at https://opensource.org/licenses/MIT).
+# The above copyright notice and this permission notice shall be included in all
+# copies or substantial portions of the Software.
+#
+
+set -ex
+
+echo "Run image tests"
+
+export ROS_DISTRO='kinetic'
+export ROS_IP='127.0.0.1'
+source /opt/ros/kinetic/setup.bash
+source /home/pi/catkin_ws/devel/setup.bash
+
+cd /home/pi/catkin_ws/src/clever/builder/test/
+./tests.sh
+./tests.py

builder/test/tests.py

@@ -0,0 +1,30 @@
+#!/usr/bin/env python
+
+# validate all required modules installed
+
+import rospy
+from geometry_msgs.msg import PoseStamped
+
+import cv2
+import cv2.aruco
+
+import numpy
+import mavros
+from mavros_msgs.msg import State, StatusText, ExtendedState
+from mavros_msgs.srv import CommandBool, CommandLong, SetMode
+
+from std_srvs.srv import Trigger
+from clever.srv import GetTelemetry, Navigate, NavigateGlobal, SetPosition, SetVelocity, \
+    SetAttitude, SetRates
+
+import tf2_ros
+import tf2_geometry_msgs
+
+import VL53L1X
+import pymavlink
+from pymavlink import mavutil
+import rpi_ws281x
+import pigpio
+from espeak import espeak
+
+print cv2.getBuildInformation()

builder/test/tests.sh

@@ -0,0 +1,51 @@
+#!/usr/bin/env bash
+
+set -ex
+
+# TODO: validate versions
+
+# validate required software is installed
+
+python --version
+python2 --version
+python3 --version
+ipython --version
+ipython3 --version
+
+node -v
+npm -v
+
+byobu --version
+nmap --version
+lsof -v
+git --version
+vim --version
+pip --version
+pip2 --version
+pip3 --version
+tcpdump --version
+monkey --version
+pigpiod -v
+i2cdetect -V
+butterfly -h
+espeak --version
+mjpg_streamer --version
+
+# ros stuff
+
+roscore -h
+rosversion clever
+rosversion aruco_pose
+rosversion vl53l1x
+rosversion opencv3
+rosversion mavros
+rosversion mavros_extras
+rosversion dynamic_reconfigure
+rosversion tf2_web_republisher
+rosversion compressed_image_transport
+rosversion rosbridge_suite
+rosversion rosserial
+rosversion usb_cam
+rosversion cv_camera
+rosversion web_video_server
+rosversion rosshow

check_assets_size.py

@@ -0,0 +1,34 @@
+#!/usr/bin/env python3
+
+import os
+import sys
+
+def human_size(num, suffix='B'):
+    for unit in ['', 'Ki', 'Mi', 'Gi', 'Ti', 'Pi', 'Ei', 'Zi']:
+        if abs(num) < 1024.0:
+            return "%3.1f %s%s" % (num, unit, suffix)
+        num /= 1024.0
+    return "%.1f %s%s" % (num, 'Yi', suffix)
+
+SIZE_LIMIT = 800 * 1024
+EXCLUDE = 'rviz.png', 'ssid.png', 'sitl_docker_demo.png', 'qgc-params.png', 'butterfly.png', \
+    'Clever main.png', 'fpv_3.png', '1_4.png', 'fpv_4.png', 'detal1.png', 'lockradio.png', \
+    'qground.png', 'allElements.png', 'download-log.png', 'explosion.png', 'rqt.png', \
+    'cl3_mountBEC.JPG', 'cl3_mountRpiCamera.JPG', 'clever4-front-black-large.png', \
+    'qgc-battery.png', 'qgc-radio.png', 'qgc-cal-acc.png', 'qgc-esc.png', 'qgc-cal-compass.png', \
+    'qgc.png', 'qgc-parameters.png', 'clever4-front-white-large.png', 'qgc-modes.png', \
+    'qgc-requires-setup.png', 'clever4-front-white.png', 'clever4-kit-white.png', '26_1.png'
+
+code = 0
+
+for root, dirs, files in os.walk('docs/'):
+    for f in files:
+        if f not in EXCLUDE:
+            path = os.path.join(root, f)
+            size = os.path.getsize(path)
+            if size > SIZE_LIMIT:
+                print('\x1b[1;31mFile too large ({}): {}\x1b[0m'.format(human_size(size), path), \
+                    file=sys.stderr)
+                code = 1
+
+sys.exit(code)

check_unused_assets.py

@@ -0,0 +1,22 @@
+#!/usr/bin/env python3
+
+import os
+import sys
+import subprocess
+
+EXCLUDE = ('clever4-front-white.png', '.DS_Store', 'clever4-front-black-large.png')
+code = 0
+
+os.chdir('./docs')
+
+for root, dirs, files in os.walk('assets'):
+    for f in files:
+        if f not in EXCLUDE:
+            path = os.path.join(root, f)
+            try:
+                subprocess.check_output(['grep', '-F', '-r', path, './ru', './en'])
+            except subprocess.CalledProcessError:
+                print('\x1b[1;31mAssets file {} is not used\x1b[0m'.format(path))
+                code = 1
+
+sys.exit(code)

clever/CMakeLists.txt

@@ -80,6 +80,7 @@ add_service_files(
   SetVelocity.srv
   SetAttitude.srv
   SetRates.srv
+  SetLEDEffect.srv
 )
 
 ## Generate actions in the 'action' folder
@@ -148,10 +149,6 @@ add_library(clever
   src/optical_flow.cpp
 )
 
-add_library(aruco_vpe
-  src/aruco_vpe.cpp
-)
-
 ## Add cmake target dependencies of the library
 ## as an example, code may need to be generated before libraries
 ## either from message generation or dynamic reconfigure
@@ -166,7 +163,9 @@ add_executable(rc src/rc.cpp)
 
 add_executable(camera_markers src/camera_markers.cpp)
 
-add_executable(frames src/frames.cpp)
+add_executable(vpe_publisher src/vpe_publisher.cpp)
+
+add_executable(led src/led.cpp)
 
 target_link_libraries(simple_offboard
   ${catkin_LIBRARIES}
@@ -177,10 +176,14 @@ target_link_libraries(rc ${catkin_LIBRARIES})
 
 target_link_libraries(camera_markers ${catkin_LIBRARIES})
 
-target_link_libraries(frames ${catkin_LIBRARIES})
+target_link_libraries(vpe_publisher ${catkin_LIBRARIES})
+
+target_link_libraries(led ${catkin_LIBRARIES})
 
 add_dependencies(simple_offboard clever_generate_messages_cpp)
 
+add_dependencies(led 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
@@ -196,10 +199,6 @@ target_link_libraries(clever
   ${catkin_LIBRARIES}
 )
 
-target_link_libraries(aruco_vpe
-  ${catkin_LIBRARIES}
-)
-
 #############
 ## Install ##
 #############
@@ -235,6 +234,19 @@ target_link_libraries(aruco_vpe
 #   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 # )
 
+# Only install udev rules when building a Debian package
+# FIXME: Other operating systems may have other prefixes
+string(FIND ${CMAKE_INSTALL_PREFIX} "/opt/ros" _PREFIX_INDEX)
+if (${_PREFIX_INDEX} EQUAL 0)
+  message(STATUS "Building as a Debian package - adding udev rules as installable files")
+  install(FILES
+    config/99-px4fmu.rules
+    DESTINATION /lib/udev/rules.d
+  )
+else()
+  message(STATUS "Building in a user workspace - not adding udev rules")
+endif()
+
 #############
 ## Testing ##
 #############
@@ -247,3 +259,8 @@ target_link_libraries(aruco_vpe
 
 ## Add folders to be run by python nosetests
 # catkin_add_nosetests(test)
+
+if (CATKIN_ENABLE_TESTING)
+  find_package(rostest REQUIRED)
+  add_rostest(test/basic.test)
+endif()

clever/config/99-px4fmu.rules

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

clever/launch/arduino.launch

@@ -1,9 +0,0 @@
-<launch>
-    <!-- Bridge to connected Arduino using rosserial -->
-    <arg name="device" default="/dev/serial/by-id/usb-1a86_USB2.0-Serial-if00-port0"/>
-    <!-- TODO: UART connection -->
-
-    <node pkg="rosserial_python" type="serial_node.py" name="serial_node" output="screen">
-        <param name="port" value="$(arg device)"/>
-    </node>
-</launch>

clever/launch/aruco.launch

@@ -1,24 +1,41 @@
 <launch>
-    <remap from="image" to="main_camera/image_raw"/>
-    <remap from="camera_info" to="main_camera/camera_info"/>
+    <arg name="aruco_detect" default="true"/>
+    <arg name="aruco_map" default="false"/>
+    <arg name="aruco_vpe" default="false"/>
 
-    <node pkg="nodelet" type="nodelet" name="aruco_pose" args="load aruco_pose/aruco_pose nodelet_manager" clear_params="true">
-        <param name="frame_id" value="aruco_map_raw"/>
-        <param name="type" value="gridboard"/>
-        <param name="markers_x" value="1"/>
-        <param name="markers_y" value="6"/>
-        <param name="first_marker" value="240"/>
-        <param name="markers_side" value="0.3362"/>
-        <param name="markers_sep" value="0.46"/>
+    <!-- For additional help go to https://clever.copterexpress.com/aruco.html -->
 
-        <!-- Custom gridboard: -->
-        <!--<rosparam param="marker_ids">[6, 5, 4, 3, 2, 1]</rosparam>-->
+    <!-- aruco_detect: detect aruco markers, estimate poses -->
+    <node name="aruco_detect" pkg="nodelet" if="$(arg aruco_detect)" type="nodelet" args="load aruco_pose/aruco_detect nodelet_manager" output="screen" clear_params="true">
+        <remap from="image_raw" to="main_camera/image_raw"/>
+        <remap from="camera_info" to="main_camera/camera_info"/>
+        <remap from="map_markers" to="aruco_map/markers" if="$(arg aruco_map)"/>
+        <param name="estimate_poses" value="true"/>
+        <param name="send_tf" value="true"/>
+        <param name="known_tilt" value="map"/>
+        <param name="length" value="0.33"/>
     </node>
 
-    <node pkg="nodelet" type="nodelet" name="aruco_vpe" args="load clever/aruco_vpe nodelet_manager" clear_params="true">
-        <param name="aruco_orientation" value="map"/>
-        <!--<param name="aruco_orientation" value="map_upside_down"/>-->
+    <!-- aruco_map: estimate aruco map pose -->
+    <node name="aruco_map" pkg="nodelet" type="nodelet" if="$(arg aruco_map)" args="load aruco_pose/aruco_map nodelet_manager" output="screen" clear_params="true">
+        <remap from="image_raw" to="main_camera/image_raw"/>
+        <remap from="camera_info" to="main_camera/camera_info"/>
+        <remap from="markers" to="aruco_detect/markers"/>
+        <param name="map" value="$(find aruco_pose)/map/map.txt"/>
+        <param name="known_tilt" value="map"/>
+        <param name="image_axis" value="true"/>
+        <param name="frame_id" value="aruco_map_detected" if="$(arg aruco_vpe)"/>
+        <param name="frame_id" value="aruco_map" unless="$(arg aruco_vpe)"/>
+        <param name="markers/frame_id" value="aruco_map"/>
+        <param name="markers/child_frame_id_prefix" value="aruco_"/>
+    </node>
 
-        <param name="use_mocap" value="true"/>
+    <!-- vpe publisher from aruco markers -->
+    <node name="vpe_publisher" pkg="clever" type="vpe_publisher" if="$(arg aruco_vpe)" output="screen" clear_params="true">
+        <remap from="~pose_cov" to="aruco_map/pose"/>
+        <remap from="~vpe" to="mavros/vision_pose/pose"/>
+        <param name="frame_id" value="aruco_map_detected"/>
+        <param name="publish_zero" value="true"/>
+        <param name="offset_frame_id" value="aruco_map"/>
     </node>
 </launch>

clever/launch/clever.launch

@@ -7,28 +7,34 @@
     <arg name="main_camera" default="true"/>
     <arg name="optical_flow" default="false"/>
     <arg name="aruco" default="false"/>
-    <arg name="rc" default="true"/>
     <arg name="rangefinder_vl53l1x" default="false"/>
-    <arg name="arduino" default="false"/>
+    <arg name="led" default="false"/>
+    <arg name="rc" default="true"/>
+
+    <!-- log formatting -->
+    <env name="ROSCONSOLE_FORMAT" value="[${severity}] [${time}]: ${logger}: ${message}"/>
 
     <!-- mavros -->
     <include file="$(find clever)/launch/mavros.launch">
         <arg name="fcu_conn" value="$(arg fcu_conn)"/>
         <arg name="fcu_ip" value="$(arg fcu_ip)"/>
         <arg name="gcs_bridge" value="$(arg gcs_bridge)"/>
-        <arg name="viz" value="true"/>
     </include>
 
     <!-- web video server -->
-    <node name="web_video_server" pkg="web_video_server" type="web_video_server" if="$(arg web_video_server)" required="false" respawn="true" respawn_delay="5"/>
+    <node name="web_video_server" pkg="web_video_server" type="web_video_server" if="$(arg web_video_server)" required="false" respawn="true" respawn_delay="5">
+        <param name="default_stream_type" value="ros_compressed"/>
+        <param name="publish_rate" value="1.0"/>
+    </node>
 
-    <!-- aruco vpe -->
+    <!-- aruco markers -->
     <include file="$(find clever)/launch/aruco.launch" if="$(arg aruco)"/>
 
     <!-- optical flow -->
-    <node pkg="nodelet" type="nodelet" name="optical_flow" args="load clever/optical_flow nodelet_manager" if="$(arg optical_flow)" clear_params="true">
-        <remap from="image" to="main_camera/image_raw"/>
+    <node pkg="nodelet" type="nodelet" name="optical_flow" args="load clever/optical_flow nodelet_manager" if="$(arg optical_flow)" clear_params="true" output="screen">
+        <remap from="image_raw" to="main_camera/image_raw"/>
         <remap from="camera_info" to="main_camera/camera_info"/>
+        <param name="calc_flow_gyro" value="true"/>
     </node>
 
     <!-- main nodelet manager -->
@@ -36,19 +42,13 @@
         <param name="num_worker_threads" value="2"/>
     </node>
 
-    <node pkg="tf2_ros" type="static_transform_publisher" name="map_upside_down_frame" args="0 0 0 3.1415926 3.1415926 0 map map_upside_down"/>
+    <node pkg="tf2_ros" type="static_transform_publisher" name="map_flipped_frame" args="0 0 0 3.1415926 3.1415926 0 map map_flipped"/>
 
     <!-- simplified offboard control -->
     <node name="simple_offboard" pkg="clever" type="simple_offboard" output="screen" clear_params="true">
-        <rosparam param="reference_frames">
-            body: map
-            base_link: map
-        </rosparam>
-    </node>
-
-    <!-- Auxiliary frames -->
-    <node name="frames" pkg="clever" type="frames" output="screen">
-        <param name="body/frame_id" value="body"/>
+        <param name="reference_frames/body" value="map"/>
+        <param name="reference_frames/base_link" value="map"/>
+        <param name="reference_frames/navigate_target" value="map"/>
     </node>
 
     <!-- main camera -->
@@ -57,16 +57,17 @@
     <!-- rosbridge -->
     <include file="$(find rosbridge_server)/launch/rosbridge_websocket.launch" if="$(eval rosbridge or rc)"/>
 
+    <!-- tf2 republisher for web visualization -->
+    <node name="tf2_web_republisher" pkg="tf2_web_republisher" type="tf2_web_republisher" output="screen" if="$(arg rosbridge)"/>
+
     <!-- vl53l1x ToF rangefinder -->
-    <node name="vl53l1x" pkg="vl53l1x" type="vl53l1x_node" output="screen" if="$(arg rangefinder_vl53l1x)">
+    <node name="rangefinder" pkg="vl53l1x" type="vl53l1x_node" output="screen" if="$(arg rangefinder_vl53l1x)">
         <param name="frame_id" value="rangefinder"/>
-        <param name="offset" value="-0.05"/>
-        <remap from="~range" to="mavros/distance_sensor/rangefinder_3_sub"/> <!-- redirect data to FCU -->
     </node>
 
+    <!-- led strip -->
+    <include file="$(find clever)/launch/led.launch" if="$(arg led)"/>
+
     <!-- rc backend -->
     <node name="rc" pkg="clever" type="rc" output="screen" if="$(arg rc)"/>
-
-    <!-- Arduino bridge -->
-    <include file="$(find clever)/launch/arduino.launch" if="$(arg arduino)"/>
 </launch>

clever/launch/led.launch

@@ -0,0 +1,38 @@
+<launch>
+    <arg name="ws281x" default="true"/>
+    <arg name="led_effect" default="true"/>
+    <arg name="led_notify" default="true"/>
+
+    <!-- For additional help go to https://clever.copterexpress.com/led.html -->
+
+    <!-- ws281x led strip driver -->
+    <node pkg="ws281x" name="led" type="ws281x_node" clear_params="true" output="screen" if="$(arg ws281x)">
+        <param name="led_count" value="58"/>
+        <param name="gpio_pin" value="21"/>
+        <param name="brightness" value="100"/>
+        <param name="strip_type" value="WS2811_STRIP_GRB"/>
+        <param name="target_frequency" value="800000"/>
+        <param name="dma" value="10"/>
+        <param name="invert" value="false"/>
+    </node>
+
+    <!-- high level led effects control, events notification with leds -->
+    <node pkg="clever" name="led_effect" type="led" ns="led" clear_params="true" output="screen" if="$(arg led_effect)">
+        <param name="blink_rate" value="2"/>
+        <param name="fade_period" value="0.5"/>
+        <param name="rainbow_period" value="5"/>
+        <!-- events effects table -->
+        <rosparam param="notify" if="$(arg led_notify)">
+            startup: { r: 255, g: 255, b: 255 }
+            connected: { effect: rainbow }
+            disconnected: { effect: blink, r: 255, g: 50, b: 50 }
+            acro: { r: 245, g: 155, b: 0 }
+            stabilized: { r: 30, g: 180, b: 50 }
+            altctl: { r: 255, g: 255, b: 40 }
+            posctl: { r: 50, g: 100, b: 220 }
+            offboard: { r: 220, g: 20, b: 250 }
+            low_battery: { threshold: 3.7, effect: blink_fast, r: 255, g: 0, b: 0 }
+            error: { effect: flash, r: 255, g: 0, b: 0 }
+        </rosparam>
+    </node>
+</launch>

clever/launch/main_camera.launch

@@ -21,11 +21,12 @@
         <param name="frame_id" value="main_camera_optical"/>
         <param name="camera_info_url" value="file://$(find clever)/camera_info/fisheye_cam_320.yaml"/>
 
-        <!-- setting camera FPS -->
-        <param name="rate" value="100"/>
-        <param name="cv_cap_prop_fps" value="40"/>
-        <param name="capture_delay" value="0.02"/>
+        <param name="rate" value="100"/> <!-- poll rate -->
+        <param name="cv_cap_prop_fps" value="40"/> <!-- camera FPS -->
+        <param name="capture_delay" value="0.02"/> <!-- approximate delay on frame retrieving -->
+        <param name="rescale_camera_info" value="true"/> <!-- automatically rescale camera calibration info -->
 
+        <!-- camera resolution, NOTE: camera_info file should match it -->
         <param name="image_width" value="320"/>
         <param name="image_height" value="240"/>
     </node>

clever/launch/mavros.launch

@@ -4,13 +4,14 @@
     <arg name="gcs_bridge" default="tcp"/>
     <arg name="viz" default="true"/>
     <arg name="respawn" default="true"/>
+    <arg name="distance_sensor_remap" default="rangefinder/range"/>
 
-    <node pkg="mavros" type="mavros_node" name="mavros" required="false" clear_params="true" respawn="$(arg respawn)" respawn_delay="2" output="screen">
+    <node pkg="mavros" type="mavros_node" name="mavros" required="false" clear_params="true" respawn="$(arg respawn)" unless="$(eval fcu_conn == 'none')" respawn_delay="1" output="screen">
         <!-- UART connection -->
         <param name="fcu_url" value="/dev/ttyAMA0:921600" if="$(eval fcu_conn is None or fcu_conn == 'uart')"/>
 
         <!-- USB connection -->
-        <param name="fcu_url" value="/dev/ttyACM0" if="$(eval fcu_conn == 'usb')"/>
+        <param name="fcu_url" value="/dev/px4fmu" if="$(eval fcu_conn == 'usb')"/>
 
         <!-- sitl -->
         <param name="fcu_url" value="udp://@$(arg fcu_ip):14557" if="$(eval fcu_conn == 'udp')"/>
@@ -18,76 +19,56 @@
         <!-- gcs bridge -->
         <param name="gcs_url" value="tcp-l://0.0.0.0:5760" if="$(eval gcs_bridge == 'tcp')"/>
         <param name="gcs_url" value="udp://0.0.0.0:14550@14550" if="$(eval gcs_bridge == 'udp')"/>
-        <param name="gcs_url" value="udp-b://$(env ROS_IP):14550@14550" if="$(eval gcs_bridge == 'udp-b')"/>
-        <param name="gcs_url" value="udp-pb://$(env ROS_IP):14550@14550" if="$(eval gcs_bridge == 'udp-pb')"/>
+        <param name="gcs_url" value="udp-b://$(env ROS_HOSTNAME):14550@14550" if="$(eval gcs_bridge == 'udp-b')"/>
+        <param name="gcs_url" value="udp-pb://$(env ROS_HOSTNAME):14550@14550" if="$(eval gcs_bridge == 'udp-pb')"/>
         <param name="gcs_url" value="" if="$(eval not gcs_bridge)"/>
         <param name="gcs_quiet_mode" value="true"/>
         <param name="conn/timeout" value="8"/>
 
-        <!-- default px4 params -->
-        <rosparam command="load" file="$(find mavros)/launch/px4_config.yaml"/>
+        <!-- basic params -->
+        <rosparam command="load" file="$(find clever)/launch/mavros_config.yaml"/>
 
-        <!-- enable setpoint_attitude/attitude -->
-        <param name="setpoint_attitude/use_quaternion" value="true"/>
+        <!-- remap rangefinder -->
+        <remap from="mavros/distance_sensor/rangefinder_sub" to="rangefinder/range"/>
 
-        <!-- rangefinders -->
-        <rosparam>
-            distance_sensor:
-                rangefinder_0:
-                    id: 0
-                    frame_id: "rangefinder"
-                    orientation: PITCH_270
-                    field_of_view: 0.5
-                rangefinder_1:
-                    id: 1
-                    frame_id: "rangefinder"
-                    orientation: PITCH_270
-                    field_of_view: 0.5
-                rangefinder_2_sub:
-                    subscriber: true
-                    id: 2
-                    orientation: PITCH_270
-                rangefinder_3_sub:
-                    subscriber: true
-                    id: 3
-                    orientation: PITCH_270
-        </rosparam>
-
-        <!-- additional params -->
-        <param name="local_position/frame_id" value="map"/>
-        <param name="local_position/tf/send" value="true"/>
-        <param name="local_position/tf/frame_id" value="map"/>
-        <param name="local_position/tf/child_frame_id" value="base_link"/>
-        <param name="global_position/tf/send" value="false"/>
-        <param name="imu/frame_id" value="base_link"/>
-        <rosparam param="plugin_blacklist">
-            - safety_area
-            - image_pub
-            - vibration
-            - rangefinder
-            - 3dr_radio
-            - actuator_control
-            - hil_controls
-            - vfr_hud
-            - vision_speed_estimate
-            - fake_gps
-            - cam_imu_sync
-            - hil
-            - adsb
-            - waypoint
-            - obstacle_distance
-            - setpoint_accel
-            - trajectory
-            - wind_estimation
-            - home_position
+        <rosparam param="plugin_whitelist">
+            - altitude
+            - command
+            - distance_sensor
+            - ftp
+            - global_position
+            - imu
+            - local_position
+            - manual_control
+            # - mocap_pose_estimate
+            - param
+            - px4flow
+            - rc_io
+            - setpoint_attitude
+            - setpoint_position
+            - setpoint_raw
+            - setpoint_velocity
+            - sys_status
+            - sys_time
+            - vision_pose_estimate
+            # - vision_speed_estimate
+            # - waypoint
         </rosparam>
     </node>
 
+    <!-- remapped distance_sensor config -->
+    <rosparam param="$(arg distance_sensor_remap)" if="$(eval bool(distance_sensor_remap))">
+        subscriber: true
+        id: 1
+        orientation: PITCH_270
+        covariance: 1  # cm
+    </rosparam>
+
     <!-- Rangefinders frame -->
     <node pkg="tf2_ros" type="static_transform_publisher" name="rangefinder_frame" args="0 0 -0.05 0 1.5707963268 0 base_link rangefinder"/>
 
     <!-- Copter visualization -->
-    <node name="copter_visualization" pkg="mavros_extras" type="copter_visualization" if="$(arg viz)">
+    <node name="visualization" pkg="mavros_extras" type="visualization" if="$(arg viz)">
         <remap to="mavros/local_position/pose" from="local_position"/>
         <remap to="mavros/setpoint_position/local" from="local_setpoint"/>
         <param name="fixed_frame_id" value="map"/>

clever/launch/mavros_config.yaml

@@ -0,0 +1,139 @@
+# Config file for mavros
+# Based on https://raw.githubusercontent.com/mavlink/mavros/master/mavros/launch/px4_config.yaml
+
+startup_px4_usb_quirk: true
+
+conn:
+  heartbeat_rate: 1.0    # send hertbeat rate in Hertz
+  timeout: 10.0          # hertbeat timeout in seconds
+  timesync_rate: 10.0    # TIMESYNC rate in Hertz (feature disabled if 0.0)
+  system_time_rate: 1.0  # send system time to FCU rate in Hertz (disabled if 0.0)
+
+time:
+  time_ref_source: "fcu"  # time_reference source
+  timesync_mode: MAVLINK
+  timesync_avg_alpha: 0.6 # timesync averaging factor
+
+global_position:
+  frame_id: "map"             # origin frame
+  child_frame_id: "base_link" # body-fixed frame
+  rot_covariance: 99999.0   # covariance for attitude?
+  gps_uere: 1.0             # User Equivalent Range Error (UERE) of GPS sensor (m)
+  use_relative_alt: true    # use relative altitude for local coordinates
+  tf:
+    send: false               # send TF?
+    frame_id: "map"  # TF frame_id
+    global_frame_id: "earth"  # TF earth frame_id
+    child_frame_id: "base_link" # TF child_frame_id
+
+imu:
+  frame_id: "base_link"
+  # need find actual values
+  linear_acceleration_stdev: 0.0003
+  angular_velocity_stdev: !degrees 0.02
+  orientation_stdev: 1.0
+  magnetic_stdev: 0.0
+
+local_position:
+  frame_id: "map"
+  tf:
+    send: true
+    send_fcu: false
+
+# setpoint_attitude
+setpoint_attitude:
+  reverse_thrust: false     # allow reversed thrust
+  use_quaternion: true     # enable PoseStamped topic subscriber
+  tf:
+    listen: false           # enable tf listener (disable topic subscribers)
+    frame_id: "map"
+    child_frame_id: "target_attitude"
+    rate_limit: 50.0
+
+setpoint_raw:
+  thrust_scaling: 1.0       # used in setpoint_raw attitude callback.
+  # Note: PX4 expects normalized thrust values between 0 and 1, which means that
+  # the scaling needs to be unitary and the inputs should be 0..1 as well.
+
+setpoint_position:
+  tf:
+    listen: false           # enable tf listener (disable topic subscribers)
+    frame_id: "map"
+    child_frame_id: "target_position"
+    rate_limit: 50.0
+  mav_frame: LOCAL_NED
+
+setpoint_velocity:
+  mav_frame: LOCAL_NED
+
+mission:
+  pull_after_gcs: true  # update mission if gcs updates
+
+distance_sensor:
+  rangefinder:
+      id: 0
+      frame_id: "rangefinder"
+      orientation: PITCH_270
+      field_of_view: 0.5
+  rangefinder_sub:
+      subscriber: true
+
+# fake_gps
+fake_gps:
+  # select data source
+  use_mocap: true         # ~mocap/pose
+  mocap_transform: true   # ~mocap/tf instead of pose
+  use_vision: false       # ~vision (pose)
+  # origin (default: Zürich)
+  geo_origin:
+    lat: 47.3667          # latitude [degrees]
+    lon: 8.5500           # longitude [degrees]
+    alt: 408.0            # altitude (height over the WGS-84 ellipsoid) [meters]
+  eph: 2.0
+  epv: 2.0
+  satellites_visible: 5   # virtual number of visible satellites
+  fix_type: 3             # type of GPS fix (default: 3D)
+  tf:
+    listen: false
+    send: false           # send TF?
+    frame_id: "map"       # TF frame_id
+    child_frame_id: "fix" # TF child_frame_id
+    rate_limit: 10.0      # TF rate
+  gps_rate: 5.0           # GPS data publishing rate
+
+mocap:
+  # select mocap source
+  use_tf: false   # ~mocap/tf
+  use_pose: true  # ~mocap/pose
+
+odometry:
+  in:
+    frame_id: "odom"
+    child_frame_id: "base_link"
+    frame_tf:
+      local_frame: "local_origin_ned"
+      body_frame_orientation: "flu"
+  out:
+    frame_tf:
+      # available: check MAV_FRAME odometry local frames in
+      # https://mavlink.io/en/messages/common.html
+      local_frame: "vision_ned"
+      # available: ned, frd or flu (though only the tf to frd is supported)
+      body_frame_orientation: "frd"
+
+px4flow:
+  frame_id: "px4flow"
+  ranger_fov: !degrees 6.8  # 6.8 degreens at 5 meters, 31 degrees at 1 meter
+  ranger_min_range: 0.3     # meters
+  ranger_max_range: 5.0     # meters
+
+vision_pose:
+  tf:
+    listen: false           # enable tf listener (disable topic subscribers)
+
+vision_speed:
+  listen_twist: true    # enable listen to twist topic, else listen to vec3d topic
+  twist_cov: true       # enable listen to twist with covariance topic
+
+vibration:
+  frame_id: "base_link"

clever/nodelet_plugins.xml

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

clever/package.xml

@@ -1,5 +1,4 @@
 <?xml version="1.0"?>
-<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="2">
   <name>clever</name>
   <version>0.0.1</version>
@@ -8,7 +7,7 @@
   <maintainer email="okalachev@gmail.com">Oleg Kalachev</maintainer>
   <license>MIT</license>
 
-  <!--url type="website">http://wiki.ros.org/clever</url-->
+  <url type="website">https://clever.copterexpress.com/</url>
   <author email="okalachev@gmail.com">Oleg Kalachev</author>
   <author email="urpylka@gmail.com">Artem Smirnov</author>
 
@@ -16,19 +15,30 @@
 
   <!-- Package format specifier version 2.0 allows specifying dependencies for both
    build- and runtime in a single <depend> element -->
-  <depend>visualization_msgs</depend>
-  <depend>tf2_geometry_msgs</depend>
-  <depend>geographiclib</depend>
   <depend>roscpp</depend>
+  <depend>rospy</depend>
+  <depend>std_srvs</depend>
+  <depend>tf</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>visualization_msgs</depend>
+  <depend>led_msgs</depend>
+  <depend>geographiclib</depend>
   <depend>nodelet</depend>
   <depend>mavros</depend>
   <depend>mavros_extras</depend>
-  <depend>lxml</depend>
   <depend>cv_camera</depend>
-  <depend>mjpg-streamer</depend>
+  <depend>cv_bridge</depend>
+  <depend>opencv3</depend>
   <depend>rosbridge_server</depend>
   <depend>web_video_server</depend>
-  <depend>ros_comm</depend>
+  <depend>tf2_web_republisher</depend>
+  <depend>python-lxml</depend>
+  <exec_depend>python-pymavlink</exec_depend>
   <!-- Use test_depend for packages you need only for testing: -->
   <!--   <test_depend>gtest</test_depend> -->
 

clever/requirements.txt

@@ -1,5 +1,5 @@
-flask==0.12.3
+flask==1.1.1
+docopt==0.6.2
 geopy==1.11.0
-pymavlink==2.2.10
 smbus2==0.2.1
 VL53L1X==0.0.2

clever/src/aruco_vpe.cpp

@@ -1,137 +0,0 @@
-#include <nodelet/nodelet.h>
-#include <pluginlib/class_list_macros.h>
-#include <tf/transform_datatypes.h>
-#include <tf2/exceptions.h>
-#include <tf2/convert.h>
-#include <tf2_ros/transform_listener.h>
-#include <tf2_ros/transform_broadcaster.h>
-#include <tf2_ros/static_transform_broadcaster.h>
-#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
-#include <geometry_msgs/TransformStamped.h>
-#include <geometry_msgs/PoseStamped.h>
-
-#include "util.h"
-
-using namespace tf2_ros;
-using geometry_msgs::PoseStamped;
-using geometry_msgs::TransformStamped;
-using std::string;
-
-class ArucoVPE : public nodelet::Nodelet
-{
-public:
-    ArucoVPE() :
-        last_published_(0),
-        lookup_timeout_(0.05)
-    {}
-
-private:
-    ros::Time last_published_;
-    ros::Duration lookup_timeout_;
-    ros::Duration reset_timeout_;
-    ros::Publisher vision_position_pub_;
-    ros::Timer dummy_vision_timer_;
-    string aruco_orientation_;
-    bool reset_vpe_;
-
-    void onInit()
-    {
-        ros::NodeHandle& nh = getNodeHandle();
-        ros::NodeHandle& nh_priv = getPrivateNodeHandle();
-
-        nh_priv.param<string>("aruco_orientation", aruco_orientation_, "map");
-        bool use_mocap;
-        nh_priv.param<bool>("use_mocap", use_mocap, false);
-        nh_priv.param<bool>("reset_vpe", reset_vpe_, !use_mocap);
-        double reset_timeout;
-        nh_priv.param("reset_timeout", reset_timeout, 2.0);
-        reset_timeout_ = ros::Duration(reset_timeout);
-
-        static ros::Subscriber pose_sub = nh.subscribe("mavros/local_position/pose", 1, &ArucoVPE::handlePose, this);
-        static ros::Subscriber aruco_pose_sub = nh.subscribe("aruco_pose/pose", 1, &ArucoVPE::handleArucoPose, this);
-
-        vision_position_pub_ = nh.advertise<PoseStamped>(use_mocap ? "mavros/mocap/pose" : "mavros/vision_pose/pose", 1);
-
-        ROS_INFO("aruco orientation frame: %s", aruco_orientation_.c_str());
-
-        dummy_vision_timer_ = nh.createTimer(ros::Duration(0.5), &ArucoVPE::publishDummy, this);
-
-        ROS_INFO("Aruco VPE initialized");
-    }
-
-    void publishDummy(const ros::TimerEvent&)
-    {
-        // This is published to init FCU's position estimator
-        static PoseStamped ps;
-        ps.header.stamp = ros::Time::now();
-        ps.pose.orientation.w = 1;
-        vision_position_pub_.publish(ps);
-    }
-
-    void  handlePose(const geometry_msgs::PoseStampedConstPtr& pose)
-    {
-        // local position is inited, stop posting dummy position
-        ROS_INFO_ONCE("Got local position, stop publishing zeroes");
-        dummy_vision_timer_.stop();
-    }
-
-    void handleArucoPose(const geometry_msgs::PoseStampedConstPtr& pose)
-    {
-        static TransformBroadcaster br;
-        static Buffer tf_buffer;
-        static TransformListener tfListener(tf_buffer);
-        static StaticTransformBroadcaster static_br;
-        static PoseStamped ps, vpe_raw, vpe;
-        TransformStamped t;
-
-        ros::Time stamp = pose->header.stamp;
-        double roll, pitch, yaw;
-
-        try
-        {
-            // Refine aruco map pose
-            // Reference in local origin
-            t = tf_buffer.lookupTransform(aruco_orientation_, "aruco_map_reference", stamp, lookup_timeout_);
-            quaternionToEuler(t.transform.rotation, roll, pitch, yaw);
-            eulerToQuaternion(t.transform.rotation, 0, 0, yaw);
-            t.child_frame_id = "aruco_map_reference_horiz";
-            br.sendTransform(t);
-
-            // Aruco map in reference
-            t = tf_buffer.lookupTransform("aruco_map_reference", "aruco_map_raw", stamp, lookup_timeout_);
-            t.header.frame_id = "aruco_map_reference_horiz";
-            t.child_frame_id = "aruco_map_vision";
-            br.sendTransform(t);
-
-            if (last_published_.toSec() == 0 || // no vpe has been posted
-               (reset_vpe_ && (ros::Time::now() - last_published_ > reset_timeout_))) // vpe origin outdated
-            {
-                ROS_INFO("Reset VPE");
-                t = tf_buffer.lookupTransform("map", "aruco_map_vision", stamp, lookup_timeout_);
-                t.child_frame_id = "aruco_map";
-                static_br.sendTransform(t);
-            }
-
-            // Calculate VPE
-            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, "map", lookup_timeout_);
-
-            vision_position_pub_.publish(vpe);
-
-            last_published_ = stamp;
-            dummy_vision_timer_.stop();
-        }
-        catch (const tf2::TransformException& e)
-        {
-            ROS_WARN_THROTTLE(10, "Aruco VPE: failed to transform: %s", e.what());
-        }
-    }
-};
-
-PLUGINLIB_EXPORT_CLASS(ArucoVPE, nodelet::Nodelet)

clever/src/frames.cpp

@@ -1,63 +0,0 @@
-/*
- * Auxiliary TF frames for CLEVER drone kit:
- * - Body frame (drone body with zero pitch and roll).
- * - TODO: REP-0105 `odom` frame emulation: continuous frame without discrete jumps.
- * - TODO: Terrain frame (base on ALTITUDE message).
- * - TODO: map_upside_down frame
- * - TODO: home frame?
- *
- * Copyright (C) 2018 Copter Express Technologies
- *
- * Author: Oleg Kalachev <okalachev@gmail.com>
- *
- * Distributed under MIT License (available at https://opensource.org/licenses/MIT).
- * The above copyright notice and this permission notice shall be included in all
- * copies or substantial portions of the Software.
- */
-
-// TODO: consider implementing as a mavros plugin
-
-#include <string>
-#include <memory>
-#include <ros/ros.h>
-#include <tf/transform_datatypes.h>
-#include <tf2_ros/transform_broadcaster.h>
-#include <geometry_msgs/TransformStamped.h>
-#include <geometry_msgs/PoseStamped.h>
-
-using std::string;
-
-static std::shared_ptr<tf2_ros::TransformBroadcaster> br;
-static geometry_msgs::TransformStamped body;
-
-inline void publishBody(const geometry_msgs::PoseStamped& pose)
-{
-	// Get only yaw from pose
-	tf::Quaternion q;
-	q.setRPY(0, 0, tf::getYaw(pose.pose.orientation));
-	tf::quaternionTFToMsg(q, body.transform.rotation);
-
-	body.transform.translation.x = pose.pose.position.x;
-	body.transform.translation.y = pose.pose.position.y;
-	body.transform.translation.z = pose.pose.position.z;
-	body.header.frame_id = pose.header.frame_id;
-	body.header.stamp = pose.header.stamp;
-	br->sendTransform(body);
-}
-
-void poseCallback(const geometry_msgs::PoseStamped& pose)
-{
-	publishBody(pose);
-}
-
-int main(int argc, char **argv) {
-	ros::init(argc, argv, "frames");
-	ros::NodeHandle nh, nh_priv("~");
-
-	nh_priv.param<string>("body/frame_id", body.child_frame_id, "body");
-
-	br = std::make_shared<tf2_ros::TransformBroadcaster>();
-	ros::Subscriber pose_sub = nh.subscribe("mavros/local_position/pose", 1, &poseCallback);
-	ROS_INFO("frames: ready");
-	ros::spin();
-}

clever/src/interactive.py

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

clever/src/led.cpp

@@ -0,0 +1,321 @@
+/*
+ * High level control for the LED strip
+ * Indicate flight events with the LED strip
+ * Copyright (C) 2019 Copter Express Technologies
+ *
+ * Author: Oleg Kalachev <okalachev@gmail.com>
+ *
+ * Distributed under MIT License (available at https://opensource.org/licenses/MIT).
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ */
+
+#include <ros/ros.h>
+#include <string>
+#include <boost/algorithm/string.hpp>
+
+#include <clever/SetLEDEffect.h>
+#include <led_msgs/SetLEDs.h>
+#include <led_msgs/LEDState.h>
+#include <led_msgs/LEDStateArray.h>
+
+#include <sensor_msgs/BatteryState.h>
+#include <mavros_msgs/State.h>
+#include <rosgraph_msgs/Log.h>
+
+clever::SetLEDEffect::Request current_effect;
+int led_count;
+ros::Timer timer;
+ros::Time start_time;
+double blink_rate, blink_fast_rate, flash_delay, fade_period, wipe_period, rainbow_period;
+double low_battery_threshold;
+bool blink_state;
+led_msgs::SetLEDs set_leds;
+led_msgs::LEDStateArray state, start_state;
+ros::ServiceClient set_leds_srv;
+mavros_msgs::State mavros_state;
+int counter;
+
+void callSetLeds()
+{
+	bool res = set_leds_srv.call(set_leds);
+	if (!res) {
+		ROS_WARN_THROTTLE(5, "Error calling set_leds service");
+	} else if (!set_leds.response.success) {
+		ROS_WARN_THROTTLE(5, "Calling set_leds failed: %s", set_leds.response.message.c_str());
+	}
+}
+
+void rainbow(uint8_t n, uint8_t& r, uint8_t& g, uint8_t& b)
+{
+	if (n < 255 / 3) {
+		r = n * 3;
+		g = 255 - n * 3;
+		b = 0;
+	} else if (n < 255 / 3 * 2) {
+		n -= 255 / 3;
+		r = 255 - n * 3;
+		g = 0;
+		b = n * 3;
+	} else {
+		n -= 255 / 3 * 2;
+		r = 0;
+		g = n * 3;
+		b = 255 - n * 3;
+	}
+}
+
+void fill(uint8_t r, uint8_t g, uint8_t b)
+{
+	set_leds.request.leds.resize(led_count);
+	for (int i = 0; i < led_count; i++) {
+		set_leds.request.leds[i].index = i;
+		set_leds.request.leds[i].r = r;
+		set_leds.request.leds[i].g = g;
+		set_leds.request.leds[i].b = b;
+	}
+	callSetLeds();
+}
+
+void proceed(const ros::TimerEvent& event)
+{
+	counter++;
+	uint8_t r, g, b;
+	set_leds.request.leds.clear();
+	set_leds.request.leds.resize(led_count);
+
+	if (current_effect.effect == "blink" || current_effect.effect == "blink_fast") {
+		blink_state = !blink_state;
+		// toggle all leds
+		if (blink_state) {
+			fill(current_effect.r, current_effect.g, current_effect.b);
+		} else {
+			fill(0, 0, 0);
+		}
+
+	} else if (current_effect.effect == "fade") {
+		// fade all leds from starting state
+		double passed = std::min((event.current_real - start_time).toSec() / fade_period, 1.0);
+		double one_minus_passed = 1 - passed;
+		for (int i = 0; i < led_count; i++) {
+			set_leds.request.leds[i].index = i;
+			set_leds.request.leds[i].r = one_minus_passed * start_state.leds[i].r + passed * current_effect.r;
+			set_leds.request.leds[i].g = one_minus_passed * start_state.leds[i].g + passed * current_effect.g;
+			set_leds.request.leds[i].b = one_minus_passed * start_state.leds[i].b + passed * current_effect.b;
+		}
+		callSetLeds();
+		if (passed >= 1.0) {
+			// fade finished
+			timer.stop();
+		}
+
+	} else if (current_effect.effect == "wipe") {
+		set_leds.request.leds.resize(1);
+		set_leds.request.leds[0].index = counter - 1;
+		set_leds.request.leds[0].r = current_effect.r;
+		set_leds.request.leds[0].g = current_effect.g;
+		set_leds.request.leds[0].b = current_effect.b;
+		callSetLeds();
+		if (counter == led_count) {
+			// wipe finished
+			timer.stop();
+		}
+
+	} else if (current_effect.effect == "rainbow_fill") {
+		rainbow(counter % 255, r, g, b);
+		for (int i = 0; i < led_count; i++) {
+			set_leds.request.leds[i].index = i;
+			set_leds.request.leds[i].r = r;
+			set_leds.request.leds[i].g = g;
+			set_leds.request.leds[i].b = b;
+		}
+		callSetLeds();
+
+	} else if (current_effect.effect == "rainbow") {
+		for (int i = 0; i < led_count; i++) {
+			int pos = (int)round(counter + (255.0 * i / led_count)) % 255;
+			rainbow(pos % 255, r, g, b);
+			set_leds.request.leds[i].index = i;
+			set_leds.request.leds[i].r = r;
+			set_leds.request.leds[i].g = g;
+			set_leds.request.leds[i].b = b;
+		}
+		callSetLeds();
+	}
+}
+
+bool setEffect(clever::SetLEDEffect::Request& req, clever::SetLEDEffect::Response& res)
+{
+	res.success = true;
+
+	if (req.effect == "") {
+		req.effect = "fill";
+	}
+
+	if (req.effect != "flash" && req.effect != "fill" && current_effect.effect == req.effect &&
+	    current_effect.r == req.r && current_effect.g == req.g && current_effect.b == req.b) {
+		res.message = "Effect already set, skip";
+		return true;
+	}
+
+	if (req.effect == "fill") {
+		fill(req.r, req.g, req.b);
+
+	} else if (req.effect == "blink") {
+		timer.setPeriod(ros::Duration(1 / blink_rate), true);
+		timer.start();
+
+	} else if (req.effect == "blink_fast") {
+		timer.setPeriod(ros::Duration(1 / blink_fast_rate), true);
+		timer.start();
+
+	} else if (req.effect == "fade") {
+		timer.setPeriod(ros::Duration(0.05), true);
+		timer.start();
+
+	} else if (req.effect == "wipe") {
+		timer.setPeriod(ros::Duration(wipe_period / led_count), true);
+		timer.start();
+
+	} else if (req.effect == "flash") {
+		ros::Duration delay(flash_delay);
+		fill(0, 0, 0);
+		delay.sleep();
+		fill(req.r, req.g, req.b);
+		delay.sleep();
+		fill(0, 0, 0);
+		delay.sleep();
+		fill(req.r, req.g, req.b);
+		delay.sleep();
+		fill(0, 0, 0);
+		delay.sleep();
+		if (current_effect.effect == "fill"||
+		    current_effect.effect == "fade" ||
+		    current_effect.effect == "wipe") {
+			// restore previous filling
+			for (int i = 0; i < led_count; i++) {
+				fill(current_effect.r, current_effect.g, current_effect.b);
+			}
+			callSetLeds();
+		}
+		return true; // this effect happens only once
+
+	} else if (req.effect == "rainbow_fill") {
+		timer.setPeriod(ros::Duration(rainbow_period / 255), true);
+		timer.start();
+
+	} else if (req.effect == "rainbow") {
+		timer.setPeriod(ros::Duration(rainbow_period / 255), true);
+		timer.start();
+
+	} else {
+		res.message = "Unknown effect: " + req.effect + ". Available effects are fill, fade, wipe, blink, blink_fast, flash, rainbow, rainbow_fill.";
+		ROS_ERROR("%s", res.message.c_str());
+		res.success = false;
+		return true;
+	}
+
+	// set current effect
+	current_effect = req;
+	counter = 0;
+	start_state = state;
+	start_time = ros::Time::now();
+
+	return true;
+}
+
+void handleState(const led_msgs::LEDStateArray& msg)
+{
+	state = msg;
+	led_count = state.leds.size();
+}
+
+bool notify(const std::string& event)
+{
+	if (ros::param::has("~notify/" + event + "/effect") ||
+	    ros::param::has("~notify/" + event + "/r") ||
+	    ros::param::has("~notify/" + event + "/g") ||
+	    ros::param::has("~notify/" + event + "/b")) {
+		ROS_INFO_THROTTLE(5, "led: notify %s", event.c_str());
+		clever::SetLEDEffect effect;
+		effect.request.effect = ros::param::param("~notify/" + event + "/effect", std::string(""));
+		effect.request.r = ros::param::param("~notify/" + event + "/r", 0);
+		effect.request.g = ros::param::param("~notify/" + event + "/g", 0);
+		effect.request.b = ros::param::param("~notify/" + event + "/b", 0);
+		setEffect(effect.request, effect.response);
+	}
+}
+
+void handleMavrosState(const mavros_msgs::State& msg)
+{
+	if (msg.connected && !mavros_state.connected) {
+		notify("connected");
+	} else if (!msg.connected && mavros_state.connected) {
+		notify("disconnected");
+	} else if (msg.armed && !mavros_state.armed) {
+		notify("armed");
+	} else if (!msg.armed && mavros_state.armed) {
+		notify("disarmed");
+	} else if (msg.mode != mavros_state.mode) {
+		// mode changed
+		std::string mode = boost::algorithm::to_lower_copy(msg.mode);
+		if (mode.find(".") != std::string::npos) {
+			// remove the part before "."
+			mode = mode.substr(mode.find(".") + 1);
+		}
+		notify(mode);
+	}
+	mavros_state = msg;
+}
+
+void handleLog(const rosgraph_msgs::Log& log)
+{
+	if (log.level >= rosgraph_msgs::Log::ERROR) {
+		notify("error");
+	}
+}
+
+void handleBattery(const sensor_msgs::BatteryState& msg)
+{
+	for (auto const& voltage : msg.cell_voltage) {
+		if (voltage < low_battery_threshold) {
+			// notify low battery every time
+			notify("low_battery");
+		}
+	}
+}
+
+int main(int argc, char **argv)
+{
+	ros::init(argc, argv, "led");
+	ros::NodeHandle nh, nh_priv("~");
+
+	nh_priv.param("blink_rate", blink_rate, 2.0);
+	nh_priv.param("blink_fast_rate", blink_fast_rate, blink_rate * 2);
+	nh_priv.param("fade_period", fade_period, 0.5);
+	nh_priv.param("wipe_period", wipe_period, 0.5);
+	nh_priv.param("flash_delay", flash_delay, 0.1);
+	nh_priv.param("rainbow_period", rainbow_period, 5.0);
+
+	nh_priv.param("notify/low_battery/threshold", low_battery_threshold, 3.7);
+
+	ros::service::waitForService("set_leds"); // cannot work without set_leds service
+	set_leds_srv = nh.serviceClient<led_msgs::SetLEDs>("set_leds", true);
+
+	// wait for leds count info
+	handleState(*ros::topic::waitForMessage<led_msgs::LEDStateArray>("state", nh));
+
+	auto state_sub = nh.subscribe("state", 1, &handleState);
+
+	auto set_effect = nh.advertiseService("set_effect", &setEffect);
+
+	auto mavros_state_sub = nh.subscribe("/mavros/state", 1, &handleMavrosState);
+	auto battery_sub = nh.subscribe("/mavros/battery", 1, &handleBattery);
+	auto rosout_sub = nh.subscribe("/rosout_agg", 1, &handleLog);
+
+	timer = nh.createTimer(ros::Duration(0), &proceed, false, false);
+
+	ROS_INFO("ready");
+	notify("startup");
+	ros::spin();
+}

clever/src/optical_flow.cpp

@@ -67,7 +67,7 @@ private:
 		roi_2_ = roi_ / 2;
 		nh_priv.param("calc_flow_gyro", calc_flow_gyro_, false);
 
-		img_sub_ = it.subscribeCamera("image", 1, &OpticalFlow::flow, this);
+		img_sub_ = it.subscribeCamera("image_raw", 1, &OpticalFlow::flow, this);
 		img_pub_ = it_priv.advertise("debug", 1);
 		flow_pub_ = nh.advertise<mavros_msgs::OpticalFlowRad>("mavros/px4flow/raw/send", 1);
 		velo_pub_ = nh_priv.advertise<geometry_msgs::TwistStamped>("angular_velocity", 1);
@@ -80,7 +80,7 @@ private:
 		flow_.distance = -1; // no distance sensor available
 		flow_.temperature = 0;
 
-		ROS_INFO("Optical Flow initialized");
+		NODELET_INFO("Optical Flow initialized");
 	}
 
 	void parseCameraInfo(const sensor_msgs::CameraInfoConstPtr &cinfo) {
@@ -117,12 +117,12 @@ private:
 			img = img(cv::Rect((msg->width / 2 - roi_2_), (msg->height / 2 - roi_2_), roi_, roi_));
 		}
 
-		img.convertTo(curr_, CV_64F);
+		img.convertTo(curr_, CV_32F);
 
 		if (prev_.empty()) {
 			prev_ = curr_.clone();
 			prev_stamp_ = msg->header.stamp;
-			cv::createHanningWindow(hann_, curr_.size(), CV_64F);
+			cv::createHanningWindow(hann_, curr_.size(), CV_32F);
 
 		} else {
 			double response;
@@ -161,7 +161,12 @@ private:
 			flow_camera.header.stamp = msg->header.stamp;
 			flow_camera.vector.x = flow_y; // +y means counter-clockwise rotation around Y axis
 			flow_camera.vector.y = -flow_x; // +x means clockwise rotation around X axis
-			tf_buffer_.transform(flow_camera, flow_fcu, fcu_frame_id_);
+			try {
+				tf_buffer_.transform(flow_camera, flow_fcu, fcu_frame_id_);
+			} catch (const tf2::TransformException& e) {
+				// transform is not available yet
+				return;
+			}
 
 			// Calculate integration time
 			ros::Duration integration_time = msg->header.stamp - prev_stamp_;
@@ -176,6 +181,8 @@ private:
 					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;
 				}
 			}
@@ -217,15 +224,16 @@ private:
 	{
 		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);
+		tf2::fromMsg(tf_buffer_.lookupTransform(frame_id, local_frame_id_, curr, ros::Duration(0.1)).transform.rotation, curr_rot);
 
 		geometry_msgs::Vector3Stamped flow;
 		flow.header.frame_id = frame_id;
 		flow.header.stamp = curr;
+		// https://en.wikipedia.org/wiki/Rotation_formalisms_in_three_dimensions#Quaternion_↔_angular_velocities
 		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();
+		flow.vector.x = -diff.x();
+		flow.vector.y = -diff.y();
+		flow.vector.z = -diff.z();
 
 		return flow;
 	}

clever/src/rc.cpp

@@ -1,6 +1,16 @@
-// CLEVER mobile remote control support:
-// * Send ManualControl messages through UDP
-// * `latched_state` topic
+/*
+ * CLEVER mobile remote control backend
+ * Send ManualControl messages through UDP
+ * 'latched_state' topic
+ *
+ * Copyright (C) 2019 Copter Express Technologies
+ *
+ * Author: Oleg Kalachev <okalachev@gmail.com>
+ *
+ * Distributed under MIT License (available at https://opensource.org/licenses/MIT).
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ */
 
 #include <sys/socket.h>
 #include <netinet/in.h>
@@ -14,160 +24,160 @@
 
 struct ControlMessage
 {
-    int16_t x, y, z, r;
+	int16_t x, y, z, r;
 } __attribute__((packed));
 
 class RC
 {
 public:
-    RC():
-        nh(),
-        nh_priv("~")
-    {
-        // Create socket thread
-        std::thread t(&RC::socketThread, this);
-        t.detach();
+	RC():
+		nh(),
+		nh_priv("~")
+	{
+		// Create socket thread
+		std::thread t(&RC::socketThread, this);
+		t.detach();
 
-        std::thread gcst(&RC::fakeGCSThread, this);
-        gcst.detach();
+		std::thread gcst(&RC::fakeGCSThread, this);
+		gcst.detach();
 
-        initLatchedState();
-    }
+		initLatchedState();
+	}
 
 private:
-    ros::NodeHandle nh, nh_priv;
-    ros::Subscriber state_sub;
-    ros::Publisher state_pub;
-    ros::Timer state_timeout_timer;
-    ros::Time last_manual_control{0};
-    mavros_msgs::StateConstPtr state_msg;
+	ros::NodeHandle nh, nh_priv;
+	ros::Subscriber state_sub;
+	ros::Publisher state_pub;
+	ros::Timer state_timeout_timer;
+	ros::Time last_manual_control{0};
+	mavros_msgs::StateConstPtr state_msg;
 
-    void handleState(const mavros_msgs::StateConstPtr& state)
-    {
-        state_timeout_timer.setPeriod(ros::Duration(3), true);
-        state_timeout_timer.start();
+	void handleState(const mavros_msgs::StateConstPtr& state)
+	{
+		state_timeout_timer.setPeriod(ros::Duration(3), true);
+		state_timeout_timer.start();
 
-        if (!state_msg ||
-            state->connected != state_msg->connected ||
-            state->mode != state_msg->mode ||
-            state->armed != state_msg->armed) {
-                state_msg = state;
-                state_pub.publish(state_msg);
-            }
-    }
+		if (!state_msg ||
+			state->connected != state_msg->connected ||
+			state->mode != state_msg->mode ||
+			state->armed != state_msg->armed) {
+				state_msg = state;
+				state_pub.publish(state_msg);
+			}
+	}
 
-    void stateTimedOut(const ros::TimerEvent&)
-    {
-        ROS_INFO("State timeout");
-        mavros_msgs::State unknown_state;
-        state_pub.publish(unknown_state);
-        state_msg = nullptr;
-    }
+	void stateTimedOut(const ros::TimerEvent&)
+	{
+		ROS_INFO("State timeout");
+		mavros_msgs::State unknown_state;
+		state_pub.publish(unknown_state);
+		state_msg = nullptr;
+	}
 
-    void initLatchedState()
-    {
-        state_sub = nh.subscribe("mavros/state", 1, &RC::handleState, this);
-        state_pub = nh.advertise<mavros_msgs::State>("state_latched", 1, true);
-        state_timeout_timer = nh.createTimer(ros::Duration(0), &RC::stateTimedOut, this, true, false);
+	void initLatchedState()
+	{
+		state_sub = nh.subscribe("mavros/state", 1, &RC::handleState, this);
+		state_pub = nh.advertise<mavros_msgs::State>("state_latched", 1, true);
+		state_timeout_timer = nh.createTimer(ros::Duration(0), &RC::stateTimedOut, this, true, false);
 
-        // Publish initial state
-        mavros_msgs::State unknown_state;
-        state_pub.publish(unknown_state);
-    }
+		// Publish initial state
+		mavros_msgs::State unknown_state;
+		state_pub.publish(unknown_state);
+	}
 
-    void fakeGCSThread()
-    {
-        // Awful workaround for fixing PX4 not sending STATUSTEXTs
-        // if there is no GCS hearbeats.
-        // TODO: use timer
-        // TODO: remove, when PX4 get this fixed.
-        ros::Publisher mavlink_pub = nh.advertise<mavros_msgs::Mavlink>("mavlink/to", 1);
+	void fakeGCSThread()
+	{
+		// Awful workaround for fixing PX4 not sending STATUSTEXTs
+		// if there is no GCS hearbeats.
+		// TODO: use timer
+		// TODO: remove, when PX4 get this fixed.
+		ros::Publisher mavlink_pub = nh.advertise<mavros_msgs::Mavlink>("mavlink/to", 1);
 
-        // HEARTBEAT from GCS message
-        mavros_msgs::Mavlink hb;
-        hb.framing_status = mavros_msgs::Mavlink::FRAMING_OK;
-        hb.magic = mavros_msgs::Mavlink::MAVLINK_V20;
-        hb.len = 9;
-        hb.incompat_flags = 0;
-        hb.compat_flags = 0;
-        hb.seq = 0;
-        hb.sysid = 255;
-        hb.compid = 0;
-        hb.checksum = 26460;
-        hb.payload64.push_back(342282393542983680);
-        hb.payload64.push_back(3);
+		// HEARTBEAT from GCS message
+		mavros_msgs::Mavlink hb;
+		hb.framing_status = mavros_msgs::Mavlink::FRAMING_OK;
+		hb.magic = mavros_msgs::Mavlink::MAVLINK_V20;
+		hb.len = 9;
+		hb.incompat_flags = 0;
+		hb.compat_flags = 0;
+		hb.seq = 0;
+		hb.sysid = 255;
+		hb.compid = 0;
+		hb.checksum = 26460;
+		hb.payload64.push_back(342282393542983680);
+		hb.payload64.push_back(3);
 
-        ros::Rate rate(1);
-        while (ros::ok()) {
-            if (ros::Time::now() - last_manual_control < ros::Duration(8)) {
-                mavlink_pub.publish(hb);
-            }
-            rate.sleep();
-        }
-    }
+		ros::Rate rate(1);
+		while (ros::ok()) {
+			if (ros::Time::now() - last_manual_control < ros::Duration(8)) {
+				mavlink_pub.publish(hb);
+			}
+			rate.sleep();
+		}
+	}
 
-    int createSocket(int port)
-    {
-        int sockfd = socket(AF_INET, SOCK_DGRAM, 0);
+	int createSocket(int port)
+	{
+		int sockfd = socket(AF_INET, SOCK_DGRAM, 0);
 
-        sockaddr_in sin;
-        sin.sin_family = AF_INET;
-        sin.sin_addr.s_addr = htonl(INADDR_ANY);
-        sin.sin_port = htons(port);
+		sockaddr_in sin;
+		sin.sin_family = AF_INET;
+		sin.sin_addr.s_addr = htonl(INADDR_ANY);
+		sin.sin_port = htons(port);
 
-        if (bind(sockfd, (sockaddr *)&sin, sizeof(sin)) < 0) {
-            ROS_FATAL("socket bind error: %s", strerror(errno));
-            close(sockfd);
-            ros::shutdown();
-        }
+		if (bind(sockfd, (sockaddr *)&sin, sizeof(sin)) < 0) {
+			ROS_FATAL("socket bind error: %s", strerror(errno));
+			close(sockfd);
+			ros::shutdown();
+		}
 
-        return sockfd;
-    }
+		return sockfd;
+	}
 
-    void socketThread()
-    {
-        int port;
-        nh_priv.param("port", port, 35602);
-        int sockfd = createSocket(port);
+	void socketThread()
+	{
+		int port;
+		nh_priv.param("port", port, 35602);
+		int sockfd = createSocket(port);
 
-        char buff[9999];
+		char buff[9999];
 
-        ros::Publisher manual_control_pub = nh.advertise<mavros_msgs::ManualControl>("mavros/manual_control/send", 1);
-        mavros_msgs::ManualControl manual_control_msg;
+		ros::Publisher manual_control_pub = nh.advertise<mavros_msgs::ManualControl>("mavros/manual_control/send", 1);
+		mavros_msgs::ManualControl manual_control_msg;
 
-        sockaddr_in client_addr;
-        socklen_t client_addr_size = sizeof(client_addr);
+		sockaddr_in client_addr;
+		socklen_t client_addr_size = sizeof(client_addr);
 
-        ROS_INFO("UDP RC initialized on port %d", port);
+		ROS_INFO("UDP RC initialized on port %d", port);
 
-        while (true) {
-            // read next UDP packet
-            int bsize = recvfrom(sockfd, &buff[0], sizeof(buff) - 1, 0, (sockaddr *) &client_addr, &client_addr_size);
+		while (true) {
+			// read next UDP packet
+			int bsize = recvfrom(sockfd, &buff[0], sizeof(buff) - 1, 0, (sockaddr *) &client_addr, &client_addr_size);
 
-            if (bsize < 0) {
-                ROS_ERROR("recvfrom() error: %s", strerror(errno));
-            } else if (bsize != sizeof(ControlMessage)) {
-                ROS_ERROR_THROTTLE(30, "Wrong UDP packet size: %d", bsize);
-            }
+			if (bsize < 0) {
+				ROS_ERROR("recvfrom() error: %s", strerror(errno));
+			} else if (bsize != sizeof(ControlMessage)) {
+				ROS_ERROR_THROTTLE(30, "Wrong UDP packet size: %d", bsize);
+			}
 
-            // unpack message
-            // warning: ignore endianness, so the code is platform-dependent
-            ControlMessage *msg = (ControlMessage *)buff;
+			// unpack message
+			// warning: ignore endianness, so the code is platform-dependent
+			ControlMessage *msg = (ControlMessage *)buff;
 
-            manual_control_msg.x = msg->x;
-            manual_control_msg.y = msg->y;
-            manual_control_msg.z = msg->z;
-            manual_control_msg.r = msg->r;
-            manual_control_pub.publish(manual_control_msg);
+			manual_control_msg.x = msg->x;
+			manual_control_msg.y = msg->y;
+			manual_control_msg.z = msg->z;
+			manual_control_msg.r = msg->r;
+			manual_control_pub.publish(manual_control_msg);
 
-            last_manual_control = ros::Time::now();
-        }
-    }
+			last_manual_control = ros::Time::now();
+		}
+	}
 };
 
 int main(int argc, char **argv)
 {
-    ros::init(argc, argv, "rc");
-    RC rc;
-    ros::spin();
+	ros::init(argc, argv, "rc");
+	RC rc;
+	ros::spin();
 }

clever/src/selfcheck.py

@@ -1,19 +1,36 @@
 #!/usr/bin/env python
+# coding=utf-8
+
+# Copyright (C) 2018 Copter Express Technologies
+#
+# Author: Oleg Kalachev <okalachev@gmail.com>
+#
+# Distributed under MIT License (available at https://opensource.org/licenses/MIT).
+# The above copyright notice and this permission notice shall be included in all
+# copies or substantial portions of the Software.
 
 import math
-from subprocess import Popen, PIPE
+import subprocess
 import re
+from collections import OrderedDict
 import traceback
+from threading import Event
+import numpy
 import rospy
+import tf2_ros
+import tf2_geometry_msgs
+from pymavlink import mavutil
 from std_srvs.srv import Trigger
-from sensor_msgs.msg import Image, CameraInfo, NavSatFix, Imu, Range
-from mavros_msgs.msg import State, OpticalFlowRad
-from geometry_msgs.msg import PoseStamped, TwistStamped
+from sensor_msgs.msg import BatteryState, Image, CameraInfo, NavSatFix, Imu, Range
+from mavros_msgs.msg import State, OpticalFlowRad, Mavlink
+from mavros_msgs.srv import ParamGet
+from geometry_msgs.msg import PoseStamped, TwistStamped, PoseWithCovarianceStamped, Vector3Stamped
+from visualization_msgs.msg import MarkerArray as VisualizationMarkerArray
 import tf.transformations as t
+from aruco_pose.msg import MarkerArray
+from mavros import mavlink
 
 
-# TODO: roscore is running
-# TODO: clever.service is running
 # TODO: check attitude is present
 # TODO: disk free space
 # TODO: map, base_link, body
@@ -26,42 +43,234 @@ import tf.transformations as t
 rospy.init_node('selfcheck')
 
 
+tf_buffer = tf2_ros.Buffer()
+tf_listener = tf2_ros.TransformListener(tf_buffer)
+
+
 failures = []
+infos = []
+current_check = None
 
 
 def failure(text, *args):
-    failures.append(text % args)
+    msg = text % args
+    rospy.logwarn('%s: %s', current_check, msg)
+    failures.append(msg)
+
+
+def info(text, *args):
+    msg = text % args
+    rospy.loginfo('%s: %s', current_check, msg)
+    infos.append(msg)
 
 
 def check(name):
     def inner(fn):
         def wrapper(*args, **kwargs):
             failures[:] = []
+            infos[:] = []
+            global current_check
+            current_check = name
             try:
                 fn(*args, **kwargs)
-                for f in failures:
-                    rospy.logwarn('%s: %s', name, f)
             except Exception as e:
                 traceback.print_exc()
-                rospy.logwarn('%s: exception occured', name)
+                rospy.logerr('%s: exception occurred', name)
                 return
-            if not failures:
+            if not failures and not infos:
                 rospy.loginfo('%s: OK', name)
         return wrapper
     return inner
 
 
+param_get = rospy.ServiceProxy('mavros/param/get', ParamGet)
+
+
+def get_param(name):
+    try:
+        res = param_get(param_id=name)
+    except rospy.ServiceException as e:
+        failure('%s: %s', name, str(e))
+        return None
+
+    if not res.success:
+        failure('Unable to retrieve PX4 parameter %s', name)
+    else:
+        if res.value.integer != 0:
+            return res.value.integer
+        return res.value.real
+
+
+recv_event = Event()
+link = mavutil.mavlink.MAVLink('', 255, 1)
+mavlink_pub = rospy.Publisher('mavlink/to', Mavlink, queue_size=1)
+mavlink_recv = ''
+
+
+def mavlink_message_handler(msg):
+    global mavlink_recv
+    if msg.msgid == 126:
+        mav_bytes_msg = mavlink.convert_to_bytes(msg)
+        mav_msg = link.decode(mav_bytes_msg)
+        mavlink_recv += ''.join(chr(x) for x in mav_msg.data[:mav_msg.count])
+        if 'nsh>' in mavlink_recv:
+            # Remove the last line, including newline before prompt
+            mavlink_recv = mavlink_recv[:mavlink_recv.find('nsh>') - 1]
+            recv_event.set()
+
+
+mavlink_sub = rospy.Subscriber('mavlink/from', Mavlink, mavlink_message_handler)
+# FIXME: not sleeping here still breaks things
+rospy.sleep(0.5)
+
+
+def mavlink_exec(cmd, timeout=3.0):
+    global mavlink_recv
+    mavlink_recv = ''
+    recv_event.clear()
+    if not cmd.endswith('\n'):
+        cmd += '\n'
+    msg = mavutil.mavlink.MAVLink_serial_control_message(
+        device=mavutil.mavlink.SERIAL_CONTROL_DEV_SHELL,
+        flags=mavutil.mavlink.SERIAL_CONTROL_FLAG_RESPOND | mavutil.mavlink.SERIAL_CONTROL_FLAG_EXCLUSIVE |
+              mavutil.mavlink.SERIAL_CONTROL_FLAG_MULTI,
+        timeout=3,
+        baudrate=0,
+        count=len(cmd),
+        data=map(ord, cmd.ljust(70, '\0')))
+    msg.pack(link)
+    ros_msg = mavlink.convert_to_rosmsg(msg)
+    mavlink_pub.publish(ros_msg)
+    recv_event.wait(timeout)
+    return mavlink_recv
+
+
+BOARD_ROTATIONS = {
+    0: 'no rotation',
+    1: 'yaw 45°',
+    2: 'yaw 90°',
+    3: 'yaw 135°',
+    4: 'yaw 180°',
+    5: 'yaw 225°',
+    6: 'yaw 270°',
+    7: 'yaw 315°',
+    8: 'roll 180°',
+    9: 'roll 180°, yaw 45°',
+    10: 'roll 180°, yaw 90°',
+    11: 'roll 180°, yaw 135°',
+    12: 'pitch 180°',
+    13: 'roll 180°, yaw 225°',
+    14: 'roll 180°, yaw 270°',
+    15: 'roll 180°, yaw 315°',
+    16: 'roll 90°',
+    17: 'roll 90°, yaw 45°',
+    18: 'roll 90°, yaw 90°',
+    19: 'roll 90°, yaw 135°',
+    20: 'roll 270°',
+    21: 'roll 270°, yaw 45°',
+    22: 'roll 270°, yaw 90°',
+    23: 'roll 270°, yaw 135°',
+    24: 'pitch 90°',
+    25: 'pitch 270°',
+    26: 'roll 270°, yaw 270°',
+    27: 'roll 180°, pitch 270°',
+    28: 'pitch 90°, yaw 180',
+    29: 'pitch 90°, roll 90°',
+    30: 'yaw 293°, pitch 68°, roll 90°',
+    31: 'pitch 90°, roll 270°',
+    32: 'pitch 9°, yaw 180°',
+    33: 'pitch 45°',
+    34: 'pitch 315°',
+}
+
+
 @check('FCU')
 def check_fcu():
     try:
         state = rospy.wait_for_message('mavros/state', State, timeout=3)
         if not state.connected:
             failure('no connection to the FCU (check wiring)')
+            return
+
+        # Make sure the console is available to us
+        mavlink_exec('\n')
+        version_str = mavlink_exec('ver all')
+        if version_str == '':
+            info('no version data available from SITL')
+
+        r = re.compile(r'^FW (git tag|version): (v?\d\.\d\.\d.*)$')
+        is_clever_firmware = False
+        for ver_line in version_str.split('\n'):
+            match = r.search(ver_line)
+            if match is not None:
+                field, version = match.groups()
+                info('firmware %s: %s' % (field, version))
+                if 'clever' in version:
+                    is_clever_firmware = True
+
+        if not is_clever_firmware:
+            failure('not running Clever PX4 firmware, check http://clever.copterexpress.com/firmware.html')
+
+        est = get_param('SYS_MC_EST_GROUP')
+        if est == 1:
+            info('selected estimator: LPE')
+            fuse = get_param('LPE_FUSION')
+            if fuse & (1 << 4):
+                info('LPE_FUSION: land detector fusion is enabled')
+            else:
+                info('LPE_FUSION: land detector fusion is disabled')
+            if fuse & (1 << 7):
+                info('LPE_FUSION: barometer fusion is enabled')
+            else:
+                info('LPE_FUSION: barometer fusion is disabled')
+
+        elif est == 2:
+            info('selected estimator: EKF2')
+        else:
+            failure('unknown selected estimator: %s', est)
+
+        rot = get_param('SENS_BOARD_ROT')
+        if rot is not None:
+            try:
+                info('board rotation: %s', BOARD_ROTATIONS[rot])
+            except KeyError:
+                failure('unknown board rotation %s', rot)
+
+        cbrk_usb_chk = get_param('CBRK_USB_CHK')
+        if cbrk_usb_chk != 197848:
+            failure('Set parameter CBRK_USB_CHK to 197848 for flying with USB connected')
+
+        try:
+            battery = rospy.wait_for_message('mavros/battery', BatteryState, timeout=3)
+            cell = battery.cell_voltage[0]
+            if cell > 4.3 or cell < 3.0:
+                failure('Incorrect cell voltage: %.2f V, see https://clever.copterexpress.com/power.html', cell)
+            elif cell < 3.7:
+                failure('Critically low cell voltage: %.2f V, recharge battery', cell)
+        except rospy.ROSException:
+            failure('no battery state')
+
     except rospy.ROSException:
         failure('no MAVROS state (check wiring)')
 
 
-@check('Camera')
+def describe_direction(v):
+    if v.x > 0.9:
+        return 'forward'
+    elif v.x < - 0.9:
+        return 'backward'
+    elif v.y > 0.9:
+        return 'left'
+    elif v.y < -0.9:
+        return 'right'
+    elif v.z > 0.9:
+        return 'upward'
+    elif v.z < -0.9:
+        return 'downward'
+    else:
+        return None
+
+
 def check_camera(name):
     try:
         img = rospy.wait_for_message(name + '/image_raw', Image, timeout=1)
@@ -69,27 +278,98 @@ def check_camera(name):
         failure('%s: no images (is the camera connected properly?)', name)
         return
     try:
-        info = rospy.wait_for_message(name + '/camera_info', CameraInfo, timeout=1)
+        camera_info = rospy.wait_for_message(name + '/camera_info', CameraInfo, timeout=1)
     except rospy.ROSException:
         failure('%s: no calibration info', name)
         return
 
-    if img.width != info.width:
-        failure('%s: calibration width doesn\'t match image width (%d != %d)', name, info.width, img.width)
-    if img.height != info.height:
-        failure('%s: calibration height doesn\'t match image height (%d != %d))', name, info.height, img.height)
+    if img.width != camera_info.width:
+        failure('%s: calibration width doesn\'t match image width (%d != %d)', name, camera_info.width, img.width)
+    if img.height != camera_info.height:
+        failure('%s: calibration height doesn\'t match image height (%d != %d))', name, camera_info.height, img.height)
 
-
-@check('Aruco detector')
-def check_aruco():
     try:
-        rospy.wait_for_message('aruco_pose/debug', Image, timeout=1)
-    except rospy.ROSException:
-        failure('no aruco_pose/debug messages')
+        optical = Vector3Stamped()
+        optical.header.frame_id = img.header.frame_id
+        optical.vector.z = 1
+        cable = Vector3Stamped()
+        cable.header.frame_id = img.header.frame_id
+        cable.vector.y = 1
+
+        optical = describe_direction(tf_buffer.transform(optical, 'base_link').vector)
+        cable = describe_direction(tf_buffer.transform(cable, 'base_link').vector)
+        if not optical or not cable:
+            info('%s: custom camera orientation detected', name)
+        else:
+            info('camera is oriented %s, camera cable goes %s', optical, cable)
+
+    except tf2_ros.TransformException:
+        failure('cannot transform from base_link to camera frame')
+
+
+@check('Main camera')
+def check_main_camera():
+    check_camera('main_camera')
+
+
+def is_process_running(binary, exact=False, full=False):
+    try:
+        args = ['pgrep']
+        if exact:
+            args.append('-x')  # match exactly with the command name
+        if full:
+            args.append('-f')  # use full process name to match
+        args.append(binary)
+        subprocess.check_output(args)
+        return True
+    except subprocess.CalledProcessError:
+        return False
+
+
+@check('ArUco markers')
+def check_aruco():
+    if is_process_running('aruco_detect', full=True):
+        info('aruco_detect/length = %g m', rospy.get_param('aruco_detect/length'))
+        known_tilt = rospy.get_param('aruco_detect/known_tilt')
+        if known_tilt == 'map':
+            known_tilt += ' (ALL markers are on the floor)'
+        elif known_tilt == 'map_flipped':
+            known_tilt += ' (ALL markers are on the ceiling)'
+        info('aruco_detector/known_tilt = %s', known_tilt)
+        try:
+            rospy.wait_for_message('aruco_detect/markers', MarkerArray, timeout=1)
+        except rospy.ROSException:
+            failure('no markers detection')
+            return
+    else:
+        info('aruco_detect is not running')
+        return
+
+    if is_process_running('aruco_map', full=True):
+        known_tilt = rospy.get_param('aruco_map/known_tilt')
+        if known_tilt == 'map':
+            known_tilt += ' (marker\'s map is on the floor)'
+        elif known_tilt == 'map_flipped':
+            known_tilt += ' (marker\'s map is on the ceiling)'
+        info('aruco_map/known_tilt = %s', known_tilt)
+
+        try:
+            visualization = rospy.wait_for_message('aruco_map/visualization', VisualizationMarkerArray, timeout=1)
+            info('map has %s markers', len(visualization.markers))
+        except:
+            failure('cannot read aruco_map/visualization topic')
+
+        try:
+            rospy.wait_for_message('aruco_map/pose', PoseWithCovarianceStamped, timeout=1)
+        except rospy.ROSException:
+            failure('no map detection')
+    else:
+        info('aruco_map is not running')
 
 
 @check('Vision position estimate')
 def check_vpe():
+    vis = None
     try:
         vis = rospy.wait_for_message('mavros/vision_pose/pose', PoseStamped, timeout=1)
     except rospy.ROSException:
@@ -97,7 +377,45 @@ def check_vpe():
             vis = rospy.wait_for_message('mavros/mocap/pose', PoseStamped, timeout=1)
         except rospy.ROSException:
             failure('no VPE or MoCap messages')
-            return
+            # check if vpe_publisher is running
+            try:
+                subprocess.check_output(['pgrep', '-x', 'vpe_publisher'])
+            except subprocess.CalledProcessError:
+                return  # it's not running, skip following checks
+
+    # check PX4 settings
+    est = get_param('SYS_MC_EST_GROUP')
+    if est == 1:
+        ext_yaw = get_param('ATT_EXT_HDG_M')
+        if ext_yaw != 1:
+            failure('vision yaw is disabled, change ATT_EXT_HDG_M parameter')
+        vision_yaw_w = get_param('ATT_W_EXT_HDG')
+        if vision_yaw_w == 0:
+            failure('vision yaw weight is zero, change ATT_W_EXT_HDG parameter')
+        else:
+            info('Vision yaw weight: %.2f', vision_yaw_w)
+        fuse = get_param('LPE_FUSION')
+        if not fuse & (1 << 2):
+            failure('vision position fusion is disabled, change LPE_FUSION parameter')
+        delay = get_param('LPE_VIS_DELAY')
+        if delay != 0:
+            failure('LPE_VIS_DELAY parameter is %s, but it should be zero', delay)
+        info('LPE_VIS_XY is %.2f m, LPE_VIS_Z is %.2f m', get_param('LPE_VIS_XY'), get_param('LPE_VIS_Z'))
+    elif est == 2:
+        fuse = get_param('EKF2_AID_MASK')
+        if not fuse & (1 << 3):
+            failure('vision position fusion is disabled, change EKF2_AID_MASK parameter')
+        if not fuse & (1 << 4):
+            failure('vision yaw fusion is disabled, change EKF2_AID_MASK parameter')
+        delay = get_param('EKF2_EV_DELAY')
+        if delay != 0:
+            failure('EKF2_EV_DELAY is %.2f, but it should be zero', delay)
+        info('EKF2_EVA_NOISE is %.3f, EKF2_EVP_NOISE is %.3f',
+            get_param('EKF2_EVA_NOISE'),
+            get_param('EKF2_EVP_NOISE'))
+
+    if not vis:
+        return
 
     # check vision pose and estimated pose inconsistency
     try:
@@ -168,7 +486,7 @@ def check_velocity():
             failure('vertical velocity estimation is %.2f m/s; is copter staying still?' % vert)
 
         angular = velocity.twist.angular
-        ANGULAR_VELOCITY_LIMIT = 0.01
+        ANGULAR_VELOCITY_LIMIT = 0.1
         if abs(angular.x) > ANGULAR_VELOCITY_LIMIT:
             failure('pitch rate estimation is %.2f rad/s (%.2f deg/s); is copter staying still?',
                     angular.x, math.degrees(angular.x))
@@ -195,6 +513,42 @@ def check_optical_flow():
     # TODO:check FPS!
     try:
         rospy.wait_for_message('mavros/px4flow/raw/send', OpticalFlowRad, timeout=0.5)
+
+        # check PX4 settings
+        rot = get_param('SENS_FLOW_ROT')
+        if rot != 0:
+            failure('SENS_FLOW_ROT parameter is %s, but it should be zero', rot)
+        est = get_param('SYS_MC_EST_GROUP')
+        if est == 1:
+            fuse = get_param('LPE_FUSION')
+            if not fuse & (1 << 1):
+                failure('optical flow fusion is disabled, change LPE_FUSION parameter')
+            if not fuse & (1 << 1):
+                failure('flow gyro compensation is disabled, change LPE_FUSION parameter')
+            scale = get_param('LPE_FLW_SCALE')
+            if not numpy.isclose(scale, 1.0):
+                failure('LPE_FLW_SCALE parameter is %.2f, but it should be 1.0', scale)
+
+            info('LPE_FLW_QMIN is %s, LPE_FLW_R is %.4f, LPE_FLW_RR is %.4f, SENS_FLOW_MINHGT is %.3f, SENS_FLOW_MAXHGT is %.3f',
+                          get_param('LPE_FLW_QMIN'),
+                          get_param('LPE_FLW_R'),
+                          get_param('LPE_FLW_RR'),
+                          get_param('SENS_FLOW_MINHGT'),
+                          get_param('SENS_FLOW_MAXHGT'))
+        elif est == 2:
+            fuse = get_param('EKF2_AID_MASK')
+            if not fuse & (1 << 1):
+                failure('optical flow fusion is disabled, change EKF2_AID_MASK parameter')
+            delay = get_param('EKF2_OF_DELAY')
+            if delay != 0:
+                failure('EKF2_OF_DELAY is %.2f, but it should be zero', delay)
+            info('EKF2_OF_QMIN is %s, EKF2_OF_N_MIN is %.4f, EKF2_OF_N_MAX is %.4f, SENS_FLOW_MINHGT is %.3f, SENS_FLOW_MAXHGT is %.3f',
+                          get_param('EKF2_OF_QMIN'),
+                          get_param('EKF2_OF_N_MIN'),
+                          get_param('EKF2_OF_N_MAX'),
+                          get_param('SENS_FLOW_MINHGT'),
+                          get_param('SENS_FLOW_MAXHGT'))
+
     except rospy.ROSException:
         failure('no optical flow data (from Raspberry)')
 
@@ -202,21 +556,46 @@ def check_optical_flow():
 @check('Rangefinder')
 def check_rangefinder():
     # TODO: check FPS!
+    rng = False
     try:
-        rospy.wait_for_message('mavros/distance_sensor/rangefinder_3_sub', Range, timeout=0.5)
+        rospy.wait_for_message('rangefinder/range', Range, timeout=4)
+        rng = True
     except rospy.ROSException:
-        failure('no randefinder data from Raspberry')
+        failure('no rangefinder data from Raspberry')
+
     try:
-        rospy.wait_for_message('mavros/distance_sensor/rangefinder_0', Range, timeout=0.5)
+        rospy.wait_for_message('mavros/distance_sensor/rangefinder', Range, timeout=4)
+        rng = True
     except rospy.ROSException:
         failure('no rangefinder data from PX4')
 
+    if not rng:
+        return
+
+    est = get_param('SYS_MC_EST_GROUP')
+    if est == 1:
+        fuse = get_param('LPE_FUSION')
+        if not fuse & (1 << 5):
+            info('"pub agl as lpos down" in LPE_FUSION is disabled, NOT operating over flat surface')
+        else:
+            info('"pub agl as lpos down" in LPE_FUSION is enabled, operating over flat surface')
+
+    elif est == 2:
+        hgt = get_param('EKF2_HGT_MODE')
+        if hgt != 2:
+            info('EKF2_HGT_MODE != Range sensor, NOT operating over flat surface')
+        else:
+            info('EKF2_HGT_MODE = Range sensor, operating over flat surface')
+        aid = get_param('EKF2_RNG_AID')
+        if aid != 1:
+            info('EKF2_RNG_AID != 1, range sensor aiding disabled')
+        else:
+            info('EKF2_RNG_AID = 1, range sensor aiding enabled')
+
 
 @check('Boot duration')
 def check_boot_duration():
-    proc = Popen('systemd-analyze', stdout=PIPE)
-    proc.wait()
-    output = proc.communicate()[0]
+    output = subprocess.check_output('systemd-analyze')
     r = re.compile(r'([\d\.]+)s$')
     duration = float(r.search(output).groups()[0])
     if duration > 15:
@@ -227,9 +606,7 @@ def check_boot_duration():
 def check_cpu_usage():
     WHITELIST = 'nodelet',
     CMD = "top -n 1 -b -i | tail -n +8 | awk '{ printf(\"%-8s\\t%-8s\\t%-8s\\n\", $1, $9, $12); }'"
-    proc = Popen(CMD, stdout=PIPE, shell=True)
-    proc.wait()
-    output = proc.communicate()[0]
+    output = subprocess.check_output(CMD, shell=True)
     processes = output.split('\n')
     for process in processes:
         if not process:
@@ -241,13 +618,83 @@ def check_cpu_usage():
                     cpu.strip(), cmd.strip(), pid.strip())
 
 
+@check('clever.service')
+def check_clever_service():
+    try:
+        output = subprocess.check_output('systemctl show -p ActiveState --value clever.service'.split(),
+                                         stderr=subprocess.STDOUT)
+    except subprocess.CalledProcessError as e:
+        failure('systemctl returned %s: %s', e.returncode, e.output)
+        return
+    if 'inactive' in output:
+        failure('service is not running, try sudo systemctl restart clever')
+        return
+    elif 'failed' in output:
+        failure('service failed to run, check your launch-files')
+
+    r = re.compile(r'^(.*)\[(FATAL|ERROR)\] \[\d+.\d+\]: (.*?)(\x1b(.*))?$')
+    error_count = OrderedDict()
+    try:
+        for line in open('/tmp/clever.err', 'r'):
+            node_error = r.search(line)
+            if node_error:
+                msg = node_error.groups()[1] + ': ' + node_error.groups()[2]
+                if msg in error_count:
+                    error_count[msg] += 1
+                else:
+                    error_count.update({msg: 1})
+            else:
+                error_count.update({line.strip(): 1})
+
+        for error in error_count:
+            if error_count[error] == 1:
+                failure(error)
+            else:
+                failure('%s (%d)', error, error_count[error])
+
+    except IOError as e:
+        failure('%s', e)
+
+
+@check('Image')
+def check_image():
+    try:
+        info('version: %s', open('/etc/clever_version').read().strip())
+    except IOError:
+        info('no /etc/clever_version file, not the Clever image?')
+
+
+@check('Preflight status')
+def check_preflight_status():
+    # Make sure the console is available to us
+    mavlink_exec('\n')
+    cmdr_output = mavlink_exec('commander check')
+    if cmdr_output == '':
+        failure('no data from FCU')
+        return
+    cmdr_lines = cmdr_output.split('\n')
+    r = re.compile(r'^(.*)(Preflight|Prearm) check: (.*)')
+    for line in cmdr_lines:
+        if 'WARN' in line:
+            failure(line[line.find(']') + 2:])
+            continue
+        match = r.search(line)
+        if match is not None:
+            check_status = match.groups()[2]
+            if check_status != 'OK':
+                failure(' '.join([match.groups()[1], 'check:', check_status]))
+
+
 def selfcheck():
+    check_image()
+    check_clever_service()
     check_fcu()
     check_imu()
     check_local_position()
     check_velocity()
     check_global_position()
-    check_camera('main_camera')
+    check_preflight_status()
+    check_main_camera()
     check_aruco()
     check_simpleoffboard()
     check_optical_flow()

clever/src/simple_offboard.cpp

@@ -20,6 +20,7 @@
 #include <tf2/utils.h>
 #include <tf2_ros/transform_listener.h>
 #include <tf2_ros/transform_broadcaster.h>
+#include <tf2_ros/static_transform_broadcaster.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 #include <std_srvs/Trigger.h>
 #include <geometry_msgs/PoseStamped.h>
@@ -34,6 +35,7 @@
 #include <mavros_msgs/AttitudeTarget.h>
 #include <mavros_msgs/Thrust.h>
 #include <mavros_msgs/State.h>
+#include <mavros_msgs/StatusText.h>
 
 #include <clever/GetTelemetry.h>
 #include <clever/Navigate.h>
@@ -53,6 +55,8 @@ using mavros_msgs::Thrust;
 
 // tf2
 tf2_ros::Buffer tf_buffer;
+std::shared_ptr<tf2_ros::TransformBroadcaster> transform_broadcaster;
+std::shared_ptr<tf2_ros::StaticTransformBroadcaster> static_transform_broadcaster;
 
 // Parameters
 string local_frame;
@@ -69,6 +73,7 @@ ros::Duration global_position_timeout;
 ros::Duration battery_timeout;
 float default_speed;
 bool auto_release;
+bool land_only_in_offboard, nav_from_sp;
 std::map<string, string> reference_frames;
 
 // Publishers
@@ -86,6 +91,7 @@ AttitudeTarget att_raw_msg;
 Thrust thrust_msg;
 TwistStamped rates_msg;
 TransformStamped target;
+geometry_msgs::TransformStamped body;
 
 // State
 PoseStamped nav_start;
@@ -96,6 +102,7 @@ float setpoint_yaw_rate;
 float nav_speed;
 bool busy = false;
 bool wait_armed = false;
+bool nav_from_sp_flag = false;
 
 enum setpoint_type_t {
 	NONE,
@@ -113,23 +120,56 @@ enum { YAW, YAW_RATE, TOWARDS } setpoint_yaw_type;
 
 // Last received telemetry messages
 mavros_msgs::State state;
+mavros_msgs::StatusText statustext;
 PoseStamped local_position;
 TwistStamped velocity;
 NavSatFix global_position;
 BatteryState battery;
 
-// Common subcriber callback template that stores message to the variable
+// Common subscriber callback template that stores message to the variable
 template<typename T, T& STORAGE>
 void handleMessage(const T& msg)
 {
 	STORAGE = msg;
 }
 
+void handleState(const mavros_msgs::State& s)
+{
+	state = s;
+	if (s.mode != "OFFBOARD") {
+		// flight intercepted
+		nav_from_sp_flag = false;
+	}
+}
+
+inline void publishBodyFrame()
+{
+	if (body.child_frame_id.empty()) return;
+
+	tf::Quaternion q;
+	q.setRPY(0, 0, tf::getYaw(local_position.pose.orientation));
+	tf::quaternionTFToMsg(q, body.transform.rotation);
+
+	body.transform.translation.x = local_position.pose.position.x;
+	body.transform.translation.y = local_position.pose.position.y;
+	body.transform.translation.z = local_position.pose.position.z;
+	body.header.frame_id = local_position.header.frame_id;
+	body.header.stamp = local_position.header.stamp;
+	transform_broadcaster->sendTransform(body);
+}
+
+void handleLocalPosition(const PoseStamped& pose)
+{
+	local_position = pose;
+	publishBodyFrame();
+	// TODO: terrain?, home?
+}
+
 // wait for transform without interrupting publishing setpoints
 inline bool waitTransform(const string& target, const string& source,
-                          const ros::Time& stamp, const ros::Duration& timeout)
+                          const ros::Time& stamp, const ros::Duration& timeout) // editorconfig-checker-disable-line
 {
-	ros::Rate r(10);
+	ros::Rate r(100);
 	auto start = ros::Time::now();
 	while (ros::ok()) {
 		if (ros::Time::now() - start > timeout) return false;
@@ -173,31 +213,29 @@ bool getTelemetry(GetTelemetry::Request& req, GetTelemetry::Response& res)
 		res.mode = state.mode;
 	}
 
-	waitTransform(local_frame, req.frame_id, stamp, telemetry_transform_timeout);
+	try {
+		waitTransform(req.frame_id, fcu_frame, stamp, telemetry_transform_timeout);
+		auto transform = tf_buffer.lookupTransform(req.frame_id, fcu_frame, stamp);
+		res.x = transform.transform.translation.x;
+		res.y = transform.transform.translation.y;
+		res.z = transform.transform.translation.z;
 
-	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) {}
+		double yaw, pitch, roll;
+		tf2::getEulerYPR(transform.transform.rotation, yaw, pitch, roll);
+		res.yaw = yaw;
+		res.pitch = pitch;
+		res.roll = roll;
+	} catch (const tf2::TransformException& e) {
+		ROS_DEBUG("%s", e.what());
 	}
 
 	if (!TIMEOUT(velocity, velocity_timeout)) {
 		try {
 			// transform velocity
+			waitTransform(req.frame_id, fcu_frame, velocity.header.stamp, telemetry_transform_timeout);
 			Vector3Stamped vec, vec_out;
-			vec.header = velocity.header;
+			vec.header.stamp = velocity.header.stamp;
+			vec.header.frame_id = velocity.header.frame_id;
 			vec.vector = velocity.twist.linear;
 			tf_buffer.transform(vec, vec_out, req.frame_id);
 
@@ -235,7 +273,7 @@ void offboardAndArm()
 
 	if (state.mode != "OFFBOARD") {
 		auto start = ros::Time::now();
-		ROS_INFO("simple_offboard: switch to OFFBOARD");
+		ROS_INFO("switch to OFFBOARD");
 		static mavros_msgs::SetMode sm;
 		sm.request.custom_mode = "OFFBOARD";
 
@@ -248,7 +286,10 @@ void offboardAndArm()
 			if (state.mode == "OFFBOARD") {
 				break;
 			} else if (ros::Time::now() - start > offboard_timeout) {
-				throw std::runtime_error("OFFBOARD request timed out");
+				string report = "OFFBOARD timed out";
+				if (statustext.header.stamp > start)
+					report += ": " + statustext.text;
+				throw std::runtime_error(report);
 			}
 			ros::spinOnce();
 			r.sleep();
@@ -257,7 +298,7 @@ void offboardAndArm()
 
 	if (!state.armed) {
 		ros::Time start = ros::Time::now();
-		ROS_INFO("simple_offboard: arming");
+		ROS_INFO("arming");
 		mavros_msgs::CommandBool srv;
 		srv.request.value = true;
 		if (!arming.call(srv)) {
@@ -270,7 +311,10 @@ void offboardAndArm()
 			if (state.armed) {
 				break;
 			} else if (ros::Time::now() - start > arming_timeout) {
-				throw std::runtime_error("Arming timed out");
+				string report = "Arming timed out";
+				if (statustext.header.stamp > start)
+					report += ": " + statustext.text;
+				throw std::runtime_error(report);
 			}
 			ros::spinOnce();
 			r.sleep();
@@ -317,6 +361,10 @@ PoseStamped globalToLocal(double lat, double lon)
 	x_offset = distance * sin(azimuth_radians);
 	y_offset = distance * cos(azimuth_radians);
 
+	if (!waitTransform(local_frame, fcu_frame, global_position.header.stamp, ros::Duration(0.2))) {
+		throw std::runtime_error("No local position");
+	}
+
 	auto local = tf_buffer.lookupTransform(local_frame, fcu_frame, global_position.header.stamp);
 
 	PoseStamped pose;
@@ -350,19 +398,7 @@ void publish(const ros::Time stamp)
 		}
 
 	} 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);
-		}
+		ROS_WARN_THROTTLE(10, "can't transform");
 	}
 
 	if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL) {
@@ -446,10 +482,12 @@ inline void checkState()
 		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)
+#define ENSURE_FINITE(var) { if (!std::isfinite(var)) throw std::runtime_error(#var " argument cannot be NaN or Inf"); }
+
+bool 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, // editorconfig-checker-disable-line
+           float lat, float lon, float thrust, float speed, string frame_id, bool auto_arm, // editorconfig-checker-disable-line
+           uint8_t& success, string& message) // editorconfig-checker-disable-line
 {
 	auto stamp = ros::Time::now();
 
@@ -457,6 +495,20 @@ inline void serve(enum setpoint_type_t sp_type, float x, float y, float z, float
 		if (busy)
 			throw std::runtime_error("Busy");
 
+		ENSURE_FINITE(x);
+		ENSURE_FINITE(y);
+		ENSURE_FINITE(z);
+		ENSURE_FINITE(vx);
+		ENSURE_FINITE(vy);
+		ENSURE_FINITE(vz);
+		ENSURE_FINITE(pitch);
+		ENSURE_FINITE(roll);
+		ENSURE_FINITE(pitch_rate);
+		ENSURE_FINITE(roll_rate);
+		ENSURE_FINITE(lat);
+		ENSURE_FINITE(lon);
+		ENSURE_FINITE(thrust);
+
 		busy = true;
 
 		// Checks
@@ -506,7 +558,9 @@ inline void serve(enum setpoint_type_t sp_type, float x, float y, float z, float
 
 		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);
+			auto pose_local = globalToLocal(lat, lon);
+			pose_local.header.stamp = stamp; // TODO: fix
+			auto xy_in_req_frame = tf_buffer.transform(pose_local, frame_id);
 			x = xy_in_req_frame.pose.position.x;
 			y = xy_in_req_frame.pose.position.y;
 		}
@@ -514,19 +568,30 @@ inline void serve(enum setpoint_type_t sp_type, float x, float y, float z, float
 		// 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) {
+			nav_from_sp_flag = false;
 		}
 
-		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 == NAVIGATE || sp_type == NAVIGATE_GLOBAL) {
+			// starting point
+			if (nav_from_sp && nav_from_sp_flag) {
+				message = "Navigating from current setpoint";
+				nav_start = position_msg;
+			} else {
+				nav_start = local_position;
 			}
+			nav_speed = speed;
+			nav_from_sp_flag = true;
 		}
 
+		// if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == POSITION || sp_type == VELOCITY) {
+		// 	if (std::isnan(yaw) && yaw_rate == 0) {
+		// 		// keep yaw unchanged
+		//		// TODO: this is incorrect, because we need yaw in desired frame
+		// 		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;
@@ -578,6 +643,19 @@ inline void serve(enum setpoint_type_t sp_type, float x, float y, float z, float
 		publish(stamp); // calculate initial transformed messages first
 		setpoint_timer.start();
 
+		// publish target frame
+		if (!target.child_frame_id.empty()) {
+			if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == POSITION) {
+				target.header.frame_id = setpoint_position.header.frame_id;
+				target.header.stamp = stamp;
+				target.transform.translation.x = setpoint_position.pose.position.x;
+				target.transform.translation.y = setpoint_position.pose.position.y;
+				target.transform.translation.z = setpoint_position.pose.position.z;
+				target.transform.rotation = setpoint_position.pose.orientation;
+				static_transform_broadcaster->sendTransform(target);
+			}
+		}
+
 		if (auto_arm) {
 			offboardAndArm();
 			wait_armed = false;
@@ -591,44 +669,38 @@ inline void serve(enum setpoint_type_t sp_type, float x, float y, float z, float
 
 	} catch (const std::exception& e) {
 		message = e.what();
-		ROS_INFO("simple_offboard: %s", message.c_str());
+		ROS_INFO("%s", message.c_str());
 		busy = false;
-		return;
+		return true;
 	}
 
 	success = true;
 	busy = false;
-	return;
+	return true;
 }
 
 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;
+	return 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);
 }
 
 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;
+	return 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);
 }
 
 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;
+	return 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);
 }
 
 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;
+	return 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);
 }
 
 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;
+	return 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);
 }
 
 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;
+	return 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);
 }
 
 bool land(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)
@@ -641,6 +713,12 @@ bool land(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)
 
 		checkState();
 
+		if (land_only_in_offboard) {
+			if (state.mode != "OFFBOARD") {
+				throw std::runtime_error("Copter is not in OFFBOARD mode");
+			}
+		}
+
 		static mavros_msgs::SetMode sm;
 		sm.request.custom_mode = "AUTO.LAND";
 
@@ -667,7 +745,7 @@ bool land(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)
 
 	} catch (const std::exception& e) {
 		res.message = e.what();
-		ROS_INFO("simple_offboard: %s", e.what());
+		ROS_INFO("%s", e.what());
 		busy = false;
 		return true;
 	}
@@ -679,13 +757,18 @@ int main(int argc, char **argv)
 	ros::NodeHandle nh, nh_priv("~");
 
 	tf2_ros::TransformListener tf_listener(tf_buffer);
+	transform_broadcaster = std::make_shared<tf2_ros::TransformBroadcaster>();
+	static_transform_broadcaster = std::make_shared<tf2_ros::StaticTransformBroadcaster>();
 
 	// Params
 	nh.param<string>("mavros/local_position/tf/frame_id", local_frame, "map");
 	nh.param<string>("mavros/local_position/tf/child_frame_id", fcu_frame, "base_link");
-	nh_priv.param("target_frame", target.child_frame_id, string("target"));
+	nh_priv.param("target_frame", target.child_frame_id, string("navigate_target"));
 	nh_priv.param("auto_release", auto_release, true);
+	nh_priv.param("land_only_in_offboard", land_only_in_offboard, true);
+	nh_priv.param("nav_from_sp", nav_from_sp, true);
 	nh_priv.param("default_speed", default_speed, 0.5f);
+	nh_priv.param<string>("body_frame", body.child_frame_id, "body");
 	nh_priv.getParam("reference_frames", reference_frames);
 
 	state_timeout = ros::Duration(nh_priv.param("state_timeout", 3.0));
@@ -705,11 +788,12 @@ int main(int argc, char **argv)
 	set_mode = nh.serviceClient<mavros_msgs::SetMode>("mavros/set_mode");
 
 	// Telemetry subscribers
-	auto state_sub = nh.subscribe("mavros/state", 1, &handleMessage<mavros_msgs::State, state>);
-	auto local_position_sub = nh.subscribe("mavros/local_position/pose", 1, &handleMessage<PoseStamped, local_position>);
-	auto velocity_sub = nh.subscribe("mavros/local_position/velocity", 1, &handleMessage<TwistStamped, velocity>);
+	auto state_sub = nh.subscribe("mavros/state", 1, &handleState);
+	auto velocity_sub = nh.subscribe("mavros/local_position/velocity_body", 1, &handleMessage<TwistStamped, velocity>);
 	auto global_position_sub = nh.subscribe("mavros/global_position/global", 1, &handleMessage<NavSatFix, global_position>);
 	auto battery_sub = nh.subscribe("mavros/battery", 1, &handleMessage<BatteryState, battery>);
+	auto statustext_sub = nh.subscribe("mavros/statustext/recv", 1, &handleMessage<mavros_msgs::StatusText, statustext>);
+	auto local_position_sub = nh.subscribe("mavros/local_position/pose", 1, &handleLocalPosition);
 
 	// Setpoint publishers
 	position_pub = nh.advertise<PoseStamped>("mavros/setpoint_position/local", 1);
@@ -737,6 +821,6 @@ int main(int argc, char **argv)
 	position_raw_msg.coordinate_frame = PositionTarget::FRAME_LOCAL_NED;
 	rates_msg.header.frame_id = fcu_frame;
 
-	ROS_INFO("simple_offboard: ready");
+	ROS_INFO("ready");
 	ros::spin();
 }

clever/src/util.h

@@ -1,17 +0,0 @@
-#pragma once
-
-#include <tf/transform_datatypes.h>
-#include <geometry_msgs/Quaternion.h>
-
-inline void quaternionToEuler(geometry_msgs::Quaternion q, double& roll, double& pitch, double& yaw)
-{
-    tf::Quaternion tfq(q.x, q.y, q.z, q.w);
-    tf::Matrix3x3 m(tfq);
-    m.getRPY(roll, pitch, yaw);
-}
-
-inline void eulerToQuaternion(geometry_msgs::Quaternion& q, double roll, double pitch, double yaw)
-{
-    tf::Quaternion tfq(roll, pitch, yaw);
-    quaternionTFToMsg(tfq, q);
-}

clever/src/util.py

@@ -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)

clever/src/vpe_publisher.cpp

@@ -0,0 +1,144 @@
+/*
+ * VPE publisher node
+ * Copyright (C) 2018 Copter Express Technologies
+ *
+ * Author: Oleg Kalachev <okalachev@gmail.com>
+ *
+ * Distributed under MIT License (available at https://opensource.org/licenses/MIT).
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ */
+
+#include <string>
+#include <ros/ros.h>
+#include <tf2/transform_datatypes.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_ros/static_transform_broadcaster.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <geometry_msgs/TransformStamped.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
+// #include <aruco_pose/MarkerArray.h>
+
+using std::string;
+using namespace geometry_msgs;
+
+string local_frame_id, frame_id, child_frame_id, offset_frame_id;
+tf2_ros::Buffer tf_buffer;
+ros::Publisher vpe_pub;
+ros::Subscriber local_position_sub;
+ros::Timer zero_timer;
+PoseStamped vpe, pose;
+ros::Time got_local_pos(0);
+ros::Duration publish_zero_timout, publish_zero_duration, offset_timeout;
+TransformStamped offset;
+
+void publishZero(const ros::TimerEvent& e)
+{
+	if (e.current_real - vpe.header.stamp < publish_zero_timout) return; // have vpe
+
+	if (e.current_real - pose.header.stamp < publish_zero_timout) { // have local position
+		if (got_local_pos.isZero()) {
+			ROS_INFO("got local position");
+			got_local_pos = e.current_real;
+		}
+
+		if (e.current_real - got_local_pos > publish_zero_duration) return; // stop publishing zero
+	} else {
+		// lost local position
+		got_local_pos = ros::Time(0);
+	}
+
+	ROS_INFO_THROTTLE(10, "publish zero");
+	static geometry_msgs::PoseStamped zero;
+	zero.header.frame_id = local_frame_id;
+	zero.header.stamp = e.current_real;
+	zero.pose.orientation.w = 1;
+	vpe_pub.publish(zero);
+}
+
+void localPositionCallback(const PoseStamped& msg) { pose = msg; }
+
+inline Pose getPose(const PoseStampedConstPtr& pose) { return pose->pose; }
+
+inline Pose getPose(const PoseWithCovarianceStampedConstPtr& pose) { return pose->pose.pose; }
+
+template <typename T>
+void callback(const T& msg)
+{
+	static tf2_ros::StaticTransformBroadcaster br;
+
+	try {
+		if (!frame_id.empty()) {
+			// get VPE transform from TF
+			auto transform = tf_buffer.lookupTransform(frame_id, child_frame_id,
+													msg->header.stamp, ros::Duration(0.02));
+			vpe.pose.position.x = transform.transform.translation.x;
+			vpe.pose.position.y = transform.transform.translation.y;
+			vpe.pose.position.z = transform.transform.translation.z;
+			vpe.pose.orientation = transform.transform.rotation;
+		} else {
+			vpe.pose = getPose(msg);
+		}
+
+		// offset
+		if (!offset_frame_id.empty()) {
+			if (msg->header.stamp - vpe.header.stamp > offset_timeout) {
+				// calculate the offset
+				offset = tf_buffer.lookupTransform(local_frame_id, frame_id,
+				                                   msg->header.stamp, ros::Duration(0.02));
+				// offset.header.frame_id = vpe.header.frame_id;
+				offset.child_frame_id = offset_frame_id;
+				br.sendTransform(offset);
+				ROS_INFO("offset reset");
+			}
+			// apply the offset
+			tf2::doTransform(vpe, vpe, offset);
+		}
+
+		vpe.header.frame_id = local_frame_id;
+		vpe.header.stamp = msg->header.stamp;
+		vpe_pub.publish(vpe);
+
+	} catch (const tf2::TransformException& e) {
+		ROS_WARN_THROTTLE(5, "%s", e.what());
+	}
+}
+
+int main(int argc, char **argv) {
+	ros::init(argc, argv, "vpe_publisher");
+	ros::NodeHandle nh, nh_priv("~");
+
+	tf2_ros::TransformListener tf_listener(tf_buffer);
+
+	nh_priv.param<string>("frame_id", frame_id, "");
+	nh_priv.param<string>("offset_frame_id", offset_frame_id, "");
+	nh_priv.param<string>("mavros/local_position/frame_id", local_frame_id, "map");
+	nh_priv.param<string>("mavros/local_position/tf/child_frame_id", child_frame_id, "base_link");
+	offset_timeout = ros::Duration(nh_priv.param("offset_timeout", 3.0));
+
+	if (!frame_id.empty()) {
+		ROS_INFO("using data from TF");
+	} else {
+		ROS_INFO("using data topic");
+	}
+
+	auto pose_sub = nh_priv.subscribe<PoseStamped>("pose", 1, &callback);
+	auto pose_cov_sub = nh_priv.subscribe<PoseWithCovarianceStamped>("pose_cov", 1, &callback);
+	//auto markers_sub = nh_priv.subscribe<aruco_pose::MarkerArray>("markers", 1, &callback);
+
+	vpe_pub = nh_priv.advertise<PoseStamped>("vpe", 1);
+	//vpe_cov_pub = nh_priv_.advertise<PoseStamped>("pose_cov_pub", 1);
+
+	if (nh_priv.param("publish_zero", false)) {
+		// publish zero to initialize the local position
+		zero_timer = nh.createTimer(ros::Duration(0.1), &publishZero);
+		publish_zero_timout = ros::Duration(nh_priv.param("publish_zero_timout", 5.0));
+		publish_zero_duration = ros::Duration(nh_priv.param("publish_zero_duration", 5.0));
+		local_position_sub = nh.subscribe("mavros/local_position/pose", 1, &localPositionCallback);
+	}
+
+	ROS_INFO("ready");
+	ros::spin();
+}

clever/srv/SetLEDEffect.srv

@@ -0,0 +1,7 @@
+string effect
+uint8 r
+uint8 g
+uint8 b
+---
+bool success
+string message

clever/test/basic.py

@@ -0,0 +1,29 @@
+#!/usr/bin/env python
+import rospy
+import pytest
+from mavros_msgs.msg import State
+
+@pytest.fixture()
+def node():
+    return rospy.init_node('clever_test', anonymous=True)
+
+def test_state(node):
+    state = rospy.wait_for_message('mavros/state', State, timeout=10)
+    assert state.connected == False
+    assert state.armed == False
+    assert state.guided == False
+    assert state.mode == ''
+
+def test_simple_offboard_services_available():
+    rospy.wait_for_service('get_telemetry', timeout=5)
+    rospy.wait_for_service('navigate', timeout=5)
+    rospy.wait_for_service('navigate_global', timeout=5)
+    rospy.wait_for_service('set_position', timeout=5)
+    rospy.wait_for_service('set_velocity', timeout=5)
+    rospy.wait_for_service('set_attitude', timeout=5)
+    rospy.wait_for_service('set_rates', timeout=5)
+    rospy.wait_for_service('land', timeout=5)
+
+def test_web_video_server(node):
+    import urllib2
+    urllib2.urlopen("http://localhost:8080").read()

clever/test/basic.test

@@ -0,0 +1,41 @@
+<launch>
+    <!-- Verify all the required nodes basically work -->
+
+    <node pkg="mavros" type="mavros_node" name="mavros" required="true" output="screen">
+        <param name="fcu_url" value="udp://@127.0.1:14557"/>
+        <rosparam command="load" file="$(find clever)/launch/mavros_config.yaml"/>
+    </node>
+
+    <node name="visualization" pkg="mavros_extras" type="visualization" required="true">
+        <remap to="mavros/local_position/pose" from="local_position"/>
+        <remap to="mavros/setpoint_position/local" from="local_setpoint"/>
+        <param name="fixed_frame_id" value="map"/>
+        <param name="child_frame_id" value="base_link"/>
+        <param name="marker_scale" value="1"/>
+        <param name="max_track_size" value="20"/>
+        <param name="num_rotors" value="4"/>
+    </node>
+
+    <node name="web_video_server" pkg="web_video_server" type="web_video_server" required="true" output="screen">
+        <param name="default_stream_type" value="ros_compressed"/>
+        <param name="publish_rate" value="1.0"/>
+    </node>
+
+    <node pkg="tf2_ros" type="static_transform_publisher" name="map_flipped_frame" args="0 0 0 3.1415926 3.1415926 0 map map_flipped" required="true"/>
+
+    <node name="simple_offboard" pkg="clever" type="simple_offboard" required="true" output="screen">
+        <param name="reference_frames/body" value="map"/>
+        <param name="reference_frames/base_link" value="map"/>
+    </node>
+
+    <node name="tf2_web_republisher" pkg="tf2_web_republisher" type="tf2_web_republisher" required="true"/>
+
+    <node name="rc" pkg="clever" type="rc" required="true" output="screen"/>
+
+    <node pkg="clever" name="led_effect" type="led" ns="led" clear_params="true" output="screen" required="true">
+        <rosparam param="notify">startup: { r: 255, g: 255, b: 255 }</rosparam>
+    </node>
+
+    <param name="test_module" value="$(find clever)/test/basic.py"/>
+    <test test-name="basic_test" pkg="ros_pytest" type="ros_pytest_runner"/>
+</launch>

clever/www/aruco_map.html

@@ -0,0 +1,19 @@
+<!DOCTYPE html>
+<html>
+	<head>
+		<script type="text/javascript" src="js/three.min.js"></script>
+		<script type="text/javascript" src="js/eventemitter2.js"></script>
+		<script type="text/javascript" src="js/roslib.js"></script>
+		<script type="text/javascript" src="js/ros3d.js"></script>
+		<title>Aruco Map visualization</title>
+	</head>
+	<body>
+		<div id="viz"></div>
+		<script type="text/javascript" src="js/viz.js"></script>
+		<script>
+			setScene('aruco_map');
+			addArucoMap();
+			addAxes();
+		</script>
+	</body>
+</html>

clever/www/docs

@@ -0,0 +1 @@
+../../_book/

clever/www/gcs.html

@@ -0,0 +1,29 @@
+<html>
+	<head>
+		<title>Disconnected</title>
+		<script src="js/roslib.js"></script>
+		<style>
+			body {
+				font-family: sans-serif;
+				background: rgba(0, 0, 0, 0.8);
+				color: rgba(255, 255, 255, 0.9);
+			}
+			.dash {
+				font-size: 30px;
+				text-align: center;
+				position: absolute;
+				left: 50%;
+				top: 50%;
+				transform: translate(-50%, -50%);
+				line-height: 150%;
+			}
+		</style>
+	</head>
+	<body>
+		<div class="dash">
+			<div class="mode">&nbsp;</div>
+			<div class="battery">&nbsp;</div>
+		</div>
+	</body>
+	<script src="js/gcs.js"></script>
+</html>

clever/www/index.html

@@ -1,11 +1,11 @@
 <h1>CLEVER Drone Kit Tools</h1>
 
 <ul>
-	<!-- <li><a href="">View user reference</a> (<a href="http://clever.copterexpress.com">http://clever.copterexpress.com</a> snapshot)</li> -->
+	<li><a href="docs">View documentation</a> (snapshot of <a href="http://clever.copterexpress.com">clever.copterexpress.com</a>)</li>
 	<li><a href="" id="wvs">View image topics</a> (<code>web_video_server</code>)</li>
 	<li><a href="" id="butterfly">Open web terminal</a> (<code>Butterfly</code>)</li>
-	<li><a href="/docs">Documentation</a> (<code>gitbook</code>)</li>
-	<!-- <li><a href="viz.html">View 3D visualization</a> (<code>ros3djs</code>)</li> -->
+	<li><a href="viz.html">View 3D visualization</a> (<code>ros3djs</code>)</li>
+	<li><a href="aruco_map.html">3D visualization for markers map</a> (<code>ros3djs</code>)</li>
 </ul>
 
 <script type="text/javascript">

clever/www/js/eventemitter2.js

@@ -0,0 +1,573 @@
+/*!
+ * EventEmitter2
+ * https://github.com/hij1nx/EventEmitter2
+ *
+ * Copyright (c) 2013 hij1nx
+ * Licensed under the MIT license.
+ */
+;!function(undefined) {
+
+  var isArray = Array.isArray ? Array.isArray : function _isArray(obj) {
+    return Object.prototype.toString.call(obj) === "[object Array]";
+  };
+  var defaultMaxListeners = 10;
+
+  function init() {
+    this._events = {};
+    if (this._conf) {
+      configure.call(this, this._conf);
+    }
+  }
+
+  function configure(conf) {
+    if (conf) {
+
+      this._conf = conf;
+
+      conf.delimiter && (this.delimiter = conf.delimiter);
+      conf.maxListeners && (this._events.maxListeners = conf.maxListeners);
+      conf.wildcard && (this.wildcard = conf.wildcard);
+      conf.newListener && (this.newListener = conf.newListener);
+
+      if (this.wildcard) {
+        this.listenerTree = {};
+      }
+    }
+  }
+
+  function EventEmitter(conf) {
+    this._events = {};
+    this.newListener = false;
+    configure.call(this, conf);
+  }
+
+  //
+  // Attention, function return type now is array, always !
+  // It has zero elements if no any matches found and one or more
+  // elements (leafs) if there are matches
+  //
+  function searchListenerTree(handlers, type, tree, i) {
+    if (!tree) {
+      return [];
+    }
+    var listeners=[], leaf, len, branch, xTree, xxTree, isolatedBranch, endReached,
+        typeLength = type.length, currentType = type[i], nextType = type[i+1];
+    if (i === typeLength && tree._listeners) {
+      //
+      // If at the end of the event(s) list and the tree has listeners
+      // invoke those listeners.
+      //
+      if (typeof tree._listeners === 'function') {
+        handlers && handlers.push(tree._listeners);
+        return [tree];
+      } else {
+        for (leaf = 0, len = tree._listeners.length; leaf < len; leaf++) {
+          handlers && handlers.push(tree._listeners[leaf]);
+        }
+        return [tree];
+      }
+    }
+
+    if ((currentType === '*' || currentType === '**') || tree[currentType]) {
+      //
+      // If the event emitted is '*' at this part
+      // or there is a concrete match at this patch
+      //
+      if (currentType === '*') {
+        for (branch in tree) {
+          if (branch !== '_listeners' && tree.hasOwnProperty(branch)) {
+            listeners = listeners.concat(searchListenerTree(handlers, type, tree[branch], i+1));
+          }
+        }
+        return listeners;
+      } else if(currentType === '**') {
+        endReached = (i+1 === typeLength || (i+2 === typeLength && nextType === '*'));
+        if(endReached && tree._listeners) {
+          // The next element has a _listeners, add it to the handlers.
+          listeners = listeners.concat(searchListenerTree(handlers, type, tree, typeLength));
+        }
+
+        for (branch in tree) {
+          if (branch !== '_listeners' && tree.hasOwnProperty(branch)) {
+            if(branch === '*' || branch === '**') {
+              if(tree[branch]._listeners && !endReached) {
+                listeners = listeners.concat(searchListenerTree(handlers, type, tree[branch], typeLength));
+              }
+              listeners = listeners.concat(searchListenerTree(handlers, type, tree[branch], i));
+            } else if(branch === nextType) {
+              listeners = listeners.concat(searchListenerTree(handlers, type, tree[branch], i+2));
+            } else {
+              // No match on this one, shift into the tree but not in the type array.
+              listeners = listeners.concat(searchListenerTree(handlers, type, tree[branch], i));
+            }
+          }
+        }
+        return listeners;
+      }
+
+      listeners = listeners.concat(searchListenerTree(handlers, type, tree[currentType], i+1));
+    }
+
+    xTree = tree['*'];
+    if (xTree) {
+      //
+      // If the listener tree will allow any match for this part,
+      // then recursively explore all branches of the tree
+      //
+      searchListenerTree(handlers, type, xTree, i+1);
+    }
+
+    xxTree = tree['**'];
+    if(xxTree) {
+      if(i < typeLength) {
+        if(xxTree._listeners) {
+          // If we have a listener on a '**', it will catch all, so add its handler.
+          searchListenerTree(handlers, type, xxTree, typeLength);
+        }
+
+        // Build arrays of matching next branches and others.
+        for(branch in xxTree) {
+          if(branch !== '_listeners' && xxTree.hasOwnProperty(branch)) {
+            if(branch === nextType) {
+              // We know the next element will match, so jump twice.
+              searchListenerTree(handlers, type, xxTree[branch], i+2);
+            } else if(branch === currentType) {
+              // Current node matches, move into the tree.
+              searchListenerTree(handlers, type, xxTree[branch], i+1);
+            } else {
+              isolatedBranch = {};
+              isolatedBranch[branch] = xxTree[branch];
+              searchListenerTree(handlers, type, { '**': isolatedBranch }, i+1);
+            }
+          }
+        }
+      } else if(xxTree._listeners) {
+        // We have reached the end and still on a '**'
+        searchListenerTree(handlers, type, xxTree, typeLength);
+      } else if(xxTree['*'] && xxTree['*']._listeners) {
+        searchListenerTree(handlers, type, xxTree['*'], typeLength);
+      }
+    }
+
+    return listeners;
+  }
+
+  function growListenerTree(type, listener) {
+
+    type = typeof type === 'string' ? type.split(this.delimiter) : type.slice();
+
+    //
+    // Looks for two consecutive '**', if so, don't add the event at all.
+    //
+    for(var i = 0, len = type.length; i+1 < len; i++) {
+      if(type[i] === '**' && type[i+1] === '**') {
+        return;
+      }
+    }
+
+    var tree = this.listenerTree;
+    var name = type.shift();
+
+    while (name) {
+
+      if (!tree[name]) {
+        tree[name] = {};
+      }
+
+      tree = tree[name];
+
+      if (type.length === 0) {
+
+        if (!tree._listeners) {
+          tree._listeners = listener;
+        }
+        else if(typeof tree._listeners === 'function') {
+          tree._listeners = [tree._listeners, listener];
+        }
+        else if (isArray(tree._listeners)) {
+
+          tree._listeners.push(listener);
+
+          if (!tree._listeners.warned) {
+
+            var m = defaultMaxListeners;
+
+            if (typeof this._events.maxListeners !== 'undefined') {
+              m = this._events.maxListeners;
+            }
+
+            if (m > 0 && tree._listeners.length > m) {
+
+              tree._listeners.warned = true;
+              console.error('(node) warning: possible EventEmitter memory ' +
+                            'leak detected. %d listeners added. ' +
+                            'Use emitter.setMaxListeners() to increase limit.',
+                            tree._listeners.length);
+              console.trace();
+            }
+          }
+        }
+        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) {
+    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; }
+    }
+
+    // Loop through the *_all* functions and invoke them.
+    if (this._all) {
+      var l = arguments.length;
+      var args = new Array(l - 1);
+      for (var i = 1; i < l; i++) args[i - 1] = arguments[i];
+      for (i = 0, l = this._all.length; i < l; i++) {
+        this.event = type;
+        this._all[i].apply(this, args);
+      }
+    }
+
+    // If there is no 'error' event listener then throw.
+    if (type === 'error') {
+
+      if (!this._all &&
+        !this._events.error &&
+        !(this.wildcard && this.listenerTree.error)) {
+
+        if (arguments[1] instanceof Error) {
+          throw arguments[1]; // Unhandled 'error' event
+        } else {
+          throw new Error("Uncaught, unspecified 'error' event.");
+        }
+        return false;
+      }
+    }
+
+    var handler;
+
+    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;
+      if (arguments.length === 1) {
+        handler.call(this);
+      }
+      else if (arguments.length > 1)
+        switch (arguments.length) {
+          case 2:
+            handler.call(this, arguments[1]);
+            break;
+          case 3:
+            handler.call(this, arguments[1], arguments[2]);
+            break;
+          // slower
+          default:
+            var l = arguments.length;
+            var args = new Array(l - 1);
+            for (var i = 1; i < l; i++) args[i - 1] = arguments[i];
+            handler.apply(this, args);
+        }
+      return true;
+    }
+    else if (handler) {
+      var l = arguments.length;
+      var args = new Array(l - 1);
+      for (var i = 1; i < l; i++) args[i - 1] = arguments[i];
+
+      var listeners = handler.slice();
+      for (var i = 0, l = listeners.length; i < l; i++) {
+        this.event = type;
+        listeners[i].apply(this, args);
+      }
+      return (listeners.length > 0) || !!this._all;
+    }
+    else {
+      return !!this._all;
+    }
+
+  };
+
+  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') {
+      // Adding the second element, need to change to array.
+      this._events[type] = [this._events[type], listener];
+    }
+    else if (isArray(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) {
+
+        var m = defaultMaxListeners;
+
+        if (typeof this._events.maxListeners !== 'undefined') {
+          m = this._events.maxListeners;
+        }
+
+        if (m > 0 && this._events[type].length > m) {
+
+          this._events[type].warned = true;
+          console.error('(node) warning: possible EventEmitter memory ' +
+                        'leak detected. %d listeners added. ' +
+                        'Use emitter.setMaxListeners() to increase limit.',
+                        this._events[type].length);
+          console.trace();
+        }
+      }
+    }
+    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<leafs.length; iLeaf++) {
+      var leaf = leafs[iLeaf];
+      handlers = leaf._listeners;
+      if (isArray(handlers)) {
+
+        var position = -1;
+
+        for (var i = 0, length = handlers.length; i < length; i++) {
+          if (handlers[i] === listener ||
+            (handlers[i].listener && handlers[i].listener === listener) ||
+            (handlers[i]._origin && handlers[i]._origin === listener)) {
+            position = i;
+            break;
+          }
+        }
+
+        if (position < 0) {
+          continue;
+        }
+
+        if(this.wildcard) {
+          leaf._listeners.splice(position, 1);
+        }
+        else {
+          this._events[type].splice(position, 1);
+        }
+
+        if (handlers.length === 0) {
+          if(this.wildcard) {
+            delete leaf._listeners;
+          }
+          else {
+            delete this._events[type];
+          }
+        }
+        return this;
+      }
+      else if (handlers === listener ||
+        (handlers.listener && handlers.listener === listener) ||
+        (handlers._origin && handlers._origin === listener)) {
+        if(this.wildcard) {
+          delete leaf._listeners;
+        }
+        else {
+          delete this._events[type];
+        }
+      }
+    }
+
+    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);
+          return this;
+        }
+      }
+    } else {
+      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<leafs.length; iLeaf++) {
+        var leaf = leafs[iLeaf];
+        leaf._listeners = null;
+      }
+    }
+    else {
+      if (!this._events[type]) return this;
+      this._events[type] = null;
+    }
+    return this;
+  };
+
+  EventEmitter.prototype.listeners = function(type) {
+    if(this.wildcard) {
+      var handlers = [];
+      var ns = typeof type === 'string' ? type.split(this.delimiter) : type.slice();
+      searchListenerTree.call(this, handlers, ns, this.listenerTree, 0);
+      return handlers;
+    }
+
+    this._events || init.call(this);
+
+    if (!this._events[type]) this._events[type] = [];
+    if (!isArray(this._events[type])) {
+      this._events[type] = [this._events[type]];
+    }
+    return this._events[type];
+  };
+
+  EventEmitter.prototype.listenersAny = function() {
+
+    if(this._all) {
+      return this._all;
+    }
+    else {
+      return [];
+    }
+
+  };
+
+  if (typeof define === 'function' && define.amd) {
+     // AMD. Register as an anonymous module.
+    define(function() {
+      return EventEmitter;
+    });
+  } else if (typeof exports === 'object') {
+    // CommonJS
+    exports.EventEmitter2 = EventEmitter;
+  }
+  else {
+    // Browser global.
+    window.EventEmitter2 = EventEmitter;
+  }
+}();

clever/www/js/gcs.js

@@ -0,0 +1,69 @@
+var body = document.querySelector('body');
+var titleEl = document.querySelector('title');
+var modeEl = document.querySelector('.mode');
+var batteryEl = document.querySelector('.battery');
+
+var url = 'ws://' + location.hostname + ':9090';
+var ros = new ROSLIB.Ros({ url: url });
+
+function speak(txt) {
+	var utterance = new SpeechSynthesisUtterance(txt);
+	window.speechSynthesis.speak(utterance);
+}
+
+ros.on('connection', function () {
+	body.classList.add('connected');
+	titleEl.innerText = 'Connected';
+});
+
+ros.on('close', function () {
+	titleEl.innerText = 'Disconnected';
+	modeEl.innerHTML = '';
+	body.classList.remove('connected');
+	setTimeout(function() {
+		titleEl.innerText = 'Reconnecting';
+		ros.connect(url);
+	}, 2000);
+});
+
+var fcuState = {};
+
+new ROSLIB.Topic({
+	ros: ros,
+	name: '/mavros/state',
+	messageType: 'mavros_msgs/State'
+}).subscribe(function(msg) {
+	modeEl.innerHTML = msg.mode;
+	if (fcuState.mode != msg.mode) {
+		// mode changed
+		speak(msg.mode + ' flight mode');
+	}
+	fcuState = msg;
+});
+
+new ROSLIB.Topic({
+	ros: ros,
+	name: '/mavros/statustext/recv',
+	messageType: 'mavros_msgs/StatusText'
+}).subscribe(function(message) {
+	var BLACKLIST = [];
+	if (message.severity <= 4) {
+		if (BLACKLIST.some(function(e) {
+				return message.text.indexOf(e) != -1;
+			})) {
+			console.log('Filtered out message ' + message.text);
+			return;
+		}
+		speak(message.text);
+	}
+});
+
+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') || '';
+});