Remove marker 17 as of too common faults + remove CRLF

We assume there won't be more than one FMU connected to the Raspberry Pi at any given time.

.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,68 @@ 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
-  draft: true
+  depth: 50
+matrix:
+  fast_finish: true
+  include:
+    - 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}
+    - 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
 
 # More info there
 # https://github.com/travis-ci/travis-ci/issues/6893

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	0.8	0	0	0	0
+3	0.365	1.335	0.8	0	0	0	0
+18	0.365	2.865	0.8	0	0	0	0
+19	0.365	4.200	0.8	0	0	0	0
+1	0.365	0.000	2.6	0	0	0	0
+0	0.365	1.335	2.6	0	0	0	0
+16	0.365	2.865	2.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,328 @@
+/*
+ * 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_;
+	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<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);
+					}
+
+					// 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,419 @@
+/*
+ * 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_;
+
+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("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);
+			} 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) {
+				cv::aruco::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,87 @@
+// 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;
+
+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);
+	}
+}

aruco_pose/src/draw.h

@@ -0,0 +1,6 @@
+#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);

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

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,8 +4,9 @@ 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
+ExecStart=/opt/ros/kinetic/bin/roslaunch clever clever.launch --wait --screen
 Restart=on-abort
 
 [Install]

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/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,6 +3,7 @@ Description=Launcher for the ROS master, parameter server and rosout logging nod
 After=network.target
 
 [Service]
+User=pi
 EnvironmentFile=/lib/systemd/system/roscore.env
 ExecStart=/opt/ros/kinetic/bin/roscore
 Restart=on-abort

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/'
@@ -110,6 +109,9 @@ ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/roscore
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/roscore.service' '/lib/systemd/system/'
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/kinetic-rosdep-clever.yaml' '/etc/ros/rosdep/'
 # ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/kinetic-ros-clever.rosinstall' '/home/pi/ros_catkin_ws/'
-${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} exec ${SCRIPTS_DIR}'/image-ros.sh' ${REPO_URL} ${IMAGE_VERSION} false false ${NUMBER_THREADS} 
+# 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,31 +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-opencv3=3.3.1neon-0stretch
+    ros-kinetic-opencv3=3.3.19-0stretch
 
 # TODO move GeographicLib datasets to Mavros debian package
 echo_stamp "Install GeographicLib datasets (needs for mavros)" \
@@ -170,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,13 +86,11 @@ 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-rosdep \
 python-rosinstall-generator=0.1.14-1 \
 python-wstool=0.1.17-1 \
 python-rosinstall=0.7.8-1 \
@@ -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,40 @@ 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
 #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 +153,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;
@@ -176,6 +176,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 +219,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

@@ -43,7 +43,7 @@ def check(name):
                     rospy.logwarn('%s: %s', name, f)
             except Exception as e:
                 traceback.print_exc()
-                rospy.logwarn('%s: exception occured', name)
+                rospy.logwarn('%s: exception occurred', name)
                 return
             if not failures:
                 rospy.loginfo('%s: OK', name)
@@ -80,12 +80,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')

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();
 
@@ -593,42 +601,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 +685,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 +712,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", 5.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/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,10 @@
 <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>
 </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,69 @@
+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 = new ROS3D.Viewer({
+	divID: 'viz',
+	width: 1000,
+	height: 600,
+	antialias: true
+});
+
+var tfClient = new ROSLIB.TFClient({
+	ros: ros,
+	angularThres: 0.01,
+	transThres: 0.01,
+	rate: 10.0,
+	fixedFrame : 'map'
+});
+
+// vehicle markers
+var vehicleMarkers = new ROS3D.MarkerArrayClient({
+	ros: ros,
+	tfClient: tfClient,
+	topic: '/vehicle_marker',
+	rootObject: viewer.scene
+});
+
+// camera markers
+var cameraMarkers = new ROS3D.MarkerArrayClient({
+	ros: ros,
+	tfClient: tfClient,
+	topic: '/main_camera/camera_markers',
+	rootObject: viewer.scene
+});
+
+// detected aruco markers
+var cameraMarkers = new ROS3D.MarkerArrayClient({
+	ros: ros,
+	tfClient: tfClient,
+	topic: '/aruco_detect/visualization',
+	rootObject: viewer.scene
+});
+
+var map = new ROS3D.Grid({
+	ros: ros,
+	tfClient: tfClient,
+	// frameID: 'map',
+	rootObject: viewer.scene
+});
+
+viewer.scene.add(map);

clever/www/viz.html

@@ -0,0 +1,14 @@
+<!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>
+	</body>
+</html>

docs/en/3g.md

@@ -0,0 +1,6 @@
+Using an external 3G modem
+===
+
+To use an external 3G modem on Raspberry, you can use the [`sakis3g`] package (https://github.com/Trixarian/sakis3g-source).
+
+TODO

docs/en/SUMMARY.md

@@ -1,25 +1,60 @@
 # Summary
 
 * [Glossary](gloss.md)
-* [Clever 2 assembly](assemble_2.md)
-* [Clever 3 assembly](assemble_3.md)
-* [Connecting 4 in 1 ESCs](4in1.md)
-* [Types of power connectors](connectortypes.md)
-* [Blanching](zap.md)
-* [Initial setup](setup.md)
-* [Using multimeter](test_connection.md)
-* [Possible radio failures](radioerrors.md)
-* [Installation of FPV](fpv.md)
-* [Safety instruction](safety.md)
-* [Soldering safety](tb.md)
-* [Connecting GPS](gps.md)
-* [Flight modes](modes.md)
-* [UART settings](uart.md)
-* [Pixhawk / Pixracer Firmware](firmware.md)
-* [PX4 Parameters](px4_parameters.md)
-* [PID Setup](calibratePID.md)
-* [Raspberry Pi](raspberry.md)
-* [RPi Image](microsd_images.md)
-* [RPi Connection to the Pixhawk](connection.md)
-* [Wi-Fi connection](wifi.md)
-* [SSH access](ssh.md)
+* Assembly
+  * [Clever 2 assembly](assemble_2.md)
+  * [Clever 3 assembly](assemble_3.md)
+  * [Installation of FPV](fpv.md)
+  * [Safety instruction](safety.md)
+  * [Connecting 4 in 1 ESCs](4in1.md)
+  * [Types of power connectors](connectortypes.md)
+  * [Blanching](zap.md)
+  * [Soldering safety](tb.md)
+  * [Using multimeter](test_connection.md)
+  * [Possible radio failures](radioerrors.md)
+  * [Connecting GPS](gps.md)
+* Configuration
+  * [Initial setup](setup.md)
+  * [Flight modes](modes.md)
+  * [Pixhawk / Pixracer Firmware](firmware.md)
+  * [PX4 Parameters](px4_parameters.md)
+  * [PID Setup](calibratePID.md)
+* Work with Raspberry Pi
+  * [Raspberry Pi](raspberry.md)
+  * [RPi Image](microsd_images.md)
+  * [RPi Connection to the Pixhawk](connection.md)
+  * [Wi-Fi connection](wifi.md)
+  * [SSH access to Raspberry Pi](ssh.md)
+  * [Configuring Wi-Fi](network.md)
+  * [Using QGroundControl via Wi-Fi](gcs_bridge.md)
+  * [Controlling Clever from a smartphone](rc.md)
+  * [UART settings](uart.md)
+  * [Viewing images from cameras](web_video_server.md)
+  * [Coordinate systems (frames)](frames.md)
+* Coding
+  * [ROS](ros.md)
+  * [MAVROS](mavros.md)
+  * [Simple OFFBOARD](simple_offboard.md)
+  * [Navigation using ArUco markers](aruco.md)
+  * [Automatic check](selfcheck.md)
+  * [Code examples](snippets.md)
+  * [Adjusting the position of the main camera](camera_frame.md)
+  * [Working with the camera](camera.md)
+  * [Working with a LED strip on Raspberry 3](leds.md)
+  * [Using rviz and rqt](rviz.md)
+  * [Working with the ultrasonic distance gage](sonar.md)
+  * [PX4 Simulation](sitl.md)
+  * [Software autorun](autolaunch.md)
+  * [Controlling the copter from Arduino](arduino.md)
+  * [Using an external 3G modem](3g.md)
+* Clever-based projects
+  * [Copter spheric guard](shield.md)
+  * [Copter Hack 2018](copterhack2018.md)
+  * [Copter Hack 2017](copterhack2017.md)
+* Supplementary materials
+  * [Contribution to Clever](contributing.md)
+  * [Flashing ESCs using BLHeliSuite](esc_firmware.md)
+  * [MAVLink](mavlink.md)
+  * [PX4 Logs and Topics](flight_logs.md)
+* Textbook
+  * [Theory and Videos](lessons.md)

docs/en/arduino.md

@@ -0,0 +1,229 @@
+# Controlling the copter from Arduino
+
+For interaction with ROS topics and services on a Raspberry Pi, you can use the [rosserial_arduino](http://wiki.ros.org/rosserial_arduino) library. This library is pre-installed on [a Raspberry Pi image](microsd_images.md).
+
+The main tutorial for rosserial: http://wiki.ros.org/rosserial_arduino/Tutorials
+
+Arudino is to be installed on Clever and connected via a USB port.
+
+## Configuring Arduino IDE
+
+To work with ROS and Arduino, you should understand the format of installed packages' messages. For this purpose [on Raspberry Pi](ssh.md), build the ROS messages library:
+
+```(bash)
+rosrun rosserial_arduino make_libraries.py.
+```
+
+The obtained folder `ros_lib` is to be copied to `<sketches folder>/libraries` on a computer with Arudino IDE.
+
+## Configuring Raspberry Pi
+
+To run the program on Arduino once, you can use command:
+
+```(bash)
+roslaunch clever arduino.launch
+```
+
+To start the link with Arduino at the startup automatically, set argument `arudino` in the Clever launch file (`~/catkin_ws/src/clever/clever/launch/clever.launch`):
+
+```xml
+<arg name="arduino" default="true"/>
+```
+
+After the launch file is edited, restart package `clever`:
+
+```(bash)
+sudo systemctl restart clever
+```
+
+## Delays
+
+When `rosserial_arduino` is used, the Arduino microcontroller should not be blocked for more than a few seconds (for example, using the `delay` function); otherwise communication between Raspberry Pi and Arduino will be broken.
+
+During implementation of long `while` cycles, ensure periodic calling the `hn.spinOnce` function:
+
+```cpp
+while(/* condition */) {
+  // ... Perform required actions
+  nh.spinOnce();
+}
+```
+
+To organize long delays, use the delays in a loop with periodic calling of the `hn.spinOnce()` function:
+
+```cpp
+// 8 second delay
+for(int i=0; i<8; i++) {
+  delay(1000);
+  nh.spinOnce();
+}
+```
+
+## Working with Clever
+
+The set of services and topics is similar to the regular set in [simple_offboard](simple_offboard.md) and [mavros](mavros.md).
+
+An example of a program that controls the copter by position using the `navigate` and `set_mode` services:
+
+```cpp
+// Connecting libraries for working with rosseral
+#include <ros.h>
+
+// Connecting Clever and MAVROS package message header files
+#include <clever/Navigate.h>
+#include <mavros_msgs/SetMode.h>
+
+using namespace clever;
+using namespace mavros_msgs;
+
+ros::NodeHandle nh;
+
+// Declaring services
+ros::ServiceClient<Navigate::Request, Navigate::Response> navigate("/navigate");
+ros::ServiceClient<SetMode::Request, SetMode::Response> setMode("/mavros/set_mode");
+
+void setup()
+{
+  // Initializing rosserial
+  nh.initNode();
+
+  // Initializing services
+  nh.serviceClient(navigate);
+  nh.serviceClient(setMode);
+
+  // Waiting for connection to Raspberry Pi
+  while(!nh.connected()) nh.spinOnce();
+  nh.loginfo("Startup complete");
+
+  // Custom settings
+  // <...>
+
+  // Test program
+  Navigate::Request nav_req;
+  Navigate::Response nav_res;
+  SetMode::Request sm_req;
+  SetMode::Response sm_res;
+
+  // Ascending to 2 meters:
+  nh.loginfo("Take off");
+  nav_req.auto_arm = false;
+  nav_req.x = 0;
+  nav_req.y = 0;
+  nav_req.z = 2;
+  nav_req.frame_id = "body";
+  nav_req.speed = 0.5;
+  navigate.call(nav_req, nav_res);
+
+  // Waiting for 5 seconds
+  for(int i=0; i<5; i++) {
+  	delay(1000);
+  	nh.spinOnce();
+  }
+
+  nav_req.auto_arm = false;
+
+  // Flying forward 3 meters:
+  nh.loginfo("Fly forward");
+  nav_req.auto_arm = true;
+  nav_req.x = 3;
+  nav_req.y = 0;
+  nav_req.z = 0;
+  nav_req.frame_id = "body";
+  nav_req.speed = 0.8;
+  navigate.call(nav_req, nav_res);
+
+  // Waiting for 5 seconds
+  for(int i=0; i<5; i++) {
+    delay(1000);
+    nh.spinOnce();
+  }
+
+  // Flying to point 1:0:2 in the marker field
+  nh.loginfo("Fly on point");
+  nav_req.auto_arm = false;
+  nav_req.x = 1;
+  nav_req.y = 0;
+  nav_req.z = 2;
+  nav_req.frame_id = "aruco_map";
+  nav_req.speed = 0.8;
+  navigate.call(nav_req, nav_res);
+
+  // Waiting for 5 seconds
+  for(int i=0; i<5; i++) {
+    delay(1000);
+    nh.spinOnce();
+  }
+
+  // Landing
+  nh.loginfo("Land");
+  sm_req.custom_mode = "AUTO.LAND";
+  setMode.call(sm_req, sm_res);
+}
+
+void loop()
+{
+}
+```
+
+## Getting telemetry
+
+With Arduino, you can use the [`get_telemetry` service](simple_offboard.md). To do so, declare it similar to the `navigate` and `set_mode` services:
+
+```cpp
+#include <ros.h>
+
+// ...
+
+#include <clever/GetTelemetry.h>
+
+// ...
+
+ros::ServiceClient<GetTelemetry::Request, GetTelemetry::Response> getTelemetry("/get_telemetry");
+
+// ...
+
+nh.serviceClient(getTelemetry);
+
+// ...
+
+GetTelemetry::Request gt_req;
+GetTelemetry::Response gt_res;
+
+
+// ...
+
+gt_req.frame_id = "aruco_map"; // frame id for x, y, z
+getTelemetry.call(gt_req, gt_res);
+
+// gt_res.x is copter position on the x axis
+// gt_res.y is copter position on the y axis
+// gt_res.z is copter position on the z axis
+```
+
+## Problem
+
+When using Arudino Nano, RAM may be insufficient. In this case, messages will appear in the Aruino IDE like:
+
+```
+Global variables use 1837 bytes (89%) of the dynamic memory, leaving 211 bytes for local variables. The maximum is 2048 bytes.
+Not enough memory, the program may be unstable.
+```
+
+You can reduce RAM usage by reducing the size of the buffers allocated for sending and receiving messages. To do this, place the following line **at the beginning** the program:
+
+```cpp
+#define __AVR_ATmega168__ 1
+```
+
+You can reduce the amount of used memory even more, if you manually configure the number publishers and subscribers, as well as the size of memory buffers allocated for messages, for example:
+
+```cpp
+#include <ros.h>
+
+// ...
+
+typedef ros::NodeHandle_<ArduinoHardware, 3, 3, 100, 100> NodeHandle;
+
+// ...
+NodeHandle nh;
+```

docs/en/aruco.md

@@ -0,0 +1,171 @@
+# Navigation using ArUco markers
+
+> **Note** Documentation for the versions [of image](microsd_images.md), starting with **0.15**. For earlier versions, see [documentation for version **0.14**](https://github.com/CopterExpress/clever/blob/v0.14/docs/ru/aruco.md).
+
+[ArUco-Markers](https://docs.opencv.org/3.2.0/d5/dae/tutorial_aruco_detection.html) is a popular technology for positioning
+robotic systems using computer vision.
+
+Examples of ArUco markers:
+
+![ArUco markers](../assets/markers.jpg)
+
+> **Hint** Use the most matte paper for printing visual markers. Glossy paper may glitter in the light, severely deteriorating the quality of recognition.
+
+For rapid generation of markers for printing, you may use an online tool: http://chev.me/arucogen/.
+
+## aruco\_pose
+
+The `aruco_pose` module allows restoring the position of the copter relative to the map of ArUco markers and communicating it to the flight controller using the [Vision Position Estimation](https://dev.px4.io/en/ros/external_position_estimation.html) mechanism.
+
+If the source of the copter position by the markers is available, the option appears for precise autonomous indoor navigation by the positions using the [simple_offboard](simple_offboard.md) module.
+
+### Turning on
+
+Make sure that in the clever launch file \(`~/catkin_ws/src/clever/clever/launch/clever.launch`\), the start of aruco\_pose and [computer vision cameras](camera.md) is turned on:
+
+```xml
+<arg name="main_camera" default="true"/>
+```
+
+```xml
+<arg name="aruco" default="true"/>
+```
+
+After the launch-file is edited, restart package `clever`:
+
+```(bash)
+sudo systemctl restart clever
+```
+
+### Calibrating the ArUco marker map
+
+An automatically generated [ArUco-board](https://docs.opencv.org/trunk/db/da9/tutorial_aruco_board_detection.html) may be used as a map of marks.
+
+The map of marks is adjusted using file `~/catkin_ws/src/clever/clever/launch/aruco.launch`. To use ArUco-board, enter its parameters:
+
+```xml
+<node pkg="nodelet" type="nodelet" name="aruco_pose" args="load aruco_pose/aruco_pose nodelet_manager">
+    <param name="frame_id" value="aruco_map_raw"/>
+    <!-- the type of the marker field -->
+    <param name="type" value="gridboard"/>
+
+    <!-- the number of markets along x -->
+    <param name="markers_x" value="1"/>
+
+    <!-- the number of markers along y -->
+    <param name="markers_y" value="6"/>
+
+    <!-- ID of the first marker (left top) -->
+    <param name="first_marker" value="240"/>
+
+    <!-- the length of the marker side in meters -->
+    <param name="markers_side" value="0.3362"/>
+
+    <!-- distance between the murders -->
+    <param name="markers_sep" value="0.46"/>
+</node>
+```
+
+The vertical and horizontal distance between the markers may be set separately:
+
+```xml
+<!-- the horizontal distance between the markers -->
+<param name="markers_sep_x" value="0.97"/>
+
+<!-- the vertical distance between the marker -->
+<param name="markers_sep_y" value="1.435"/>
+```
+
+If a map with a custom order of marker IDs is used, parameter `marker_ids` may be used:
+
+```xml
+<rosparam param="marker_ids">[5, 7, 9, 11, 13, 15]</rosparam>
+```
+
+The markers are numbered from the top left corner of the field.
+
+For monitoring the map that is currently used by the copter for navigation, one can watch the content of topic `aruco_pose/map_image`. In a browser, it may be viewed with [web\_video\_server](web_video_server.md) by following the link [http://192.168.11.1:8080/snapshot?topic=/aruco\_pose/map\_image](http://192.168.11.1:8080/snapshot?topic=/aruco_pose/map_image):
+
+![](../../assets/Снимок экрана 2017-11-27 в 23.20.49.png)
+
+When flying, make sure that the markers glued to the floor correspond to the map.
+
+In topic `aruco_pose/debug` \([http://192.168.11.1:8080/snapshot?topic=/aruco\_pose/debug](http://192.168.11.1:8080/snapshot?topic=/aruco_pose/debug)\) the current result of markers recognitions is available:
+
+TODO
+
+### The system of coordinates
+
+According to [agreement](http://www.ros.org/reps/rep-0103.html), the standard ENU system of coordinates is used in the marker field:
+
+* x — rightward \(conditional East\);
+* y — forward \(conditional North\);
+* z — upward.
+
+_Note_: the definition above is provided for a situation when the marker field is on the floor.
+
+First, the zero is the bottom left point of the marker field. The yaw angle is considered zero when the copter is faced rightward\(along the x-axis\).
+
+![The system of markers coordinates](../assets/aruco-frame.png)
+
+### Configuring the flight controller
+
+Correct Vision Position Estimation requires making sure \(via [QGroundControl](gcs_bridge.md)\) that:
+
+* **For Pixhawk**: Firmware with LPE \(local position estimator\) is installed. For Pixhawk [download firmware `px4fmu-v2_lpe.px4`](https://github.com/PX4/Firmware/releases).
+
+  **For Pixracer**: parameter `SYS_MC_EST_GROUP` should be set to`local_position_estimator, attitude_estimator_q`.
+
+  > **Note** After changing the value of parameter `SYS_MC_EST_GROUP` restart the flight controller.
+* In parameter `LPE_FUSION` **only** flags `vision position`, `land detector` are enabled. The final value _20_.
+* Compass disabled: `ATT_W_MAG` = 0
+* Complimentary filter external heading weight: `ATT_W_EXT_HDG` = 0.5
+* Orientation by yaw by vision enabled: `ATT_EXT_HDG_M` = 2 `MOCAP`.
+* VPE settings: `LPE_VIS_DELAY` = 0 sec, `LPE_VIS_XY` = 0.1 m, `LPE_VIS_Z` = 0.15 m.
+* Recommended settings for land detector:
+  * `COM_DISARM_LAND` = 1 s
+  * `LNDMC_ROT_MAX` = 45 deg
+  * `LNDMC_THR_RANGE` = 0.5
+  * `LNDMC_Z_VEL_MAX` = 1 m/s
+
+<!--
+For the ease of configuring, you may use a ready settings file for [Clever 2](https://github.com/CopterExpress/clever/blob/master/docs/assets/Clever2LPE_160118.params) or for [Clever 3](https://github.com/CopterExpress/clever/blob/master/docs/assets/Clever3_LPE_020218.params) and upload it to the controller using menu Tools - Load from file in tab Parameters in QGroundControl.
+
+![](../assets/Screenshot from 2018-02-27 22-30-50.png)
+-->
+
+### Flight
+
+A properly configured copter starts holding position by VPE \(in [modes](modes.md) `POSCTL` or `OFFBOARD`\) automatically.
+
+For [autonomous flights](simple_offboard.md) do you will be able to use functions `navigate`, `set_position`, `set_velocity`. For flying to specific coordinates of the marker field, use frame `aruco_map`:
+
+```python
+# First, the copter has to take off to see the map of marks
+# and for frame aruco_map to appear:
+navigate(0, 0, 2, frame_id='body', speed=0.5, auto_arm=True) #  take off to the altitude of 2 meters
+
+time.sleep(5)
+
+# Flying to coordinate 2:2 of the marker field at the altitude of 2 meters
+navigate(2, 2, 2, speed=1, frame_id='aruco_map')  #  flying to coordinate 2:2 at the altitude of 3 meters
+```
+
+See [other functions](simple_offboard.md) simple_offboard.
+
+### Location of markers on the ceiling
+
+![Markers on the ceiling](../assets/IMG_4175.JPG)
+
+To navigate by the markers placed on the ceiling, you need to set the main camera facing up and [set the corresponding frame of the camera](camera_frame.md).
+
+To set the map of markers in a "turned over" system of coordinates, change parameter `aruco_orientation` in  file `~/catkin_ws/src/clever/clever/aruco.launch`:
+
+```xml
+<param name="aruco_orientation" value="map_upside_down"/>
+```
+
+When this parameter is set, frame aruco\_map will also be "turned over". Thus, to fly at the altitude of 2 meters below the ceiling, argument `z` should be set to 2:
+
+```python
+navigate(x=1, y=2, z=1.1, speed=0.5, frame_id='aruco_map')