Add articles on new aruco navigation to gitbook

`cv::aruco::drawAxis` would attempt to draw detected frame origin even when it's behind the camera. This resulted in an invalid, flipped frame displayed in `/aruco_map/debug`.

This commit prevents drawing frame axis if the frame origin (projected to the screen space) is behind the screen plane.

.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

@@ -15,15 +15,59 @@
         "names": [
             "MAVLink",
             "ROS",
+            "ROS Kinetic",
+            "OpenCV",
+            "GitHub",
+            "FPV",
+            "PPM",
+            "PWM",
             "Python",
             "C++",
             "PX4",
+            "QGroundControl",
+            "QGC",
             "WireShark",
+            "FlightPlot",
+            "OFFBOARD",
+            "LPE",
+            "EKF2",
+            "SITL",
+            "PID",
             "Wi-Fi",
             "Raspberry Pi",
+            "RPi",
+            "Linux",
+            "Windows",
+            "macOS",
+            "iOS",
+            "Android",
+            "Bluetooth",
+            "Raspbian",
+            "Raspbian Jesse",
+            "Raspbian Stretch",
             "Pixhawk",
             "Pixracer",
-            "ArUco"
+            "Arduino",
+            "GPS",
+            "ArUco",
+            "LIRC",
+            "GPIO",
+            "HC-SR04",
+            "STM",
+            "LED",
+            "USB",
+            "FAT32",
+            "uORB",
+            "SSH",
+            "API",
+            "UART",
+            "GND",
+            "VCC",
+            "SCL",
+            "SDA",
+            "TCP",
+            "UDP",
+            "QR"
         ],
         "code_blocks": false
     },

.travis.yml

@@ -4,30 +4,81 @@ 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:
+        - docker run --privileged --rm -v /dev:/dev -v $(pwd):/builder/repo -e TRAVIS_TAG="${TRAVIS_TAG}" ${DOCKER}
+      before_cache:
+        - cp images/*.zip imgcache
+      before_deploy:
+        # Set up git user name and tag this commit
+        - git config --local user.name "goldarte"
+        - git config --local user.email "goldartt@gmail.com"
+        - sudo chmod -R 777 *
+        - cd images && zip ${IMAGE_NAME}.zip ${IMAGE_NAME}
+      deploy:
+        provider: releases
+        api_key: ${GITHUB_OAUTH_TOKEN}
+        file: ${IMAGE_NAME}.zip
+        skip_cleanup: true
+        on:
+          tags: true
+        draft: true
+        name: ${TRAVIS_TAG}
+    - stage: Build
+      name: "Documentation"
+      language: node_js
+      node_js:
+        - "10"
+      before_script:
+        - npm install gitbook-cli -g
+        - npm install markdownlint-cli -g
+        - gitbook -V
+        - markdownlint -V
+      script:
+        - markdownlint docs
+        - gitbook install
+        - gitbook build
+# ***
+# Disable deployments for now, revisit this later
+#   --sfalexrog, 06.02.2019
+# ***
+#      deploy:
+#        provider: pages
+#        local-dir: _book
+#        skip-cleanup: true
+#        github-token: ${GITHUB_OAUTH_TOKEN}
+#        keep-history: true
+#        target-branch: gh-pages
+#        on:
+#          branch: WIP/gitbook-autobuild
+    - stage: Annotate
+      name: Auto-generate changelog
+      language: python
+      python: 3.6
+      install:
+        - pip install GitPython PyGithub
+      script:
+        - PYTHONUNBUFFERED=1 python ./gen_changelog.py
+stages:
+  - Build
+  - Annotate
 # More info there
 # https://github.com/travis-ci/travis-ci/issues/6893
 # https://docs.travis-ci.com/user/customizing-the-build/

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}
 )
@@ -210,3 +212,8 @@ 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)
+endif()

aruco_pose/README.md

@@ -0,0 +1,119 @@
+# 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)
+
+Map file has one marker per line with the following line format:
+
+```
+marker_id marker_length x y z yaw pitch roll
+```
+
+Where yaw, pitch and roll are extrinsic rotation around Z, Y, X axis, respectively.
+
+See examples in [`map`](map/) directory.
+
+### Topics
+
+#### Subscribed
+
+* `image_raw` (*sensor_msgs/Image*) – camera image (used for debug image)
+* `camera_info` (*sensor_msgs/CameraInfo*) – camera calibration info (used for debug image)
+* `markers` (*aruco_pose/MarkerArray*) – list of markers detected by `aruco_pose` nodelet
+
+#### Published
+
+* `~pose` (*geometry_msgs/PoseWithCovarianceStamped*) – estimated map pose
+* `~image` (*sensor_msgs/Image*) – planarized map image
+* `~visualization` (*visualization_msgs/MarkerArray*) – markers map visualization for rviz
+* `~debug` (*sensor_msgs/Image*) – debug image with detected markers and map axis
+
+### Published transforms
+
+* `<camera_frame>` => `<map_name>` – markers map pose
+
+## Running tests
+
+Command for running tests:
+
+```bash
+rostest aruco_pose basic.test
+```
+
+## Copyright
+
+Copyright © 2018 Copter Express Technologies. Author: Oleg Kalachev.
+
+Distributed under MIT License (https://opensource.org/licenses/MIT).

aruco_pose/launch/sample.launch

@@ -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/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,35 @@
 <?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>
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>

aruco_pose/src/aruco_detect.cpp

@@ -0,0 +1,330 @@
+/*
+ * 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 <ros/ros.h>
+#include <nodelet/nodelet.h>
+#include <pluginlib/class_list_macros.h>
+#include <tf/transform_datatypes.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <image_transport/image_transport.h>
+#include <cv_bridge/cv_bridge.h>
+#include <geometry_msgs/Vector3.h>
+#include <geometry_msgs/Pose.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
+#include <geometry_msgs/TransformStamped.h>
+#include <visualization_msgs/Marker.h>
+#include <visualization_msgs/MarkerArray.h>
+
+#include <opencv2/opencv.hpp>
+#include <opencv2/highgui.hpp>
+#include <opencv2/aruco.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/calib3d/calib3d.hpp>
+
+#include <aruco_pose/Marker.h>
+#include <aruco_pose/MarkerArray.h>
+
+#include "utils.h"
+
+using std::vector;
+using cv::Mat;
+
+class ArucoDetect : public nodelet::Nodelet {
+private:
+	ros::NodeHandle nh_, nh_priv_;
+	tf2_ros::TransformBroadcaster br_;
+	tf2_ros::Buffer tf_buffer_;
+	tf2_ros::TransformListener tf_listener_{tf_buffer_};
+	cv::Ptr<cv::aruco::Dictionary> dictionary_;
+	cv::Ptr<cv::aruco::DetectorParameters> parameters_;
+	image_transport::Publisher debug_pub_;
+	image_transport::CameraSubscriber img_sub_;
+	ros::Publisher markers_pub_, vis_markers_pub_;
+	bool estimate_poses_, send_tf_, 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_;
+	visualization_msgs::MarkerArray vis_array_;
+
+public:
+	virtual void onInit()
+	{
+		nh_ = getNodeHandle();
+		nh_priv_ = getPrivateNodeHandle();
+
+		int dictionary;
+		nh_priv_.param("dictionary", dictionary, 2);
+		nh_priv_.param("estimate_poses", estimate_poses_, true);
+		nh_priv_.param("send_tf", send_tf_, true);
+		if (estimate_poses_ && !nh_priv_.getParam("length", length_)) {
+			ROS_FATAL("aruco_detect: can't estimate marker's poses as ~length parameter is not defined");
+			ros::shutdown();
+		}
+		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_);
+
+		debug_pub_ = it_priv.advertise("debug", 1);
+		markers_pub_ = nh_priv_.advertise<aruco_pose::MarkerArray>("markers", 1);
+		vis_markers_pub_ = nh_priv_.advertise<visualization_msgs::MarkerArray>("visualization", 1);
+		img_sub_ = it.subscribeCamera("image_raw", 1, &ArucoDetect::imageCallback, this);
+
+		ROS_INFO("aruco_detect: ready");
+	}
+
+private:
+	void imageCallback(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr &cinfo)
+	{
+		Mat image = cv_bridge::toCvShare(msg, "bgr8")->image;
+
+		vector<int> ids;
+		vector<vector<cv::Point2f>> corners, rejected;
+		vector<cv::Vec3d> rvecs, tvecs;
+		vector<cv::Point3f> obj_points;
+		geometry_msgs::TransformStamped snap_to;
+
+		// Detect markers
+		cv::aruco::detectMarkers(image, dictionary_, corners, ids, parameters_, rejected);
+
+		array_.header.stamp = msg->header.stamp;
+		array_.header.frame_id = msg->header.frame_id;
+		array_.markers.clear();
+
+		if (ids.size() != 0) {
+			parseCameraInfo(cinfo, camera_matrix_, dist_coeffs_);
+
+			// Estimate individual markers' poses
+			if (estimate_poses_) {
+				cv::aruco::estimatePoseSingleMarkers(corners, length_, camera_matrix_, dist_coeffs_,
+				                                     rvecs, tvecs);
+
+				// 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) {
+						ROS_WARN_THROTTLE(5, "aruco_detect: can't snap: %s", e.what());
+					}
+				}
+			}
+
+			array_.markers.reserve(ids.size());
+			aruco_pose::Marker marker;
+			geometry_msgs::TransformStamped transform;
+			transform.header.stamp = msg->header.stamp;
+			transform.header.frame_id = msg->header.frame_id;
+
+			for (unsigned int i = 0; i < ids.size(); i++) {
+				marker.id = ids[i];
+				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]);
+						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_;
+		}
+	}
+};
+
+PLUGINLIB_EXPORT_CLASS(ArucoDetect, nodelet::Nodelet)

aruco_pose/src/aruco_map.cpp

@@ -0,0 +1,421 @@
+/*
+ * 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 <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_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_, 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_;
+	tf2_ros::TransformBroadcaster br_;
+	tf2_ros::Buffer tf_buffer_;
+	tf2_ros::TransformListener tf_listener_{tf_buffer_};
+	visualization_msgs::MarkerArray vis_array_;
+	std::string known_tilt_;
+	int image_width_, image_height_, image_margin_;
+	bool auto_flip_;
+
+public:
+	virtual void onInit()
+	{
+		nh_ = getNodeHandle();
+		nh_priv_ = getPrivateNodeHandle();
+
+		image_transport::ImageTransport it_priv(nh_priv_);
+
+		// TODO: why image_transport doesn't work here?
+		img_pub_ = nh_priv_.advertise<sensor_msgs::Image>("image", 1, true);
+
+		board_ = cv::makePtr<cv::aruco::Board>();
+		board_->dictionary = cv::aruco::getPredefinedDictionary(
+			                 static_cast<cv::aruco::PREDEFINED_DICTIONARY_NAME>(nh_priv_.param("dictionary", 2)));
+		camera_matrix_ = cv::Mat::zeros(3, 3, CV_64F);
+		dist_coeffs_ = cv::Mat::zeros(8, 1, CV_64F);
+
+		std::string type, map;
+		nh_priv_.param<std::string>("type", type, "map");
+		nh_priv_.param<std::string>("frame_id", transform_.child_frame_id, "aruco_map");
+		nh_priv_.param<std::string>("known_tilt", known_tilt_, "");
+		nh_priv_.param("auto_flip", auto_flip_, false);
+		nh_priv_.param("image_width", image_width_, 2000);
+		nh_priv_.param("image_height", image_height_, 2000);
+		nh_priv_.param("image_margin", image_margin_, 200);
+
+		// createStripLine();
+
+		if (type == "map") {
+			param(nh_priv_, "map", map);
+			loadMap(map);
+		} else if (type == "gridboard") {
+			createGridBoard();
+		} else {
+			ROS_FATAL("aruco_map: 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));
+
+		publishMapImage();
+		vis_markers_pub_.publish(vis_array_);
+
+		ROS_INFO("aruco_map: 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) {
+				ROS_WARN_THROTTLE(1, "aruco_map: can't snap: %s", e.what());
+			}
+
+			geometry_msgs::TransformStamped shift;
+			shift.transform.translation.x = -center_x;
+			shift.transform.translation.y = -center_y;
+			shift.transform.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()) {
+			ROS_FATAL("aruco_map: %s - %s", strerror(errno), filename.c_str());
+			ros::shutdown();
+		}
+
+		while (std::getline(f, line)) {
+			int id;
+			double length, x, y, z, yaw, pitch, roll;
+
+			std::istringstream s(line);
+
+			if (!(s >> id >> length >> x >> y >> z >> yaw >> pitch >> roll)) {
+				ROS_ERROR("aruco_map: cannot parse line: %s", line.c_str());
+				continue;
+			}
+			addMarker(id, length, x, y, z, yaw, pitch, roll);
+		}
+
+		ROS_INFO("aruco_map: loading %s complete (%d markers)", filename.c_str(), static_cast<int>(board_->ids.size()));
+	}
+
+	void createGridBoard()
+	{
+		ROS_INFO("aruco_map: generate gridboard");
+		ROS_WARN("aruco_map: gridboard maps are deprecated");
+
+		int markers_x, markers_y, first_marker;
+		double markers_side, markers_sep_x, markers_sep_y;
+		std::vector<int> marker_ids;
+		nh_priv_.param<int>("markers_x", markers_x, 10);
+		nh_priv_.param<int>("markers_y", markers_y, 10);
+		nh_priv_.param<int>("first_marker", first_marker, 0);
+
+		param(nh_priv_, "markers_side", markers_side);
+		param(nh_priv_, "markers_sep_x", markers_sep_x);
+		param(nh_priv_, "markers_sep_y", markers_sep_y);
+
+		if (nh_priv_.getParam("marker_ids", marker_ids)) {
+			if ((unsigned int)(markers_x * markers_y) != marker_ids.size()) {
+				ROS_FATAL("~marker_ids length should be equal to ~markers_x * ~markers_y");
+				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;
+				ROS_INFO("add marker %d %g %g", marker_ids[y * markers_y + x], x_pos, y_pos);
+				addMarker(marker_ids[y * markers_y + x], markers_side, x_pos, y_pos, 0, 0, 0, 0);
+			}
+		}
+	}
+
+	// void createStripLine()
+	// {
+	// 	visualization_msgs::Marker marker;
+	// 	marker.header.frame_id = transform_.child_frame_id;
+	// 	marker.action = visualization_msgs::Marker::ADD;
+	// 	marker.ns = "aruco_map_link";
+	// 	marker.type = visualization_msgs::Marker::LINE_STRIP;
+	// 	marker.scale.x = 0.02;
+	// 	marker.color.g = 1;
+	// 	marker.color.a = 0.8;
+	// 	marker.frame_locked = true;
+	// 	marker.pose.orientation.w = 1;
+	// 	vis_array_.markers.push_back(marker);
+	// }
+
+	void addMarker(int id, double length, double x, double y, double z,
+				   double yaw, double pitch, double roll)
+	{
+		// 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 visualization marker
+		visualization_msgs::Marker marker;
+		marker.header.frame_id = transform_.child_frame_id;
+		// marker.header.stamp = stamp;
+		marker.action = visualization_msgs::Marker::ADD;
+		marker.id = vis_array_.markers.size();
+		marker.ns = "aruco_map_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 = 0.5;
+		marker.color.b = 0.5;
+		marker.color.a = 0.8;
+		marker.pose.position.x = x;
+		marker.pose.position.y = y;
+		marker.pose.position.z = z;
+		tf::quaternionTFToMsg(q, marker.pose.orientation);
+		marker.frame_locked = true;
+		vis_array_.markers.push_back(marker);
+
+		// Add linking line
+		// geometry_msgs::Point p;
+		// p.x = x;
+		// p.y = y;
+		// p.z = z;
+		// vis_array_.markers.at(0).points.push_back(p);
+	}
+
+	void publishMapImage()
+	{
+		cv::Size size(image_width_, image_height_);
+		cv::Mat image;
+		cv_bridge::CvImage msg;
+
+		if (!board_->ids.empty()) {
+			_drawPlanarBoard(board_, size, image, image_margin_, 1);
+		} 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,777 @@
+// This code is basically taken from https://github.com/opencv/opencv_contrib/blob/master/modules/aruco/src/aruco.cpp
+// with some improvements and fixes
+
+#include "draw.h"
+
+using namespace cv;
+using namespace cv::aruco;
+
+static void _cvProjectPoints2( const CvMat* object_points, const CvMat* rotation_vector,
+                       const CvMat* translation_vector, const CvMat* camera_matrix,
+                       const CvMat* distortion_coeffs, CvMat* image_points,
+                       CvMat* dpdrot CV_DEFAULT(NULL), CvMat* dpdt CV_DEFAULT(NULL),
+                       CvMat* dpdf CV_DEFAULT(NULL), CvMat* dpdc CV_DEFAULT(NULL),
+                       CvMat* dpddist CV_DEFAULT(NULL),
+                       double aspect_ratio CV_DEFAULT(0));
+
+static void _projectPoints( InputArray objectPoints,
+                    InputArray rvec, InputArray tvec,
+                    InputArray cameraMatrix, InputArray distCoeffs,
+                    OutputArray imagePoints,
+                    OutputArray jacobian = noArray(),
+                    double aspectRatio = 0 );
+
+
+void _drawPlanarBoard(Board *_board, Size outSize, OutputArray _img, int marginSize,
+                      int borderBits) {
+
+	CV_Assert(outSize.area() > 0);
+	CV_Assert(marginSize >= 0);
+
+	_img.create(outSize, CV_8UC1);
+	Mat out = _img.getMat();
+	out.setTo(Scalar::all(255));
+	out.adjustROI(-marginSize, -marginSize, -marginSize, -marginSize);
+
+	// calculate max and min values in XY plane
+	CV_Assert(_board->objPoints.size() > 0);
+	float minX, maxX, minY, maxY;
+	minX = maxX = _board->objPoints[0][0].x;
+	minY = maxY = _board->objPoints[0][0].y;
+
+	for(unsigned int i = 0; i < _board->objPoints.size(); i++) {
+		for(int j = 0; j < 4; j++) {
+			minX = min(minX, _board->objPoints[i][j].x);
+			maxX = max(maxX, _board->objPoints[i][j].x);
+			minY = min(minY, _board->objPoints[i][j].y);
+			maxY = max(maxY, _board->objPoints[i][j].y);
+		}
+	}
+
+	float sizeX = maxX - minX;
+	float sizeY = maxY - minY;
+
+	// proportion transformations
+	float xReduction = sizeX / float(out.cols);
+	float yReduction = sizeY / float(out.rows);
+
+	// determine the zone where the markers are placed
+	if(xReduction > yReduction) {
+		int nRows = int(sizeY / xReduction);
+		int rowsMargins = (out.rows - nRows) / 2;
+		out.adjustROI(-rowsMargins, -rowsMargins, 0, 0);
+	} else {
+		int nCols = int(sizeX / yReduction);
+		int colsMargins = (out.cols - nCols) / 2;
+		out.adjustROI(0, 0, -colsMargins, -colsMargins);
+	}
+
+	// now paint each marker
+	Dictionary &dictionary = *(_board->dictionary);
+	Mat marker;
+	Point2f outCorners[3];
+	Point2f inCorners[3];
+	for(unsigned int m = 0; m < _board->objPoints.size(); m++) {
+		// transform corners to markerZone coordinates
+		for(int j = 0; j < 3; j++) {
+			Point2f pf = Point2f(_board->objPoints[m][j].x, _board->objPoints[m][j].y);
+			// move top left to 0, 0
+			pf -= Point2f(minX, minY);
+			pf.x = pf.x / sizeX * float(out.cols);
+			pf.y = (1.0f - pf.y / sizeY) * float(out.rows);
+			outCorners[j] = pf;
+		}
+
+		// get marker
+		Size dst_sz(outCorners[2] - outCorners[0]); // assuming CCW order
+		// dst_sz.width = dst_sz.height = std::min(dst_sz.width, dst_sz.height); //marker should be square
+		double diag = std::round(std::hypot(dst_sz.width, dst_sz.height));
+		int side = std::round(diag / std::sqrt(2));
+
+		dictionary.drawMarker(_board->ids[m], side, marker, borderBits);
+
+		// interpolate tiny marker to marker position in markerZone
+		inCorners[0] = Point2f(-0.5f, -0.5f);
+		inCorners[1] = Point2f(marker.cols - 0.5f, -0.5f);
+		inCorners[2] = Point2f(marker.cols - 0.5f, marker.rows - 0.5f);
+
+		// remove perspective
+		Mat transformation = getAffineTransform(inCorners, outCorners);
+		warpAffine(marker, out, transformation, out.size(), INTER_LINEAR,
+						BORDER_TRANSPARENT);
+	}
+}
+
+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);
+
+    if (imagePointsZ[0].z < 0 ||
+        imagePointsZ[1].z < 0 ||
+        imagePointsZ[2].z < 0 ||
+        imagePointsZ[3].z < 0)
+    {
+        // Any axis point is behind screen plane -> don't draw anything
+        return;
+    }
+
+    // Intersect axis lines with screen plane (they may be outside)
+    std::vector<Point2f> imagePoints(4);
+    imagePoints[0] = Point2f{imagePointsZ[0].x, imagePointsZ[0].y};
+    imagePoints[1] = Point2f{imagePointsZ[1].x, imagePointsZ[1].y};
+    imagePoints[2] = Point2f{imagePointsZ[2].x, imagePointsZ[2].y};
+    imagePoints[3] = Point2f{imagePointsZ[3].x, imagePointsZ[3].y};
+
+    // draw axis lines
+    line(_image, imagePoints[0], imagePoints[1], Scalar(0, 0, 255), 3);
+    line(_image, imagePoints[0], imagePoints[2], Scalar(0, 255, 0), 3);
+    line(_image, imagePoints[0], imagePoints[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,8 @@
+#include <cmath>
+#include <ros/ros.h>
+#include <opencv2/opencv.hpp>
+#include <opencv2/aruco.hpp>
+
+void _drawPlanarBoard(cv::aruco::Board *_board, cv::Size outSize, cv::OutputArray _img, int marginSize, int borderBits);
+void _drawAxis(cv::InputOutputArray image, cv::InputArray cameraMatrix, cv::InputArray distCoeffs,
+              cv::InputArray rvec, cv::InputArray tvec, float length);

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

aruco_pose/test/basic.test

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

aruco_pose/test/basic.txt

@@ -0,0 +1,7 @@
+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
+10	0.5	0.5	2	0	1.2	0	0
+11	0.2	0.5	0.5	0	0.0	-1	1
+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]

book.json

@@ -10,15 +10,19 @@
         "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"
     ],
     "pluginsConfig": {
         "yametrika": {
             "id": 49359238
         },
         "bulk-redirect": {
-            "basepath": "/",
+            "basepath": "",
             "redirectsFile": "redirects.json"
+        },
+        "sitemap": {
+            "hostname": "https://clever.copterexpress.com"
         }
     }
 }

builder/assets/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"
+

builder/assets/clever.service

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

builder/assets/kinetic-rosdep-clever.yaml

@@ -529,3 +529,15 @@ libogg:
 vl53l1x:
   debian:
     stretch: [ros-kinetic-vl53l1x]
+interactive_markers:
+  debian:
+    stretch: [ros-kinetic-interactive-markers]
+interactive_marker_proxy:
+  debian:
+    stretch: [ros-kinetic-interactive-marker-proxy]
+tf2_web_republisher:
+  debian:
+    stretch: [ros-kinetic-tf2-web-republisher]
+image_publisher:
+  debian:
+    stretch: [ros-kinetic-image-publisher]

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

@@ -7,4 +7,4 @@ 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
+ROS_HOSTNAME=raspberrypi.local

builder/assets/roscore.service

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

builder/image-build.sh

@@ -11,7 +11,7 @@
 
 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-2018-06-29/2018-06-27-raspbian-stretch-lite.zip"
 
 export DEBIAN_FRONTEND=${DEBIAN_FRONTEND:='noninteractive'}
 export LANG=${LANG:='C.UTF-8'}
@@ -53,16 +53,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 +79,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/'
@@ -109,7 +108,11 @@ ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/clever.
 ${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/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 ${SCRIPTS_DIR}'/assets/99-px4fmu.rules' '/lib/udev/rules.d/'
+${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-ros.sh

@@ -66,6 +66,9 @@ echo_stamp "Init rosdep" \
 && echo "yaml file:///etc/ros/rosdep/kinetic-rosdep-clever.yaml" >> /etc/ros/rosdep/sources.list.d/20-default.list \
 && rosdep update
 
+echo_stamp "Populate rosdep for ROS user"
+sudo -u pi rosdep update
+
 resolve_rosdep() {
   # TEMPLATE: resolve_rosdep <CATKIN_PATH> <ROS_DISTRO> <OS_DISTRO> <OS_VERSION>
   CATKIN_PATH=$1
@@ -75,7 +78,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,30 +135,40 @@ 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 "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 \
+&& catkin_make run_tests \
+&& catkin_test_results \
 && 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 \
+    ros-kinetic-tf2-web-republisher \
     ros-kinetic-compressed-image-transport \
     ros-kinetic-rosbridge-suite \
     ros-kinetic-rosserial \
     ros-kinetic-usb-cam \
-    ros-kinetic-vl53l1x
+    ros-kinetic-vl53l1x \
+    ros-kinetic-opencv3=3.3.19-0stretch
 
 # TODO move GeographicLib datasets to Mavros debian package
 echo_stamp "Install GeographicLib datasets (needs for mavros)" \
@@ -169,7 +182,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='raspberrypi.local'
 source /opt/ros/kinetic/setup.bash
 source /home/pi/catkin_ws/devel/setup.bash
 EOF

builder/image-software.sh

@@ -58,7 +58,7 @@ echo_stamp "Install apt keys & repos"
 # TODO: This STDOUT consist 'OK'
 curl http://repo.coex.space/aptly_repo_signing.key 2> /dev/null | apt-key add -
 apt-get update \
-&& apt-get install --no-install-recommends -y -qq dirmngr=2.1.18-8~deb9u3 > /dev/null \
+&& apt-get install --no-install-recommends -y -qq dirmngr=2.1.18-8~deb9u4 > /dev/null \
 && apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key 421C365BD9FF1F717815A3895523BAEEB01FA116
 
 echo "deb http://packages.ros.org/ros/ubuntu stretch main" > /etc/apt/sources.list.d/ros-latest.list
@@ -86,14 +86,12 @@ dnsmasq=2.76-5+rpt1+deb9u1  \
 tmux=2.3-4 \
 vim=2:8.0.0197-4+deb9u1 \
 cmake=3.7.2-1 \
-python-pip=9.0.1-2+rpt2 \
-python3-pip=9.0.1-2+rpt2 \
 libjpeg8-dev=8d1-2 \
 tcpdump \
 ltrace \
 libpoco-dev=1.7.6+dfsg1-5+deb9u1 \
-python-rosdep=0.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,6 +99,9 @@ libffi-dev \
 monkey=1.6.9-1 \
 pigpio python-pigpio python3-pigpio \
 i2c-tools \
+ntpdate \
+python-dev \
+python3-dev \
 && echo_stamp "Everything was installed!" "SUCCESS" \
 || (echo_stamp "Some packages wasn't installed!" "ERROR"; exit 1)
 
@@ -108,29 +109,41 @@ i2c-tools \
 sed -i "s/updates_available//" /usr/share/byobu/status/status
 # sed -i "s/updates_available//" /home/pi/.byobu/status
 
-#echo_stamp "Upgrade pip"
+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
@@ -141,7 +154,7 @@ EOF
 echo_stamp "Attempting to kill dirmngr"
 gpgconf --kill dirmngr
 # dirmngr is only used by apt-key, so we can safely kill it.
-# We ignore killall's exit value as well.
-killall -w -9 dirmngr || true
+# 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,22 @@
+#!/usr/bin/env bash
+
+#
+# Validate built image using tests
+#
+# Copyright (C) 2018 Copter Express Technologies
+#
+# Author: Oleg Kalachev <okalachev@gmail.com>
+#
+
+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
+
+print cv2.getBuildInformation()
+

builder/test/tests.sh

@@ -0,0 +1,48 @@
+#!/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
+
+# 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

clever/CMakeLists.txt

@@ -148,10 +148,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
@@ -168,6 +164,8 @@ add_executable(camera_markers src/camera_markers.cpp)
 
 add_executable(frames src/frames.cpp)
 
+add_executable(vpe_publisher src/vpe_publisher.cpp)
+
 target_link_libraries(simple_offboard
   ${catkin_LIBRARIES}
   ${GeographicLib_LIBRARIES}
@@ -179,6 +177,8 @@ target_link_libraries(camera_markers ${catkin_LIBRARIES})
 
 target_link_libraries(frames ${catkin_LIBRARIES})
 
+target_link_libraries(vpe_publisher ${catkin_LIBRARIES})
+
 add_dependencies(simple_offboard clever_generate_messages_cpp)
 
 ## Rename C++ executable without prefix
@@ -196,10 +196,6 @@ target_link_libraries(clever
   ${catkin_LIBRARIES}
 )
 
-target_link_libraries(aruco_vpe
-  ${catkin_LIBRARIES}
-)
-
 #############
 ## Install ##
 #############

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,37 @@
 <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"/>
+        <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="frame_id" value="aruco_map_detected" if="$(arg aruco_vpe)"/>
+        <param name="frame_id" value="aruco_map" unless="$(arg aruco_vpe)"/>
+    </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

@@ -16,19 +16,21 @@
         <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"/>
+    </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,14 +38,12 @@
         <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>
+        <param name="reference_frames/body" value="map"/>
+        <param name="reference_frames/base_link" value="map"/>
     </node>
 
     <!-- Auxiliary frames -->
@@ -57,16 +57,21 @@
     <!-- 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)">
         <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 -->
+        <remap from="~range" to="mavros/distance_sensor/rangefinder_sub"/> <!-- redirect data to FCU -->
     </node>
 
     <!-- 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)"/>
+    <node pkg="rosserial_python" type="serial_node.py" name="serial_node" output="screen" if="$(arg arduino)">
+        <param name="port" value="/dev/serial/by-id/usb-1a86_USB2.0-Serial-if00-port0"/>
+    </node>
 </launch>

clever/launch/main_camera.launch

@@ -21,11 +21,11 @@
         <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 -->
 
+        <!-- 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

@@ -5,12 +5,12 @@
     <arg name="viz" default="true"/>
     <arg name="respawn" default="true"/>
 
-    <node pkg="mavros" type="mavros_node" name="mavros" required="false" clear_params="true" respawn="$(arg respawn)" respawn_delay="2" output="screen">
+    <node pkg="mavros" type="mavros_node" name="mavros" required="false" clear_params="true" respawn="$(arg respawn)" 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,68 +18,37 @@
         <!-- 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"/>
-
-        <!-- 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>
 

clever/launch/mavros_config.yaml

@@ -0,0 +1,142 @@
+# 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
+      id: 1
+      orientation: PITCH_270
+      covariance: 1  # cm
+
+# 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,28 @@
 
   <!-- 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>geographiclib</depend>
   <depend>nodelet</depend>
   <depend>mavros</depend>
   <depend>mavros_extras</depend>
   <depend>lxml</depend>
   <depend>cv_camera</depend>
+  <depend>cv_bridge</depend>
+  <depend>opencv3</depend>
   <depend>mjpg-streamer</depend>
   <depend>rosbridge_server</depend>
   <depend>web_video_server</depend>
-  <depend>ros_comm</depend>
   <!-- Use test_depend for packages you need only for testing: -->
   <!--   <test_depend>gtest</test_depend> -->
 

clever/requirements.txt

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

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

@@ -8,8 +8,10 @@ import rospy
 from std_srvs.srv import Trigger
 from sensor_msgs.msg import Image, CameraInfo, NavSatFix, Imu, Range
 from mavros_msgs.msg import State, OpticalFlowRad
-from geometry_msgs.msg import PoseStamped, TwistStamped
+from mavros_msgs.srv import ParamGet
+from geometry_msgs.msg import PoseStamped, TwistStamped, PoseWithCovarianceStamped
 import tf.transformations as t
+from aruco_pose.msg import MarkerArray
 
 
 # TODO: roscore is running
@@ -42,8 +44,10 @@ def check(name):
                 for f in failures:
                     rospy.logwarn('%s: %s', name, f)
             except Exception as e:
+                for f in failures:
+                    rospy.logwarn('%s: %s', name, f)
                 traceback.print_exc()
-                rospy.logwarn('%s: exception occured', name)
+                rospy.logwarn('%s: exception occurred', name)
                 return
             if not failures:
                 rospy.loginfo('%s: OK', name)
@@ -51,12 +55,44 @@ def check(name):
     return inner
 
 
+param_get = rospy.ServiceProxy('mavros/param/get', ParamGet)
+
+
+def get_param(name):
+    res = param_get(param_id=name)
+    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
+
+
 @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)')
+
+        est = get_param('SYS_MC_EST_GROUP')
+        if est == 1:
+            rospy.loginfo('Selected estimator: LPE')
+            fuse = get_param('LPE_FUSION')
+            if fuse & (1 << 4):
+                rospy.loginfo('LPE_FUSION: land detector fusion is enabled')
+            else:
+                rospy.loginfo('LPE_FUSION: land detector fusion is disabled')
+            if fuse & (1 << 7):
+                rospy.loginfo('LPE_FUSION: barometer fusion is enabled')
+            else:
+                rospy.loginfo('LPE_FUSION: barometer fusion is disabled')
+
+        elif est == 2:
+            rospy.loginfo('Selected estimator: EKF2')
+        else:
+            failure('Unknown selected estimator: %s', est)
+
     except rospy.ROSException:
         failure('no MAVROS state (check wiring)')
 
@@ -80,12 +116,17 @@ def check_camera(name):
         failure('%s: calibration height doesn\'t match image height (%d != %d))', name, info.height, img.height)
 
 
-@check('Aruco detector')
+@check('ArUco detector')
 def check_aruco():
     try:
-        rospy.wait_for_message('aruco_pose/debug', Image, timeout=1)
+        rospy.wait_for_message('aruco_detect/markers', MarkerArray, timeout=1)
     except rospy.ROSException:
-        failure('no aruco_pose/debug messages')
+        failure('no markers detection')
+        return
+    try:
+        rospy.wait_for_message('aruco_map/pose', PoseWithCovarianceStamped, timeout=1)
+    except rospy.ROSException:
+        failure('no map detection')
 
 
 @check('Vision position estimate')
@@ -99,6 +140,37 @@ def check_vpe():
             failure('no VPE or MoCap messages')
             return
 
+    # 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:
+            rospy.loginfo('Vision yaw weight: %.2f', vision_yaw_w)
+        fuse = get_param('LPE_FUSION')
+        if not fuse & (1 << 2):
+            failure('vision position fusing 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)
+        rospy.loginfo('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 fusing is disabled, change EKF2_AID_MASK parameter')
+        if not fuse & (1 << 4):
+            failure('vision yaw fusing 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)
+        rospy.loginfo('EKF2_EVA_NOISE is %.3f, EKF2_EVP_NOISE is %.3f',
+            get_param('EKF2_EVA_NOISE'),
+            get_param('EKF2_EVP_NOISE'))
+
     # check vision pose and estimated pose inconsistency
     try:
         pose = rospy.wait_for_message('mavros/local_position/pose', PoseStamped, timeout=1)
@@ -195,6 +267,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 fusing 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 scale != 0:
+                failure('LPE_FLW_SCALE parameter is %.2f, but it should be 1.0', scale)
+
+            rospy.loginfo('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 fusing 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)
+            rospy.loginfo('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,15 +310,42 @@ 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('mavros/distance_sensor/rangefinder_sub', Range, timeout=0.5)
+        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=0.5)
+        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):
+            rospy.loginfo('"pub agl as lpos down" in LPE_FUSION is disabled, NOT operating over flat surface')
+        else:
+            rospy.loginfo('"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:
+            rospy.loginfo('EKF2_HGT_MODE != Range sensor, NOT operating over flat surface')
+        else:
+            rospy.loginfo('EKF2_HGT_MODE = Range sensor, operating over flat surface')
+        aid = get_param('EKF2_RNG_AID')
+        if aid != 1:
+            rospy.loginfo('EKF2_RNG_AID != 1, range sensor aiding disabled')
+        else:
+            rospy.loginfo('EKF2_RNG_AID = 1, range sensor aiding enabled')
+
 
 @check('Boot duration')
 def check_boot_duration():

clever/src/simple_offboard.cpp

@@ -34,6 +34,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>
@@ -113,6 +114,7 @@ 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;
@@ -248,7 +250,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();
@@ -270,7 +275,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();
@@ -446,10 +454,10 @@ 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)
+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,
+           float lat, float lon, float thrust, float speed, string frame_id, bool auto_arm,
+           uint8_t& success, string& message)
 {
 	auto stamp = ros::Time::now();
 
@@ -520,12 +528,13 @@ inline void serve(enum setpoint_type_t sp_type, float x, float y, float z, float
 			nav_speed = speed;
 		}
 
-		if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == POSITION || sp_type == VELOCITY) {
-			if (std::isnan(yaw) && yaw_rate == 0) {
-				// keep yaw unchanged
-				yaw = tf2::getYaw(local_position.pose.orientation);
-			}
-		}
+		// if (sp_type == 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
@@ -593,42 +602,36 @@ inline void serve(enum setpoint_type_t sp_type, float x, float y, float z, float
 		message = e.what();
 		ROS_INFO("simple_offboard: %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)
@@ -683,7 +686,7 @@ int main(int argc, char **argv)
 	// 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("default_speed", default_speed, 0.5f);
 	nh_priv.getParam("reference_frames", reference_frames);
@@ -710,6 +713,7 @@ int main(int argc, char **argv)
 	auto velocity_sub = nh.subscribe("mavros/local_position/velocity", 1, &handleMessage<TwistStamped, velocity>);
 	auto global_position_sub = nh.subscribe("mavros/global_position/global", 1, &handleMessage<NavSatFix, global_position>);
 	auto battery_sub = nh.subscribe("mavros/battery", 1, &handleMessage<BatteryState, battery>);
+	auto statustext_sub = nh.subscribe("mavros/statustext/recv", 1, &handleMessage<mavros_msgs::StatusText, statustext>);
 
 	// Setpoint publishers
 	position_pub = nh.advertise<PoseStamped>("mavros/setpoint_position/local", 1);

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("vpe_publisher: 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, "vpe_publisher: 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("vpe_publisher: 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, "vpe_publisher: %s", e.what());
+	}
+}
+
+int main(int argc, char **argv) {
+	ros::init(argc, argv, "vpe_publisher");
+	ros::NodeHandle nh, nh_priv("~");
+
+	tf2_ros::TransformListener tf_listener(tf_buffer);
+
+	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("vpe_publisher: using data from TF");
+	} else {
+		ROS_INFO("vpe_publisher: 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("vpe_publisher: ready");
+	ros::spin();
+}

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.host + ':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') || '';
+});

clever/www/js/viz.js

@@ -0,0 +1,94 @@
+var ros = new ROSLIB.Ros({
+	url : 'ws://' + location.host + ':9090'
+});
+
+var titleEl = document.querySelector('title');
+
+ros.on('error', function(error) {
+	titleEl.innerText = 'Disconnected';
+	err = error;
+	alert('Connection error: please enable \'rosbridge\' in clever.launch!');
+});
+
+ros.on('connection', function() {
+	console.log('connected');
+	titleEl.innerText = 'Connected';
+});
+
+ros.on('close', function() {
+	console.log('disconnected');
+	titleEl.innerText = 'Disconnected';
+});
+
+var viewer, tfClient;
+
+function setScene(fixedFrame) {
+	viewer = new ROS3D.Viewer({
+		divID: 'viz',
+		width: 1000,
+		height: 600,
+		antialias: true
+	});
+
+	tfClient = new ROSLIB.TFClient({
+		ros: ros,
+		angularThres: 0.01,
+		transThres: 0.01,
+		rate: 10.0,
+		fixedFrame : fixedFrame
+	});
+
+	var map = new ROS3D.Grid({
+		ros: ros,
+		tfClient: tfClient,
+		rootObject: viewer.scene
+	});
+
+	viewer.scene.add(map);
+}
+
+function addAxes() {
+	var axes = new ROS3D.Axes({
+		ros: ros,
+		tfClient: tfClient,
+		rootObject: viewer.scene
+	});
+	viewer.scene.add(axes);
+}
+
+function addVehicle() {
+	new ROS3D.MarkerArrayClient({
+		ros: ros,
+		tfClient: tfClient,
+		topic: '/vehicle_marker',
+		rootObject: viewer.scene
+	});
+}
+
+
+function addCamera() {
+	new ROS3D.MarkerArrayClient({
+		ros: ros,
+		tfClient: tfClient,
+		topic: '/main_camera/camera_markers',
+		rootObject: viewer.scene
+	});
+}
+
+function addAruco() {
+	new ROS3D.MarkerArrayClient({
+		ros: ros,
+		tfClient: tfClient,
+		topic: '/aruco_detect/visualization',
+		rootObject: viewer.scene
+	});
+}
+
+function addArucoMap() {
+	new ROS3D.MarkerArrayClient({
+		ros: ros,
+		tfClient: tfClient,
+		topic: '/aruco_map/visualization',
+		rootObject: viewer.scene
+	});
+}

clever/www/viz.html

@@ -0,0 +1,20 @@
+<!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>3D visualization</title>
+	</head>
+	<body>
+		<div id="viz"></div>
+		<script type="text/javascript" src="js/viz.js"></script>
+		<script>
+			setScene('map');
+			addVehicle();
+			addCamera();
+			addAruco();
+		</script>
+	</body>
+</html>