Add a marker to nti novgorod map

docs: edit nti2019.md
Edit NTI markers
2026-06-01 07:29:32 +00:00 · 2019-04-10 18:42:08 +03:00 · 2019-04-09 20:44:27 +03:00 · 2019-04-09 20:07:08 +03:00 · 2019-04-09 16:32:43 +03:00 · 2019-04-08 20:34:00 +03:00
408 changed files with 4076 additions and 10128 deletions
--- a/.markdownlint.json
+++ b/.markdownlint.json
@@ -13,59 +13,31 @@
    "MD040": false,
    "MD044": {
        "names": [
-            "COEX",
-            "Copter Express",
-            "Коптер Экспресс",
-            "Клевер",
            "MAVLink",
            "ROS",
            "ROS Kinetic",
            "OpenCV",
-            "Gazebo",
            "GitHub",
            "FPV",
            "PPM",
            "PWM",
            "Python",
            "C++",
-            "JavaScript",
-            "Node.js",
-            "Django",
-            "Flask",
-            "HTTP",
-            "HTTPS",
-            "WebSocket",
-            "RPC",
            "PX4",
-            "ArduPilot",
-            "jMAVSim",
-            "px4.io",
-            "logs.px4.io",
            "QGroundControl",
            "QGC",
            "WireShark",
            "FlightPlot",
            "OFFBOARD",
-            "ACRO",
-            "RPY",
            "LPE",
            "EKF2",
-            "IMU",
-            "VPE",
            "SITL",
            "PID",
            "Wi-Fi",
            "Raspberry Pi",
            "RPi",
            "Linux",
-            "GNU",
-            "GNU/Linux",
            "Windows",
-            "Docker",
-            "RFC",
-            "Travis",
-            "travis-ci.org",
-            "travis-ci.com",
            "macOS",
            "iOS",
            "Android",
@@ -73,7 +45,6 @@
            "Raspbian",
            "Raspbian Jesse",
            "Raspbian Stretch",
-            "Raspbian Buster",
            "Pixhawk",
            "Pixracer",
            "Arduino",
@@ -82,19 +53,12 @@
            "LIRC",
            "GPIO",
            "HC-SR04",
-            "RCW-0001",
-            "RealSense",
-            "NUC",
-            "NVIDIA",
-            "Jetson",
-            "Jetson Nano",
            "STM",
            "LED",
            "USB",
            "FAT32",
            "uORB",
            "SSH",
-            "PuTTY",
            "API",
            "UART",
            "GND",
@@ -103,8 +67,7 @@
            "SDA",
            "TCP",
            "UDP",
-            "QR",
-            "Li-ion"
+            "QR"
        ],
        "code_blocks": false
    },
--- a/.travis.yml
+++ b/.travis.yml
@@ -23,16 +23,7 @@ jobs:
 # Check if there are any cached images, copy them to our "images" directory
        - if [ -n "$(ls -A imgcache/*.zip)" ]; then mkdir -p images && cp imgcache/*.zip images; fi
      script:
-        - if [[ -z ${TRAVIS_TAG} && "${TRAVIS_PULL_REQUEST}" != "false" ]]; then
-            echo "Commit range is ${TRAVIS_COMMIT_RANGE}" &&
-            if [ $(git diff --name-only ${TRAVIS_COMMIT_RANGE} | grep -v ^"docs/" | wc -l) -eq 0 ]; then
-              echo " === Docs-only change; skipping build ===" &&
-              export SKIP_BUILD=true;
-            fi;
-          fi
-        - if [ -z ${SKIP_BUILD} ]; then
-            docker run --privileged --rm -v /dev:/dev -v $(pwd):/builder/repo -e TRAVIS_TAG="${TRAVIS_TAG}" ${DOCKER};
-          fi
+        - 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:
@@ -62,22 +53,21 @@ jobs:
        - markdownlint -V
      script:
        - markdownlint docs
-        - ./check_assets_size.py
-        - ./check_unused_assets.py
        - gitbook install
        - gitbook build
-      deploy:
-        provider: pages
-        local-dir: _book
-        skip-cleanup: true
-        github-token: ${GITHUB_OAUTH_TOKEN}
-        keep-history: true
-        target-branch: master
-        repo: CopterExpress/clever-gitbook
-        fqdn: clever.copterexpress.com
-        verbose: true
-        on:
-          branch: master
+# ***
+# Disable deployments for now, revisit this later
+#   --sfalexrog, 06.02.2019
+# ***
+#      deploy:
+#        provider: pages
+#        local-dir: _book
+#        skip-cleanup: true
+#        github-token: ${GITHUB_OAUTH_TOKEN}
+#        keep-history: true
+#        target-branch: gh-pages
+#        on:
+#          branch: WIP/gitbook-autobuild
    - stage: Annotate
      name: Auto-generate changelog
      language: python
@@ -86,14 +76,6 @@ jobs:
        - pip install GitPython PyGithub
      script:
        - PYTHONUNBUFFERED=1 python ./gen_changelog.py
-    - stage: Build
-      name: Editorconfig-lint
-      language: generic
-      before_script:
-        - wget https://github.com/okalachev/editorconfig-checker/releases/download/1.2.1-disable-spaces-amount/ec-linux-amd64
-        - chmod +x ec-linux-amd64
-      script:
-        - ./ec-linux-amd64 -spaces-after-tabs -e "roslib.js|ros3d.js|eventemitter2.js|draw.cpp|BinUtils.swift|\.idea|apps/android/app|Assets.xcassets|test_parser_pass.txt|test_node_failure.txt"
 stages:
  - Build
  - Annotate
--- a/README.md
+++ b/README.md
@@ -1,16 +1,16 @@
 # CLEVER

-<img src="docs/assets/clever4-front-white.png" align="right" width="400px" alt="CLEVER drone">
+<img src="docs/assets/clever3.png" align="right" width="300px" alt="CLEVER drone">

 CLEVER (Russian: *"Клевер"*, meaning *"Clover"*) is an educational programmable drone kit consisting of an unassembled quadcopter, open source software and documentation. The kit includes Pixhawk/Pixracer autopilot running PX4 firmware, Raspberry Pi 3 as companion computer, a camera for computer vision navigation as well as additional sensors and peripheral devices.

 Copter Express has implemented a large number of different autonomous drone projects using exactly the same platform: [automated pizza delivery](https://www.youtube.com/watch?v=hmkAoZOtF58) in Samara and Kazan, coffee delivery in Skolkovo Innovation Center, [autonomous quadcopter with charging station](https://www.youtube.com/watch?v=RjX6nUqw1mI) for site monitoring and security, winning drones on [Robocross-2016](https://www.youtube.com/watch?v=dGbDaz_VmYU) and [Robocross-2017](https://youtu.be/AQnd2CRczbQ) competitions and many others.

-**The main documentation is available [on Gitbook](https://clever.copterexpress.com/).**
+**The main documentation in Russian is available [on our Gitbook](https://clever.copterexpress.com/).**

 Use it to learn how to assemble, configure, pilot and program autonomous CLEVER drone.

-## Raspberry Pi image
+## Preconfigured RPi 3 image

 **Preconfigured image for Raspberry Pi 3 with installed and configured software, ready to fly, is available [in the Releases section](https://github.com/CopterExpress/clever/releases).**

@@ -30,74 +30,49 @@ API description (in Russian) for autonomous flights is available [on GitBook](ht

 ## Manual installation

-Install ROS Kinetic according to the [documentation](http://wiki.ros.org/kinetic/Installation), then [create a Catkin workspace](http://wiki.ros.org/catkin/Tutorials/create_a_workspace).
+Install ROS Kinetic according to the [documentation](http://wiki.ros.org/kinetic/Installation).

-Clone this repo to directory `~/catkin_ws/src/clever`:
+Clone repo to directory `/home/pi/catkin_ws/src/clever`:

 ```bash
 cd ~/catkin_ws/src
 git clone https://github.com/CopterExpress/clever.git clever
 ```

-All the required ROS packages (including `mavros` and `opencv`) can be installed using `rosdep`:
-
-```bash
-cd ~/catkin_ws/
-rosdep install -y --from-paths src --ignore-src
-```
-
-Build ROS packages (on memory constrained platforms you might be going to need to use `-j1` key):
+Build ROS packages:

 ```bash
 cd ~/catkin_ws
 catkin_make -j1
 ```

-To complete `mavros` install you'll need to install `geographiclib` datasets:
+Enable systemd service `roscore` (if not enabled):

 ```bash
-curl https://raw.githubusercontent.com/mavlink/mavros/master/mavros/scripts/install_geographiclib_datasets.sh | sudo bash
-```
-
-You may optionally install udev rules to provide `/dev/px4fmu` symlink to your PX4-based flight controller connected over USB. Copy `99-px4fmu.rules` to your `/lib/udev/rules.d` folder:
-
-```bash
-cd ~/catkin_ws/src/clever/clever/config
-sudo cp 99-px4fmu.rules /lib/udev/rules.d
-```
-
-Alternatively you may change the `fcu_url` property in `mavros.launch` file to point to your flight controller device.
-
-## Running
-
-Enable systemd service `roscore` (if not running):
-
-```bash
-sudo systemctl enable /home/<username>/catkin_ws/src/clever/builder/assets/roscore.service
+sudo systemctl enable /home/pi/catkin_ws/src/clever/deploy/roscore.service
 sudo systemctl start roscore
 ```

-To start connection to SITL, use:
+Enable systemd service `clever`:

 ```bash
-roslaunch clever sitl.launch
-```
-
-To start connection to the flight controller, use:
-
-```bash
-roslaunch clever clever.launch
-```
-
-> Note that the package is configured to connect to `/dev/px4fmu` by default (see [previous section](#manual-installation)). Install udev rules or specify path to your FCU device in `mavros.launch`.
-
-Also, you can enable and start the systemd service:
-
-```bash
-sudo systemctl enable /home/<username>/catkin_ws/src/clever/deploy/clever.service
+sudo systemctl enable /home/pi/catkin_ws/src/clever/deploy/clever.service
 sudo systemctl start clever
 ```

-## License
+### Dependencies

-While the Clever platform source code is available under the MIT License, note, that the [documentation](docs/) is licensed under the Creative Commons Attribution-NonCommercial-ShareAlike 4.0 International License.
+[ROS Kinetic](http://wiki.ros.org/kinetic).
+
+Necessary ROS packages:
+
+* `opencv3`
+* `mavros`
+* `rosbridge_suite`
+* `web_video_server`
+* `cv_camera`
+* `nodelet`
+* `dynamic_reconfigure`
+* `bondcpp`, branch `master`
+* `roslint`
+* `rosserial`
--- a/apps/android/app/src/main/assets/main.js
+++ b/apps/android/app/src/main/assets/main.js
@@ -23,18 +23,19 @@ function throttle(func, ms) {
 }

 function postAppMessage(msg) {
-	if (window.webkit != undefined) {
-		if (window.webkit.messageHandlers.appInterface != undefined) {
-			window.webkit.messageHandlers.appInterface.postMessage(JSON.stringify(msg));
-		}
-	} else if (window.appInterface != undefined) {
-		window.appInterface.postMessage(JSON.stringify(msg));
-	}
+    if (window.webkit != undefined) {
+        if (window.webkit.messageHandlers.appInterface != undefined) {
+            window.webkit.messageHandlers.appInterface.postMessage(JSON.stringify(msg));
+        }
+    }
+    else if (window.appInterface != undefined) {
+        window.appInterface.postMessage(JSON.stringify(msg));
+    }
 }

 function callNativeApp(name, msg) {
 	try {
-		postAppMessage(msg);
+        postAppMessage(msg);
 		return true;
 	} catch(err) {
 		console.warn('The native context does not exist yet');
@@ -108,12 +109,12 @@ function stickTouchStart(e) {

 function stickTouchMove(e) {
 	for (touch of e.changedTouches) {
-		onStickTouchMove(touch);
-	}
-	//onStickTouchMove(e.changedTouches[0]);
-	rcPublishThrottled();
-	e.stopPropagation();
-	e.preventDefault();
+            onStickTouchMove(touch);
+        }
+    	//onStickTouchMove(e.changedTouches[0]);
+    	rcPublishThrottled();
+    	e.stopPropagation();
+    	e.preventDefault();
 }

 function stickTouchEnd(e) {
@@ -135,4 +136,4 @@ stickRight.addEventListener('touchmove', stickTouchMove);
 stickLeft.addEventListener('touchstart', stickTouchStart);
 stickRight.addEventListener('touchstart', stickTouchStart);
 stickLeft.addEventListener('touchend', stickTouchEnd);
-stickRight.addEventListener('touchend', stickTouchEnd);
+stickRight.addEventListener('touchend', stickTouchEnd);
--- a/aruco_pose/CMakeLists.txt
+++ b/aruco_pose/CMakeLists.txt
@@ -173,7 +173,7 @@ target_link_libraries(aruco_pose
 # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html

 ## Mark executable scripts (Python etc.) for installation
-## in contrast to setup.py, you can choose the destination
+ ## in contrast to setup.py, you can choose the destination
 # install(PROGRAMS
 #   scripts/my_python_script
 #   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
@@ -216,8 +216,4 @@ target_link_libraries(aruco_pose
 if (CATKIN_ENABLE_TESTING)
  find_package(rostest REQUIRED)
  add_rostest(test/basic.test)
-  add_rostest(test/test_parser_pass.test)
-  add_rostest(test/test_parser_empty_map.test)
-  add_rostest(test/test_node_failure.test)
-  add_rostest(test/largemap.test)
 endif()
--- a/aruco_pose/README.md
+++ b/aruco_pose/README.md
@@ -74,7 +74,6 @@ It's recommended to run it within the same nodelet manager with the camera nodel
 * `~image_width` – debug image width (default: 2000)
 * `~image_height` – debug image height (default: 2000)
 * `~image_margin` – debug image margin (default: 200)
-* `~dictionary` (*int*) – ArUco dictionary (default: 2) - should be the same as `dictionary` parameter of `aruco_detect` nodelet

 Map file has one marker per line with the following line format:

@@ -110,7 +109,7 @@ See examples in [`map`](map/) directory.
 Command for running tests:

 ```bash
-catkin_make run_tests && catkin_test_results
+rostest aruco_pose basic.test
 ```

 ## Copyright
--- a/aruco_pose/map/cmit.txt
+++ b/aruco_pose/map/cmit.txt
@@ -1,100 +0,0 @@
-0	0.33	0.0	9.0	0	0	0	0	
-1	0.33	1.0	9.0	0	0	0	0	
-2	0.33	2.0	9.0	0	0	0	0	
-3	0.33	3.0	9.0	0	0	0	0	
-4	0.33	4.0	9.0	0	0	0	0	
-5	0.33	5.0	9.0	0	0	0	0	
-6	0.33	6.0	9.0	0	0	0	0	
-7	0.33	7.0	9.0	0	0	0	0	
-8	0.33	8.0	9.0	0	0	0	0	
-9	0.33	9.0	9.0	0	0	0	0	
-10	0.33	0.0	8.0	0	0	0	0	
-11	0.33	1.0	8.0	0	0	0	0	
-12	0.33	2.0	8.0	0	0	0	0	
-13	0.33	3.0	8.0	0	0	0	0	
-14	0.33	4.0	8.0	0	0	0	0	
-15	0.33	5.0	8.0	0	0	0	0	
-16	0.33	6.0	8.0	0	0	0	0	
-#17	0.33	7.0	8.0	0	0	0	0	
-18	0.33	8.0	8.0	0	0	0	0	
-19	0.33	9.0	8.0	0	0	0	0	
-20	0.33	0.0	7.0	0	0	0	0	
-21	0.33	1.0	7.0	0	0	0	0	
-22	0.33	2.0	7.0	0	0	0	0	
-23	0.33	3.0	7.0	0	0	0	0	
-24	0.33	4.0	7.0	0	0	0	0	
-25	0.33	5.0	7.0	0	0	0	0	
-26	0.33	6.0	7.0	0	0	0	0	
-27	0.33	7.0	7.0	0	0	0	0	
-28	0.33	8.0	7.0	0	0	0	0	
-29	0.33	9.0	7.0	0	0	0	0	
-30	0.33	0.0	6.0	0	0	0	0	
-31	0.33	1.0	6.0	0	0	0	0	
-32	0.33	2.0	6.0	0	0	0	0	
-33	0.33	3.0	6.0	0	0	0	0	
-34	0.33	4.0	6.0	0	0	0	0	
-35	0.33	5.0	6.0	0	0	0	0	
-36	0.33	6.0	6.0	0	0	0	0	
-37	0.33	7.0	6.0	0	0	0	0	
-38	0.33	8.0	6.0	0	0	0	0	
-39	0.33	9.0	6.0	0	0	0	0	
-40	0.33	0.0	5.0	0	0	0	0	
-41	0.33	1.0	5.0	0	0	0	0	
-42	0.33	2.0	5.0	0	0	0	0	
-43	0.33	3.0	5.0	0	0	0	0	
-44	0.33	4.0	5.0	0	0	0	0	
-45	0.33	5.0	5.0	0	0	0	0	
-46	0.33	6.0	5.0	0	0	0	0	
-47	0.33	7.0	5.0	0	0	0	0	
-48	0.33	8.0	5.0	0	0	0	0	
-49	0.33	9.0	5.0	0	0	0	0	
-50	0.33	0.0	4.0	0	0	0	0	
-51	0.33	1.0	4.0	0	0	0	0	
-52	0.33	2.0	4.0	0	0	0	0	
-53	0.33	3.0	4.0	0	0	0	0	
-54	0.33	4.0	4.0	0	0	0	0	
-55	0.33	5.0	4.0	0	0	0	0	
-56	0.33	6.0	4.0	0	0	0	0	
-57	0.33	7.0	4.0	0	0	0	0	
-58	0.33	8.0	4.0	0	0	0	0	
-59	0.33	9.0	4.0	0	0	0	0	
-60	0.33	0.0	3.0	0	0	0	0	
-61	0.33	1.0	3.0	0	0	0	0	
-62	0.33	2.0	3.0	0	0	0	0	
-63	0.33	3.0	3.0	0	0	0	0	
-64	0.33	4.0	3.0	0	0	0	0	
-65	0.33	5.0	3.0	0	0	0	0	
-66	0.33	6.0	3.0	0	0	0	0	
-67	0.33	7.0	3.0	0	0	0	0	
-68	0.33	8.0	3.0	0	0	0	0	
-69	0.33	9.0	3.0	0	0	0	0	
-70	0.33	0.0	2.0	0	0	0	0	
-71	0.33	1.0	2.0	0	0	0	0	
-72	0.33	2.0	2.0	0	0	0	0	
-73	0.33	3.0	2.0	0	0	0	0	
-74	0.33	4.0	2.0	0	0	0	0	
-75	0.33	5.0	2.0	0	0	0	0	
-76	0.33	6.0	2.0	0	0	0	0	
-77	0.33	7.0	2.0	0	0	0	0	
-78	0.33	8.0	2.0	0	0	0	0	
-79	0.33	9.0	2.0	0	0	0	0	
-80	0.33	0.0	1.0	0	0	0	0	
-81	0.33	1.0	1.0	0	0	0	0	
-82	0.33	2.0	1.0	0	0	0	0	
-83	0.33	3.0	1.0	0	0	0	0	
-84	0.33	4.0	1.0	0	0	0	0	
-85	0.33	5.0	1.0	0	0	0	0	
-86	0.33	6.0	1.0	0	0	0	0	
-87	0.33	7.0	1.0	0	0	0	0	
-88	0.33	8.0	1.0	0	0	0	0	
-89	0.33	9.0	1.0	0	0	0	0	
-90	0.33	0.0	0.0	0	0	0	0	
-91	0.33	1.0	0.0	0	0	0	0	
-92	0.33	2.0	0.0	0	0	0	0	
-93	0.33	3.0	0.0	0	0	0	0	
-94	0.33	4.0	0.0	0	0	0	0	
-95	0.33	5.0	0.0	0	0	0	0	
-96	0.33	6.0	0.0	0	0	0	0	
-97	0.33	7.0	0.0	0	0	0	0	
-98	0.33	8.0	0.0	0	0	0	0	
-99	0.33	9.0	0.0	0	0	0	0	
--- a/aruco_pose/map/nti_novgorod.txt
+++ b/aruco_pose/map/nti_novgorod.txt
@@ -0,0 +1,22 @@
+34	0.33	0	0	0	0	0	0
+37	0.33	0.74	0	0	0	0	0
+100	0.05	0.37	0.2	0	0	0	0
+6	0.10	0.37	0.39	0	0	0	0
+25	0.33	0	0.54	0	0	0	0
+28	0.33	0.74	0.54	0	0	0	0
+32	0.33	2.74	-0.11	1.20	0	0	0
+29	0.33	2.41	0.69	1.20	0	0	0
+30	0.33	0	2.3	0.05	0	0	0
+31	0.33	0	2.94	0.05	0	0	0
+27	0.33	0.74	2.3	0.05	0	0	0
+26	0.33	0.74	2.94	0.05	0	0	0
+43	0.33	2.39	2.13	0.8	0	0	0
+42	0.33	2.39	3.03	0.8	0	0	0
+40	0.33	3.32	2.13	0.8	0	0	0
+41	0.33	3.32	3.03	0.8	0	0	0
+44	0.33	5.26	2.92	0.05	0	0	0
+50	0.33	5.51	3.38	0.05	0	0	0
+46	0.33	3.95	3.65	0.1	0	0	0
+36	0.33	4.76	-0.18	0.3	0	0	0
+35	0.33	5.36	-0.18	0.3	0	0	0
+33	0.33	5.2	0.61	0.3	0	0	0
--- a/aruco_pose/package.xml
+++ b/aruco_pose/package.xml
@@ -30,7 +30,6 @@
  <depend>rostest</depend>

  <test_depend>image_publisher</test_depend>
-  <test_depend>ros_pytest</test_depend>

  <!-- The export tag contains other, unspecified, tags -->
  <export>
--- a/aruco_pose/src/aruco_detect.cpp
+++ b/aruco_pose/src/aruco_detect.cpp
@@ -19,7 +19,6 @@
 #include <string>
 #include <map>
 #include <unordered_map>
-#include <unordered_set>
 #include <ros/ros.h>
 #include <nodelet/nodelet.h>
 #include <pluginlib/class_list_macros.h>
@@ -63,14 +62,12 @@ private:
 	image_transport::Publisher debug_pub_;
 	image_transport::CameraSubscriber img_sub_;
 	ros::Publisher markers_pub_, vis_markers_pub_;
-	ros::Subscriber map_markers_sub_;
 	bool estimate_poses_, send_tf_, auto_flip_;
 	double length_;
 	std::unordered_map<int, double> length_override_;
 	std::string frame_id_prefix_, known_tilt_;
 	Mat camera_matrix_, dist_coeffs_;
 	aruco_pose::MarkerArray array_;
-	std::unordered_set<int> map_markers_ids_;
 	visualization_msgs::MarkerArray vis_array_;

 public:
@@ -84,7 +81,7 @@ public:
 		nh_priv_.param("estimate_poses", estimate_poses_, true);
 		nh_priv_.param("send_tf", send_tf_, true);
 		if (estimate_poses_ && !nh_priv_.getParam("length", length_)) {
-			NODELET_FATAL("can't estimate marker's poses as ~length parameter is not defined");
+			ROS_FATAL("aruco_detect: can't estimate marker's poses as ~length parameter is not defined");
 			ros::shutdown();
 		}
 		readLengthOverride();
@@ -104,13 +101,12 @@ public:
 		image_transport::ImageTransport it(nh_);
 		image_transport::ImageTransport it_priv(nh_priv_);

-		map_markers_sub_ = nh_.subscribe("map_markers", 1, &ArucoDetect::mapMarkersCallback, this);
 		debug_pub_ = it_priv.advertise("debug", 1);
 		markers_pub_ = nh_priv_.advertise<aruco_pose::MarkerArray>("markers", 1);
 		vis_markers_pub_ = nh_priv_.advertise<visualization_msgs::MarkerArray>("visualization", 1);
 		img_sub_ = it.subscribeCamera("image_raw", 1, &ArucoDetect::imageCallback, this);

-		NODELET_INFO("ready");
+		ROS_INFO("aruco_detect: ready");
 	}

 private:
@@ -162,7 +158,7 @@ private:
 						snap_to = tf_buffer_.lookupTransform(msg->header.frame_id, known_tilt_,
 						                                     msg->header.stamp, ros::Duration(0.02));
 					} catch (const tf2::TransformException& e) {
-						NODELET_WARN_THROTTLE(5, "can't snap: %s", e.what());
+						ROS_WARN_THROTTLE(5, "aruco_detect: can't snap: %s", e.what());
 					}
 				}
 			}
@@ -189,13 +185,9 @@ private:
 					// TODO: check IDs are unique
 					if (send_tf_) {
 						transform.child_frame_id = getChildFrameId(ids[i]);
-
-						// check if such static transform is in the map
-						if (map_markers_ids_.find(ids[i]) == map_markers_ids_.end()) {
-							transform.transform.rotation = marker.pose.orientation;
-							fillTranslation(transform.transform.translation, tvecs[i]);
-							br_.sendTransform(transform);
-						}
+						transform.transform.rotation = marker.pose.orientation;
+						fillTranslation(transform.transform.translation, tvecs[i]);
+						br_.sendTransform(transform);
 					}
 				}
 				array_.markers.push_back(marker);
@@ -333,14 +325,6 @@ private:
 			return length_;
 		}
 	}
-
-	void mapMarkersCallback(const aruco_pose::MarkerArray& msg)
-	{
-		map_markers_ids_.clear();
-		for (auto const& marker : msg.markers) {
-			map_markers_ids_.insert(marker.id);
-		}
-	}
 };

 PLUGINLIB_EXPORT_CLASS(ArucoDetect, nodelet::Nodelet)
--- a/aruco_pose/src/aruco_map.cpp
+++ b/aruco_pose/src/aruco_map.cpp
@@ -18,7 +18,6 @@
 #include <string>
 #include <vector>
 #include <fstream>
-#include <algorithm>
 #include <ros/ros.h>
 #include <nodelet/nodelet.h>
 #include <pluginlib/class_list_macros.h>
@@ -28,7 +27,6 @@
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
 #include <tf2_ros/transform_broadcaster.h>
-#include <tf2_ros/static_transform_broadcaster.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 #include <message_filters/subscriber.h>
 #include <message_filters/synchronizer.h>
@@ -59,7 +57,7 @@ typedef message_filters::sync_policies::ExactTime<Image, CameraInfo, MarkerArray
 class ArucoMap : public nodelet::Nodelet {
 private:
 	ros::NodeHandle nh_, nh_priv_;
-	ros::Publisher img_pub_, pose_pub_, markers_pub_, vis_markers_pub_;
+	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_;
@@ -69,16 +67,13 @@ private:
 	Mat camera_matrix_, dist_coeffs_;
 	geometry_msgs::TransformStamped transform_;
 	geometry_msgs::PoseWithCovarianceStamped pose_;
-	vector<geometry_msgs::TransformStamped> markers_transforms_;
-	aruco_pose::MarkerArray markers_;
 	tf2_ros::TransformBroadcaster br_;
-	tf2_ros::StaticTransformBroadcaster static_br_;
 	tf2_ros::Buffer tf_buffer_;
 	tf2_ros::TransformListener tf_listener_{tf_buffer_};
 	visualization_msgs::MarkerArray vis_array_;
-	std::string known_tilt_, map_, markers_frame_, markers_parent_frame_;
+	std::string known_tilt_;
 	int image_width_, image_height_, image_margin_;
-	bool auto_flip_, image_axis_;
+	bool auto_flip_;

 public:
 	virtual void onInit()
@@ -90,7 +85,6 @@ public:

 		// TODO: why image_transport doesn't work here?
 		img_pub_ = nh_priv_.advertise<sensor_msgs::Image>("image", 1, true);
-		markers_pub_ = nh_priv_.advertise<aruco_pose::MarkerArray>("markers", 1, true);

 		board_ = cv::makePtr<cv::aruco::Board>();
 		board_->dictionary = cv::aruco::getPredefinedDictionary(
@@ -106,9 +100,6 @@ public:
 		nh_priv_.param("image_width", image_width_, 2000);
 		nh_priv_.param("image_height", image_height_, 2000);
 		nh_priv_.param("image_margin", image_margin_, 200);
-		nh_priv_.param("image_axis", image_axis_, true);
-		nh_priv_.param<std::string>("markers/frame_id", markers_parent_frame_, transform_.child_frame_id);
-		nh_priv_.param<std::string>("markers/child_frame_id_prefix", markers_frame_, "");

 		// createStripLine();

@@ -118,7 +109,7 @@ public:
 		} else if (type == "gridboard") {
 			createGridBoard();
 		} else {
-			NODELET_FATAL("unknown type: %s", type.c_str());
+			ROS_FATAL("aruco_map: unknown type: %s", type.c_str());
 			ros::shutdown();
 		}

@@ -133,12 +124,10 @@ public:
 		sync_.reset(new message_filters::Synchronizer<SyncPolicy>(SyncPolicy(10), image_sub_, info_sub_, markers_sub_));
 		sync_->registerCallback(boost::bind(&ArucoMap::callback, this, _1, _2, _3));

-		publishMarkersFrames();
-		publishMarkers();
 		publishMapImage();
 		vis_markers_pub_.publish(vis_array_);

-		NODELET_INFO("ready");
+		ROS_INFO("aruco_map: ready");
 	}

 	void callback(const sensor_msgs::ImageConstPtr& image,
@@ -198,7 +187,7 @@ public:
 				                                          known_tilt_, markers->header.stamp, ros::Duration(0.02));
 				snapOrientation(transform_.transform.rotation, snap_to.transform.rotation, auto_flip_);
 			} catch (const tf2::TransformException& e) {
-				NODELET_WARN_THROTTLE(1, "can't snap: %s", e.what());
+				ROS_WARN_THROTTLE(1, "aruco_map: can't snap: %s", e.what());
 			}

 			geometry_msgs::TransformStamped shift;
@@ -271,7 +260,7 @@ publish_debug:
 		std::string line;

 		if (!f.good()) {
-			NODELET_FATAL("%s - %s", strerror(errno), filename.c_str());
+			ROS_FATAL("aruco_map: %s - %s", strerror(errno), filename.c_str());
 			ros::shutdown();
 		}

@@ -281,60 +270,20 @@ publish_debug:

 			std::istringstream s(line);

-			// Read first character to see whether it's a comment
-			char first = 0;
-			if (!(s >> first)) {
-				// No non-whitespace characters, must be a blank line
+			if (!(s >> id >> length >> x >> y >> z >> yaw >> pitch >> roll)) {
+				ROS_ERROR("aruco_map: cannot parse line: %s", line.c_str());
 				continue;
 			}
-
-			if (first == '#') {
-				NODELET_DEBUG("Skipping line as a comment: %s", line.c_str());
-				continue;
-			} else if (isdigit(first)) {
-				// Put the digit back into the stream
-				// Note that this is a non-modifying putback, so this should work with istreams
-				// (see https://en.cppreference.com/w/cpp/io/basic_istream/putback)
-				s.putback(first);
-			} else {
-				// Probably garbage data; inform user and throw an exception, possibly killing nodelet
-				NODELET_FATAL("Malformed input: %s", line.c_str());
-				ros::shutdown();
-				throw std::runtime_error("Malformed input");
-			}
-
-			if (!(s >> id >> length >> x >> y)) {
-				NODELET_ERROR("Not enough data in line: %s; "
-				          "Each marker must have at least id, length, x, y fields", line.c_str());
-				continue;
-			}
-			// Be less strict about z, yaw, pitch roll
-			if (!(s >> z)) {
-				NODELET_DEBUG("No z coordinate provided for marker %d, assuming 0", id);
-				z = 0;
-			}
-			if (!(s >> yaw)) {
-				NODELET_DEBUG("No yaw provided for marker %d, assuming 0", id);
-				yaw = 0;
-			}
-			if (!(s >> pitch)) {
-				NODELET_DEBUG("No pitch provided for marker %d, assuming 0", id);
-				pitch = 0;
-			}
-			if (!(s >> roll)) {
-				NODELET_DEBUG("No roll provided for marker %d, assuming 0", id);
-				roll = 0;
-			}
 			addMarker(id, length, x, y, z, yaw, pitch, roll);
 		}

-		NODELET_INFO("loading %s complete (%d markers)", filename.c_str(), static_cast<int>(board_->ids.size()));
+		ROS_INFO("aruco_map: loading %s complete (%d markers)", filename.c_str(), static_cast<int>(board_->ids.size()));
 	}

 	void createGridBoard()
 	{
-		NODELET_INFO("generate gridboard");
-		NODELET_WARN("gridboard maps are deprecated");
+		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;
@@ -349,7 +298,7 @@ publish_debug:

 		if (nh_priv_.getParam("marker_ids", marker_ids)) {
 			if ((unsigned int)(markers_x * markers_y) != marker_ids.size()) {
-				NODELET_FATAL("~marker_ids length should be equal to ~markers_x * ~markers_y");
+				ROS_FATAL("~marker_ids length should be equal to ~markers_x * ~markers_y");
 				ros::shutdown();
 			}
 		} else {
@@ -366,7 +315,7 @@ publish_debug:
 			for(int x = 0; x < markers_x; x++) {
 				double x_pos = x * (markers_side + markers_sep_x);
 				double y_pos = max_y - y * (markers_side + markers_sep_y) - markers_side;
-				NODELET_INFO("add marker %d %g %g", marker_ids[y * markers_y + x], x_pos, y_pos);
+				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);
 			}
 		}
@@ -390,19 +339,6 @@ publish_debug:
 	void addMarker(int id, double length, double x, double y, double z,
 				   double yaw, double pitch, double roll)
 	{
-		// Check whether the id is in range for current dictionary
-		int num_markers = board_->dictionary->bytesList.rows;
-		if (num_markers <= id) {
-			NODELET_ERROR("Marker id %d is not in dictionary; current dictionary contains %d markers. "
-			              "Please see https://github.com/CopterExpress/clever/blob/master/aruco_pose/README.md#parameters for details",
-					  id, num_markers);
-			return;
-		}
-		// Check if marker is already in the board
-		if (std::count(board_->ids.begin(), board_->ids.end(), id) > 0) {
-			NODELET_ERROR("Marker id %d is already in the map", id);
-			return;
-		}
 		// Create transform
 		tf::Quaternion q;
 		q.setRPY(roll, pitch, yaw);
@@ -432,45 +368,27 @@ publish_debug:
 		board_->ids.push_back(id);
 		board_->objPoints.push_back(obj_points);

-		// Add marker's static transform
-		if (!markers_frame_.empty()) {
-			geometry_msgs::TransformStamped marker_transform;
-			marker_transform.header.frame_id = markers_parent_frame_;
-			marker_transform.child_frame_id = markers_frame_ + std::to_string(id);
-			tf::transformTFToMsg(transform, marker_transform.transform);
-			markers_transforms_.push_back(marker_transform);
-		}
-
-		// Add marker to array
-		aruco_pose::Marker marker;
-		marker.id = id;
-		marker.length = length;
+		// 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);
-		markers_.markers.push_back(marker);
-
-		// Add visualization marker
-		visualization_msgs::Marker vis_marker;
-		vis_marker.header.frame_id = transform_.child_frame_id;
-		vis_marker.action = visualization_msgs::Marker::ADD;
-		vis_marker.id = vis_array_.markers.size();
-		vis_marker.ns = "aruco_map_marker";
-		vis_marker.type = visualization_msgs::Marker::CUBE;
-		vis_marker.scale.x = length;
-		vis_marker.scale.y = length;
-		vis_marker.scale.z = 0.001;
-		vis_marker.color.r = 1;
-		vis_marker.color.g = 0.5;
-		vis_marker.color.b = 0.5;
-		vis_marker.color.a = 0.8;
-		vis_marker.pose.position.x = x;
-		vis_marker.pose.position.y = y;
-		vis_marker.pose.position.z = z;
-		tf::quaternionTFToMsg(q, marker.pose.orientation);
-		vis_marker.frame_locked = true;
-		vis_array_.markers.push_back(vis_marker);
+		marker.frame_locked = true;
+		vis_array_.markers.push_back(marker);

 		// Add linking line
 		// geometry_msgs::Point p;
@@ -480,18 +398,6 @@ publish_debug:
 		// vis_array_.markers.at(0).points.push_back(p);
 	}

-	void publishMarkersFrames()
-	{
-		if (!markers_transforms_.empty()) {
-			static_br_.sendTransform(markers_transforms_);
-		}
-	}
-
-	void publishMarkers()
-	{
-		markers_pub_.publish(markers_);
-	}
-
 	void publishMapImage()
 	{
 		cv::Size size(image_width_, image_height_);
@@ -499,15 +405,14 @@ publish_debug:
 		cv_bridge::CvImage msg;

 		if (!board_->ids.empty()) {
-			_drawPlanarBoard(board_, size, image, image_margin_, 1, image_axis_);
-			msg.encoding = image_axis_ ? sensor_msgs::image_encodings::RGB8 : sensor_msgs::image_encodings::MONO8;
+			_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.encoding = sensor_msgs::image_encodings::MONO8;
 		msg.image = image;
 		img_pub_.publish(msg.toImageMsg());
 	}
--- a/aruco_pose/src/draw.cpp
+++ b/aruco_pose/src/draw.cpp
@@ -2,7 +2,6 @@
 // with some improvements and fixes

 #include "draw.h"
-#include <math.h>

 using namespace cv;
 using namespace cv::aruco;
@@ -24,12 +23,12 @@ static void _projectPoints( InputArray objectPoints,


 void _drawPlanarBoard(Board *_board, Size outSize, OutputArray _img, int marginSize,
-                      int borderBits, bool drawAxis) {
+                      int borderBits) {

 	CV_Assert(outSize.area() > 0);
 	CV_Assert(marginSize >= 0);

-	_img.create(outSize, drawAxis ? CV_8UC3 : CV_8UC1);
+	_img.create(outSize, CV_8UC1);
 	Mat out = _img.getMat();
 	out.setTo(Scalar::all(255));
 	out.adjustROI(-marginSize, -marginSize, -marginSize, -marginSize);
@@ -88,12 +87,8 @@ void _drawPlanarBoard(Board *_board, Size outSize, OutputArray _img, int marginS
 		// dst_sz.width = dst_sz.height = std::min(dst_sz.width, dst_sz.height); //marker should be square
 		double diag = std::round(std::hypot(dst_sz.width, dst_sz.height));
 		int side = std::round(diag / std::sqrt(2));
-		side = std::max(side, 10);

 		dictionary.drawMarker(_board->ids[m], side, marker, borderBits);
-		if (drawAxis) {
-			cvtColor(marker, marker, COLOR_GRAY2RGB);
-		}

 		// interpolate tiny marker to marker position in markerZone
 		inCorners[0] = Point2f(-0.5f, -0.5f);
@@ -105,104 +100,74 @@ void _drawPlanarBoard(Board *_board, Size outSize, OutputArray _img, int marginS
 		warpAffine(marker, out, transformation, out.size(), INTER_LINEAR,
 						BORDER_TRANSPARENT);
 	}
-
-	// draw axis
-	if (drawAxis) {
-		Size wholeSize; Point ofs;
-		out.locateROI(wholeSize, ofs);
-		auto out_copy = _img.getMat();
-
-		cv::Point center(ofs.x - minX / sizeX * float(out.cols), ofs.y + out.rows + minY / sizeY * float(out.rows));
-
-		int axis_points[3][2] = {{300, 0}, {0, -300}, {-150, 150}};
-		Point axis_names[3] = {Point(270, 50), Point(25, -270), Point(-160, 115)};
-		Scalar colors[] = {Scalar(255, 0, 0), Scalar(0, 255, 0), Scalar(0, 0, 255)};
-		String names[] = {"X", "Y", "Z"};
-
-		int r_half = 14;
-		int height = 55;
-
-		for(int poly = 2; poly >= 0; poly--){
-			double alpha = atan2(0 - axis_points[poly][0], 0 - axis_points[poly][1]);
-			float x_delta = r_half * cos(alpha);
-			float y_delta = r_half * sin(alpha);
-
-			Point polygon_vertices[1][3];
-			polygon_vertices[0][0] = center + Point(axis_points[poly][0] + x_delta, axis_points[poly][1] - y_delta);
-			polygon_vertices[0][1] = center + Point(axis_points[poly][0] - x_delta, axis_points[poly][1] + y_delta);
-			polygon_vertices[0][2] = center + Point(axis_points[poly][0] - sin(alpha) * height, axis_points[poly][1] - cos(alpha) * height);
-
-			const Point* ppt[1] = {polygon_vertices[0]};
-			int npt[] = {3};
-
-			fillPoly(out_copy, ppt, npt, 1, colors[poly]);
-			putText(out_copy, names[poly], center + axis_names[poly], FONT_HERSHEY_SIMPLEX, 1.2, colors[poly], 7);
-			line(out_copy, center, center + Point(axis_points[poly][0], axis_points[poly][1]), colors[poly], 10);
-		}
-	}
 }

 /* Draw a (potentially partially visible) line. */
 static void linePartial(InputOutputArray image, Point3f p1, Point3f p2, const Scalar& color,
-                        int thickness = 1, int lineType = LINE_8, int shift = 0)
+        int thickness = 1, int lineType = LINE_8, int shift = 0)
 {
-	// If both points are behind the screen, don't draw anything
-	if (p1.z <= 0 && p2.z <= 0) {
-		return;
-	}
-	Point2f p1p{p1.x, p1.y};
-	Point2f p2p{p2.x, p2.y};
-	// If points are on the different sides of the plane, compute intersection point
-	if (p1.z * p2.z < 0) {
-		// Compute intersection point with the screen
-		// We denote alpha as such:
-		// xi = (1 - alpha) * x1 + alpha * x2
-		// yi = (1 - alpha) * y1 + alpha * y2
-		// zi = (1 - alpha) * z1 + alpha * z2 = 0
-		// Thus, alpha can be expressed as
-		// alpha = z1 / (z1 - z2)
-		float alpha = p1.z / (p1.z - p2.z);
-		Point2f pi{(1 - alpha) * p1.x + alpha * p2.x, (1 - alpha) * p1.y + alpha * p2.y};
-		// Now, if z1 is negative, we draw the line from (xi, yi) to (x2, y2), else we draw from (x1, y1) to (xi, yi)
-		if (p1.z < 0) {
-			p1p = pi;
-		} else {
-			p2p = pi;
-		}
-	}
-	line(image, p1p, p2p, color, thickness, lineType, shift);
+    // If both points are behind the screen, don't draw anything
+    if (p1.z <= 0 && p2.z <= 0)
+    {
+        return;
+    }
+    Point2f p1p{p1.x, p1.y};
+    Point2f p2p{p2.x, p2.y};
+    // If points are on the different sides of the plane, compute intersection point
+    if (p1.z * p2.z < 0)
+    {
+        // Compute intersection point with the screen
+        // We denote alpha as such:
+        // xi = (1 - alpha) * x1 + alpha * x2
+        // yi = (1 - alpha) * y1 + alpha * y2
+        // zi = (1 - alpha) * z1 + alpha * z2 = 0
+        // Thus, alpha can be expressed as
+        // alpha = z1 / (z1 - z2)
+        float alpha = p1.z / (p1.z - p2.z);
+        Point2f pi{(1 - alpha) * p1.x + alpha * p2.x, (1 - alpha) * p1.y + alpha * p2.y};
+        // Now, if z1 is negative, we draw the line from (xi, yi) to (x2, y2), else we draw from (x1, y1) to (xi, yi)
+        if (p1.z < 0)
+        {
+            p1p = pi;
+        }
+        else
+        {
+            p2p = pi;
+        }
+    }
+    line(image, p1p, p2p, color, thickness, lineType, shift);
 }

 void _drawAxis(InputOutputArray _image, InputArray _cameraMatrix, InputArray _distCoeffs,
              InputArray _rvec, InputArray _tvec, float length) {

-	CV_Assert(_image.getMat().total() != 0 &&
-			  (_image.getMat().channels() == 1 || _image.getMat().channels() == 3));
-	CV_Assert(length > 0);
+    CV_Assert(_image.getMat().total() != 0 &&
+              (_image.getMat().channels() == 1 || _image.getMat().channels() == 3));
+    CV_Assert(length > 0);

-	// project axis points
-	std::vector<Point3f> axisPoints;
-	axisPoints.push_back(Point3f(0, 0, 0));
-	axisPoints.push_back(Point3f(length, 0, 0));
-	axisPoints.push_back(Point3f(0, length, 0));
-	axisPoints.push_back(Point3f(0, 0, length));
-	std::vector<Point3f> imagePointsZ;
-	_projectPoints(axisPoints, _rvec, _tvec, _cameraMatrix, _distCoeffs, imagePointsZ);
+    // project axis points
+    std::vector< Point3f > axisPoints;
+    axisPoints.push_back(Point3f(0, 0, 0));
+    axisPoints.push_back(Point3f(length, 0, 0));
+    axisPoints.push_back(Point3f(0, length, 0));
+    axisPoints.push_back(Point3f(0, 0, length));
+    std::vector< Point3f > imagePointsZ;
+    _projectPoints(axisPoints, _rvec, _tvec, _cameraMatrix, _distCoeffs, imagePointsZ);

-	// draw axis lines
-	linePartial(_image, imagePointsZ[0], imagePointsZ[1], Scalar(0, 0, 255), 3);
-	linePartial(_image, imagePointsZ[0], imagePointsZ[2], Scalar(0, 255, 0), 3);
-	linePartial(_image, imagePointsZ[0], imagePointsZ[3], Scalar(255, 0, 0), 3);
+    // draw axis lines
+    linePartial(_image, imagePointsZ[0], imagePointsZ[1], Scalar(0, 0, 255), 3);
+    linePartial(_image, imagePointsZ[0], imagePointsZ[2], Scalar(0, 255, 0), 3);
+    linePartial(_image, imagePointsZ[0], imagePointsZ[3], Scalar(255, 0, 0), 3);
 }

 static CvMat _cvMat(const cv::Mat& m)
 {
-	CvMat self;
-	CV_DbgAssert(m.dims <= 2);
-	self = cvMat(m.rows, m.dims == 1 ? 1 : m.cols, m.type(), m.data);
-	self.step = (int)m.step[0];
-	self.type = (self.type & ~cv::Mat::CONTINUOUS_FLAG) | (m.flags & cv::Mat::CONTINUOUS_FLAG);
-	return self;
+    CvMat self;
+    CV_DbgAssert(m.dims <= 2);
+    self = cvMat(m.rows, m.dims == 1 ? 1 : m.cols, m.type(), m.data);
+    self.step = (int)m.step[0];
+    self.type = (self.type & ~cv::Mat::CONTINUOUS_FLAG) | (m.flags & cv::Mat::CONTINUOUS_FLAG);
+    return self;
 }

 static void _projectPoints( InputArray _opoints,
@@ -214,47 +179,47 @@ static void _projectPoints( InputArray _opoints,
                        OutputArray _jacobian,
                        double aspectRatio )
 {
-	Mat opoints = _opoints.getMat();
-	int npoints = opoints.checkVector(3), depth = opoints.depth();
-	CV_Assert(npoints >= 0 && (depth == CV_32F || depth == CV_64F));
+    Mat opoints = _opoints.getMat();
+    int npoints = opoints.checkVector(3), depth = opoints.depth();
+    CV_Assert(npoints >= 0 && (depth == CV_32F || depth == CV_64F));

-	CvMat dpdrot, dpdt, dpdf, dpdc, dpddist;
-	CvMat *pdpdrot = 0, *pdpdt = 0, *pdpdf = 0, *pdpdc = 0, *pdpddist = 0;
+    CvMat dpdrot, dpdt, dpdf, dpdc, dpddist;
+    CvMat *pdpdrot=0, *pdpdt=0, *pdpdf=0, *pdpdc=0, *pdpddist=0;

-	CV_Assert(_ipoints.needed());
+    CV_Assert( _ipoints.needed() );

-	_ipoints.create(npoints, 1, CV_MAKETYPE(depth, 3), -1, true);
-	Mat imagePoints = _ipoints.getMat();
-	CvMat c_imagePoints = _cvMat(imagePoints);
-	CvMat c_objectPoints = _cvMat(opoints);
-	Mat cameraMatrix = _cameraMatrix.getMat();
+    _ipoints.create(npoints, 1, CV_MAKETYPE(depth, 3), -1, true);
+    Mat imagePoints = _ipoints.getMat();
+    CvMat c_imagePoints = _cvMat(imagePoints);
+    CvMat c_objectPoints = _cvMat(opoints);
+    Mat cameraMatrix = _cameraMatrix.getMat();

-	Mat rvec = _rvec.getMat(), tvec = _tvec.getMat();
-	CvMat c_cameraMatrix = _cvMat(cameraMatrix);
-	CvMat c_rvec = _cvMat(rvec), c_tvec = _cvMat(tvec);
+    Mat rvec = _rvec.getMat(), tvec = _tvec.getMat();
+    CvMat c_cameraMatrix = _cvMat(cameraMatrix);
+    CvMat c_rvec = _cvMat(rvec), c_tvec = _cvMat(tvec);

-	double dc0buf[5] = {0};
-	Mat dc0(5, 1, CV_64F, dc0buf);
-	Mat distCoeffs = _distCoeffs.getMat();
-	if (distCoeffs.empty())
-		distCoeffs = dc0;
-	CvMat c_distCoeffs = _cvMat(distCoeffs);
-	int ndistCoeffs = distCoeffs.rows + distCoeffs.cols - 1;
+    double dc0buf[5]={0};
+    Mat dc0(5,1,CV_64F,dc0buf);
+    Mat distCoeffs = _distCoeffs.getMat();
+    if( distCoeffs.empty() )
+        distCoeffs = dc0;
+    CvMat c_distCoeffs = _cvMat(distCoeffs);
+    int ndistCoeffs = distCoeffs.rows + distCoeffs.cols - 1;

-	Mat jacobian;
-	if (_jacobian.needed())
-	{
-		_jacobian.create(npoints * 2, 3 + 3 + 2 + 2 + ndistCoeffs, CV_64F);
-		jacobian = _jacobian.getMat();
-		pdpdrot = &(dpdrot = _cvMat(jacobian.colRange(0, 3)));
-		pdpdt = &(dpdt = _cvMat(jacobian.colRange(3, 6)));
-		pdpdf = &(dpdf = _cvMat(jacobian.colRange(6, 8)));
-		pdpdc = &(dpdc = _cvMat(jacobian.colRange(8, 10)));
-		pdpddist = &(dpddist = _cvMat(jacobian.colRange(10, 10 + ndistCoeffs)));
-	}
+    Mat jacobian;
+    if( _jacobian.needed() )
+    {
+        _jacobian.create(npoints*2, 3+3+2+2+ndistCoeffs, CV_64F);
+        jacobian = _jacobian.getMat();
+        pdpdrot = &(dpdrot = _cvMat(jacobian.colRange(0, 3)));
+        pdpdt = &(dpdt = _cvMat(jacobian.colRange(3, 6)));
+        pdpdf = &(dpdf = _cvMat(jacobian.colRange(6, 8)));
+        pdpdc = &(dpdc = _cvMat(jacobian.colRange(8, 10)));
+        pdpddist = &(dpddist = _cvMat(jacobian.colRange(10, 10+ndistCoeffs)));
+    }

-	_cvProjectPoints2(&c_objectPoints, &c_rvec, &c_tvec, &c_cameraMatrix, &c_distCoeffs,
-					  &c_imagePoints, pdpdrot, pdpdt, pdpdf, pdpdc, pdpddist, aspectRatio);
+    _cvProjectPoints2( &c_objectPoints, &c_rvec, &c_tvec, &c_cameraMatrix, &c_distCoeffs,
+                      &c_imagePoints, pdpdrot, pdpdt, pdpdf, pdpdc, pdpddist, aspectRatio );
 }

 namespace _detail
--- a/aruco_pose/src/draw.h
+++ b/aruco_pose/src/draw.h
@@ -3,7 +3,6 @@
 #include <opencv2/opencv.hpp>
 #include <opencv2/aruco.hpp>

-void _drawPlanarBoard(cv::aruco::Board *_board, cv::Size outSize, cv::OutputArray _img,
-                      int marginSize, int borderBits, bool drawAxis); // editorconfig-checker-disable-line
+void _drawPlanarBoard(cv::aruco::Board *_board, cv::Size outSize, cv::OutputArray _img, int marginSize, int borderBits);
 void _drawAxis(cv::InputOutputArray image, cv::InputArray cameraMatrix, cv::InputArray distCoeffs,
-               cv::InputArray rvec, cv::InputArray tvec, float length); // editorconfig-checker-disable-line
+              cv::InputArray rvec, cv::InputArray tvec, float length);
--- a/aruco_pose/src/genmap.py
+++ b/aruco_pose/src/genmap.py
@@ -1,13 +1,5 @@
 #!/usr/bin/env python

-# Copyright (C) 2018 Copter Express Technologies
-#
-# Author: Oleg Kalachev <okalachev@gmail.com>
-#
-# Distributed under MIT License (available at https://opensource.org/licenses/MIT).
-# The above copyright notice and this permission notice shall be included in all
-# copies or substantial portions of the Software.
-
 """Markers map generator

 Generate map file for aruco_map nodelet.
--- a/aruco_pose/src/utils.h
+++ b/aruco_pose/src/utils.h
@@ -1,14 +1,3 @@
-/*
- * Utility functions
- * Copyright (C) 2018 Copter Express Technologies
- *
- * Author: Oleg Kalachev <okalachev@gmail.com>
- *
- * Distributed under MIT License (available at https://opensource.org/licenses/MIT).
- * The above copyright notice and this permission notice shall be included in all
- * copies or substantial portions of the Software.
- */
-
 #pragma once

 #include <cmath>
@@ -30,7 +19,8 @@ static void param(ros::NodeHandle nh, const std::string& param_name, T& param_va
 	}
 }

-static void parseCameraInfo(const sensor_msgs::CameraInfoConstPtr& cinfo, cv::Mat& matrix, cv::Mat& dist)
+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)
--- a/aruco_pose/test/basic.py
+++ b/aruco_pose/test/basic.py
@@ -1,204 +1,101 @@
+#!/usr/bin/env python
+import sys
+import unittest
+import json
 import rospy
-import pytest
+import rostest

-import tf2_ros
-import tf2_geometry_msgs
 from geometry_msgs.msg import PoseWithCovarianceStamped
 from sensor_msgs.msg import Image
 from aruco_pose.msg import MarkerArray
 from visualization_msgs.msg import MarkerArray as VisMarkerArray


-@pytest.fixture
-def node():
-    return rospy.init_node('aruco_pose_test', anonymous=True)
+class TestArucoPose(unittest.TestCase):
+    def setUp(self):
+        rospy.init_node('test_aruco_detect', anonymous=True)

-@pytest.fixture
-def tf_buffer():
-    buf = tf2_ros.Buffer()
-    tf2_ros.TransformListener(buf)
-    return buf
+    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')

-def approx(expected):
-    return pytest.approx(expected, abs=1e-4) # compare floats more roughly
+        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)

-def test_markers(node):
-    markers = rospy.wait_for_message('aruco_detect/markers', MarkerArray, timeout=5)
-    assert len(markers.markers) == 5
-    assert markers.header.frame_id == 'main_camera_optical'
+        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)

-    assert markers.markers[0].id == 2
-    assert markers.markers[0].length == approx(0.33)
-    assert markers.markers[0].pose.position.x == approx(0.36706567854)
-    assert markers.markers[0].pose.position.y == approx(0.290484516644)
-    assert markers.markers[0].pose.position.z == approx(2.18787602301)
-    assert markers.markers[0].pose.orientation.x == approx(0.993997406299)
-    assert markers.markers[0].pose.orientation.y == approx(-0.00532003481626)
-    assert markers.markers[0].pose.orientation.z == approx(-0.107390951553)
-    assert markers.markers[0].pose.orientation.w == approx(0.0201999263402)
-    assert markers.markers[0].c1.x == approx(415.557739258)
-    assert markers.markers[0].c1.y == approx(335.557739258)
-    assert markers.markers[0].c2.x == approx(509.442260742)
-    assert markers.markers[0].c2.y == approx(335.557739258)
-    assert markers.markers[0].c3.x == approx(509.442260742)
-    assert markers.markers[0].c3.y == approx(429.442260742)
-    assert markers.markers[0].c4.x == approx(415.557739258)
-    assert markers.markers[0].c4.y == approx(429.442260742)
+        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)

-    assert markers.markers[4].id == 3
-    assert markers.markers[4].length == approx(0.1)
-    assert markers.markers[4].pose.position.x == approx(-0.1805169666)
-    assert markers.markers[4].pose.position.y == approx(-0.200697302327)
-    assert markers.markers[4].pose.position.z == approx(0.585767514823)
-    assert markers.markers[4].pose.orientation.x == approx(-0.961738074009)
-    assert markers.markers[4].pose.orientation.y == approx(-0.0375180244707)
-    assert markers.markers[4].pose.orientation.z == approx(-0.0115387773672)
-    assert markers.markers[4].pose.orientation.w == approx(0.271144115664)
-    assert markers.markers[4].c1.x == approx(129.557723999)
-    assert markers.markers[4].c1.y == approx(49.557723999)
-    assert markers.markers[4].c2.x == approx(223.442276001)
-    assert markers.markers[4].c2.y == approx(49.557723999)
-    assert markers.markers[4].c3.x == approx(223.442276001)
-    assert markers.markers[4].c3.y == approx(143.442276001)
-    assert markers.markers[4].c4.x == approx(129.557723999)
-    assert markers.markers[4].c4.y == approx(143.442276001)
+    def test_visualization(self):
+        vis = rospy.wait_for_message('aruco_detect/visualization', VisMarkerArray, timeout=5)
+        self.assertEqual(len(vis.markers), 9)

-    assert markers.markers[1].id == 1
-    assert markers.markers[1].length == approx(0.33)
-    assert markers.markers[3].id == 4
-    assert markers.markers[3].length == approx(0.33)
+    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')

-    assert markers.markers[2].id == 100
-    assert markers.markers[2].length == approx(0.33)
-    assert markers.markers[2].pose.position.x == approx(-1.37600105389)
-    assert markers.markers[2].pose.position.y == approx(-0.323028530991)
-    assert markers.markers[2].pose.position.z == approx(2.94611272668)
-    assert markers.markers[2].pose.orientation.x == approx(-0.955543925678)
-    assert markers.markers[2].pose.orientation.y == approx(0.0458801909197)
-    assert markers.markers[2].pose.orientation.z == approx(-0.249604946264)
-    assert markers.markers[2].pose.orientation.w == approx(-0.150093920537)
-    assert markers.markers[2].c1.x == approx(52.557723999)
-    assert markers.markers[2].c1.y == approx(205.557723999)
-    assert markers.markers[2].c2.x == approx(113.442276001)
-    assert markers.markers[2].c2.y == approx(205.557723999)
-    assert markers.markers[2].c3.x == approx(113.442276001)
-    assert markers.markers[2].c3.y == approx(265.442260742)
-    assert markers.markers[2].c4.x == approx(52.557723999)
-    assert markers.markers[2].c4.y == approx(265.442260742)
+    def test_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_markers_frames(node, tf_buffer):
-    marker_2 = tf_buffer.lookup_transform('main_camera_optical', 'aruco_2', rospy.Time(), rospy.Duration(5))
-    assert marker_2.transform.translation.x == approx(0.36706567854)
-    assert marker_2.transform.translation.y == approx(0.290484516644)
-    assert marker_2.transform.translation.z == approx(2.18787602301)
-    assert marker_2.transform.rotation.x == approx(0.993997406299)
-    assert marker_2.transform.rotation.y == approx(-0.00532003481626)
-    assert marker_2.transform.rotation.z == approx(-0.107390951553)
-    assert marker_2.transform.rotation.w == approx(0.0201999263402)
+    def test_map_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_markers_frames(node, tf_buffer):
-    stamp = rospy.get_rostime()
-    timeout = rospy.Duration(5)
+    def test_map_visualization(self):
+        vis = rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5)

-    marker_1 = tf_buffer.lookup_transform('aruco_map', 'aruco_in_map_1', stamp, timeout)
-    assert marker_1.transform.translation.x == approx(0)
-    assert marker_1.transform.translation.y == approx(0)
-    assert marker_1.transform.translation.z == approx(0)
+    def test_map_debug(self):
+        img = rospy.wait_for_message('aruco_map/debug', Image, timeout=5)

-    marker_4 = tf_buffer.lookup_transform('aruco_map', 'aruco_in_map_4', stamp, timeout)
-    assert marker_4.transform.translation.x == approx(1)
-    assert marker_4.transform.translation.y == approx(1)
-    assert marker_4.transform.translation.z == approx(0)
+    # def test_transforms(self):
+    #     pass

-    marker_12 = tf_buffer.lookup_transform('aruco_map', 'aruco_in_map_12', stamp, timeout)
-    assert marker_12.transform.translation.x == approx(0.2)
-    assert marker_12.transform.translation.y == approx(0.5)
-    assert marker_12.transform.translation.z == approx(0)

-def test_visualization(node):
-    vis = rospy.wait_for_message('aruco_detect/visualization', VisMarkerArray, timeout=5)
-    assert len(vis.markers) == 11
-
-def test_debug(node):
-    img = rospy.wait_for_message('aruco_detect/debug', Image, timeout=5)
-    assert img.width == 640
-    assert img.height == 480
-    assert img.header.frame_id == 'main_camera_optical'
-
-def test_map(node):
-    pose = rospy.wait_for_message('aruco_map/pose', PoseWithCovarianceStamped, timeout=5)
-    assert pose.header.frame_id == 'main_camera_optical'
-    assert pose.pose.pose.position.x == approx(-0.629167753342)
-    assert pose.pose.pose.position.y == approx(0.293822650809)
-    assert pose.pose.pose.position.z == approx(2.12641343155)
-    assert pose.pose.pose.orientation.x == approx(-0.998383794799)
-    assert pose.pose.pose.orientation.y == approx(-5.20919098575e-06)
-    assert pose.pose.pose.orientation.z == approx(-0.0300861070302)
-    assert pose.pose.pose.orientation.w == approx(0.0482143590507)
-
-def test_map_image(node):
-    img = rospy.wait_for_message('aruco_map/image', Image, timeout=5)
-    assert img.width == 2000
-    assert img.height == 2000
-    assert img.encoding == 'mono8'
-
-def test_map_markers(node):
-    markers = rospy.wait_for_message('aruco_map/markers', MarkerArray, timeout=5)
-    assert markers.markers[0].id == 1
-    assert markers.markers[1].id == 2
-    assert markers.markers[2].id == 3
-    assert markers.markers[3].id == 4
-    assert markers.markers[4].id == 10
-    assert markers.markers[5].id == 11
-    assert markers.markers[6].id == 12
-
-    assert markers.markers[0].pose.position.x == 0
-    assert markers.markers[0].pose.position.y == 0
-    assert markers.markers[0].pose.position.z == 0
-    assert markers.markers[0].pose.orientation.x == 0
-    assert markers.markers[0].pose.orientation.y == 0
-    assert markers.markers[0].pose.orientation.z == 0
-    assert markers.markers[0].pose.orientation.w == 1
-    assert markers.markers[0].length == approx(0.33)
-
-    assert markers.markers[1].pose.position.x == 1
-    assert markers.markers[1].pose.position.y == 0
-    assert markers.markers[1].pose.position.z == 0
-    assert markers.markers[1].pose.orientation.x == 0
-    assert markers.markers[1].pose.orientation.y == 0
-    assert markers.markers[1].pose.orientation.z == 0
-    assert markers.markers[1].pose.orientation.w == 1
-    assert markers.markers[1].length == approx(0.33)
-
-    assert markers.markers[2].pose.position.x == 0
-    assert markers.markers[2].pose.position.y == 1
-    assert markers.markers[2].pose.position.z == 0
-    assert markers.markers[2].pose.orientation.x == 0
-    assert markers.markers[2].pose.orientation.y == 0
-    assert markers.markers[2].pose.orientation.z == 0
-    assert markers.markers[2].pose.orientation.w == 1
-    assert markers.markers[2].length == approx(0.33)
-
-    assert markers.markers[3].pose.position.x == 1
-    assert markers.markers[3].pose.position.y == 1
-    assert markers.markers[3].pose.position.z == 0
-    assert markers.markers[3].pose.orientation.x == 0
-    assert markers.markers[3].pose.orientation.y == 0
-    assert markers.markers[3].pose.orientation.z == 0
-    assert markers.markers[3].pose.orientation.w == 1
-    assert markers.markers[3].length == approx(0.33)
-
-    assert markers.markers[4].pose.position.x == approx(0.5)
-    assert markers.markers[4].pose.position.y == 2
-    assert markers.markers[4].pose.position.z == 0
-    assert markers.markers[4].pose.orientation.x == 0
-    assert markers.markers[4].pose.orientation.y == 0
-    assert markers.markers[4].pose.orientation.z == approx(0.5646424733950354)
-    assert markers.markers[4].pose.orientation.w == approx(0.8253356149096783)
-    assert markers.markers[4].length == approx(0.5)
-
-def test_map_visualization(node):
-    vis = rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5)
-
-def test_map_debug(node):
-    img = rospy.wait_for_message('aruco_map/debug', Image, timeout=5)
+rostest.rosrun('aruco_pose', 'test_aruco_detect', TestArucoPose, sys.argv)
--- a/aruco_pose/test/basic.test
+++ b/aruco_pose/test/basic.test
@@ -5,27 +5,23 @@
        <param name="camera_info_url" value="file://$(find aruco_pose)/test/camera_info.yaml" />
    </node>

-    <node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager" required="true"/>
+    <node pkg="nodelet" 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" required="true">
+    <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"/>
-        <param name="send_tf" value="true"/>
    </node>

-    <node name="aruco_map" pkg="nodelet" type="nodelet" args="load aruco_pose/aruco_map nodelet_manager" clear_params="true" required="true">
+    <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"/>
-        <param name="markers/frame_id" value="aruco_map"/>
-        <param name="markers/child_frame_id_prefix" value="aruco_in_map_"/>
    </node>

-    <param name="test_module" value="$(find aruco_pose)/test/basic.py"/>
-    <test test-name="aruco_pose_test" pkg="ros_pytest" type="ros_pytest_runner"/>
+    <test test-name="test_aruco_pose" pkg="aruco_pose" type="basic.py"/>
 </launch>
--- a/aruco_pose/test/basic.txt
+++ b/aruco_pose/test/basic.txt
@@ -1,11 +1,7 @@
-# Default markers
 1	0.33	0	0	0	0	0	0
 2	0.33	1	0	0	0	0	0
 3	0.33	0	1	0	0	0	0
 4	0.33	1	1	0	0	0	0
-# Marker with non-zero yaw rotation
 10	0.5	0.5	2	0	1.2	0	0
-# Marker with non-zero pitch and roll rotation
 11	0.2	0.5	0.5	0	0.0	-1	1
-# Marker with yaw, pitch and roll rotation
 12	0.4	0.2	0.5	0	0.1	-1.2	-0.5
--- a/aruco_pose/test/largemap.py
+++ b/aruco_pose/test/largemap.py
@@ -1,26 +0,0 @@
-#!/usr/bin/env python
-import sys
-import unittest
-import json
-import rospy
-import rostest
-
-from sensor_msgs.msg import Image
-from visualization_msgs.msg import MarkerArray as VisMarkerArray
-
-
-class TestArucoPose(unittest.TestCase):
-    def setUp(self):
-        rospy.init_node('test_aruco_largemap', anonymous=True)
-
-    def test_map_image(self):
-        img = rospy.wait_for_message('aruco_map/image', Image, timeout=5)
-        self.assertEqual(img.width, 2000)
-        self.assertEqual(img.height, 2000)
-        self.assertEqual(img.encoding, 'mono8')
-
-    def test_map_visualization(self):
-        vis = rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5)
-
-
-rostest.rosrun('aruco_pose', 'test_aruco_largemap', TestArucoPose, sys.argv)
--- a/aruco_pose/test/largemap.test
+++ b/aruco_pose/test/largemap.test
@@ -1,13 +0,0 @@
-<launch>
-    <node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager" required="true"/>
-
-    <node name="aruco_map" pkg="nodelet" type="nodelet" args="load aruco_pose/aruco_map nodelet_manager" clear_params="true" required="true">
-        <remap from="image_raw" to="main_camera/image_raw"/>
-        <remap from="camera_info" to="main_camera/camera_info"/>
-        <remap from="markers" to="aruco_detect/markers"/>
-        <param name="type" value="map"/>
-        <param name="map" value="$(find aruco_pose)/test/largemap.txt"/>
-    </node>
-
-    <test test-name="test_aruco_pose_largemap" pkg="aruco_pose" type="largemap.py"/>
-</launch>
--- a/aruco_pose/test/largemap.txt
+++ b/aruco_pose/test/largemap.txt
@@ -1,11 +0,0 @@
-0 0.2 0 0 0 0 0 0
-1 0.2 10 0 0 0 0 0
-2 0.2 20 0 0 0 0 0
-3 0.2 30 0 0 0 0 0
-4 0.2 40 0 0 0 0 0
-5 0.2 50 0 0 0 0 0
-6 0.2 60 0 0 0 0 0
-7 0.2 70 0 0 0 0 0
-8 0.2 80 0 0 0 0 0
-9 0.2 90 0 0 0 0 0
-10 0.2 100 0 0 0 0 0
--- a/aruco_pose/test/map.png
+++ b/aruco_pose/test/map.png
--- a/aruco_pose/test/test_node_failure.py
+++ b/aruco_pose/test/test_node_failure.py
@@ -1,13 +0,0 @@
-import rospy
-import pytest
-
-from visualization_msgs.msg import MarkerArray as VisMarkerArray
-
-
-@pytest.fixture
-def node():
-    return rospy.init_node('aruco_pose_test', anonymous=True)
-
-def test_node_failure(node):
-    with pytest.raises(rospy.exceptions.ROSException):
-        rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5)
--- a/aruco_pose/test/test_node_failure.test
+++ b/aruco_pose/test/test_node_failure.test
@@ -1,14 +0,0 @@
-<launch>
-    <node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager"/>
-
-    <node name="aruco_map" pkg="nodelet" type="nodelet" args="load aruco_pose/aruco_map nodelet_manager" clear_params="true">
-        <remap from="image_raw" to="main_camera/image_raw"/>
-        <remap from="camera_info" to="main_camera/camera_info"/>
-        <remap from="markers" to="aruco_detect/markers"/>
-        <param name="type" value="map"/>
-        <param name="map" value="$(find aruco_pose)/test/test_node_failure.txt"/>
-    </node>
-
-    <param name="test_module" value="$(find aruco_pose)/test/test_node_failure.py"/>
-    <test test-name="test_node_failure" pkg="ros_pytest" type="ros_pytest_runner"/>
-</launch>
--- a/aruco_pose/test/test_node_failure.txt
+++ b/aruco_pose/test/test_node_failure.txt
@@ -1,4 +0,0 @@
-# Any garbage data (pretty much anything apart from a comment starting with a hash starting
-# with a hash sign or a number) will be interpreted as garbage data; the node should fail
-# after reading it.
-// Don't try to put your comments this way, it will kill your node!
--- a/aruco_pose/test/test_parser_empty_map.py
+++ b/aruco_pose/test/test_parser_empty_map.py
@@ -1,13 +0,0 @@
-import rospy
-import pytest
-
-from visualization_msgs.msg import MarkerArray as VisMarkerArray
-
-
-@pytest.fixture
-def node():
-    return rospy.init_node('aruco_pose_test_empty_map', anonymous=True)
-
-def test_empty_map(node):
-    markers = rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5)
-    assert len(markers.markers) == 0
--- a/aruco_pose/test/test_parser_empty_map.test
+++ b/aruco_pose/test/test_parser_empty_map.test
@@ -1,14 +0,0 @@
-<launch>
-    <node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager"/>
-
-    <node name="aruco_map" pkg="nodelet" type="nodelet" args="load aruco_pose/aruco_map nodelet_manager" clear_params="true">
-        <remap from="image_raw" to="main_camera/image_raw"/>
-        <remap from="camera_info" to="main_camera/camera_info"/>
-        <remap from="markers" to="aruco_detect/markers"/>
-        <param name="type" value="map"/>
-        <param name="map" value="$(find aruco_pose)/test/test_parser_empty_map.txt"/>
-    </node>
-
-    <param name="test_module" value="$(find aruco_pose)/test/test_parser_empty_map.py"/>
-    <test test-name="test_node_empty_map" pkg="ros_pytest" type="ros_pytest_runner"/>
-</launch>
--- a/aruco_pose/test/test_parser_empty_map.txt
+++ b/aruco_pose/test/test_parser_empty_map.txt
@@ -1,6 +0,0 @@
-# Failing markers: Not enough parameters to add a marker
-1
-2 1
-3 0.5 1
-# Failing markers: Marker IDs outside of dictionary range
-1001 1 1 0
--- a/aruco_pose/test/test_parser_pass.py
+++ b/aruco_pose/test/test_parser_pass.py
@@ -1,61 +0,0 @@
-import rospy
-import pytest
-
-from sensor_msgs.msg import Image
-from aruco_pose.msg import MarkerArray
-from visualization_msgs.msg import MarkerArray as VisMarkerArray
-
-
-@pytest.fixture
-def node():
-    return rospy.init_node('aruco_pose_test', anonymous=True)
-
-def approx(expected):
-    return pytest.approx(expected, abs=1e-4) # compare floats more roughly
-
-def test_markers(node):
-    markers = rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5)
-    assert len(markers.markers) == 6
-
-    assert markers.markers[0].pose.position.x == approx(0)
-    assert markers.markers[0].pose.position.y == approx(0)
-    assert markers.markers[0].pose.position.z == approx(0)
-
-    assert markers.markers[1].pose.position.x == approx(1)
-    assert markers.markers[1].pose.position.y == approx(1)
-    assert markers.markers[1].pose.position.z == approx(1)
-
-    assert markers.markers[2].pose.position.x == approx(1)
-    assert markers.markers[2].pose.position.y == approx(0)
-    assert markers.markers[2].pose.position.z == approx(0.5)
-
-    assert markers.markers[3].pose.position.x == approx(0)
-    assert markers.markers[3].pose.position.y == approx(1)
-    assert markers.markers[3].pose.position.z == approx(0)
-
-    assert markers.markers[4].pose.position.x == approx(1)
-    assert markers.markers[4].pose.position.y == approx(0.5)
-    assert markers.markers[4].pose.position.z == approx(0)
-
-    assert markers.markers[5].pose.position.x == approx(2.2)
-    assert markers.markers[5].pose.position.y == approx(0.2)
-    assert markers.markers[5].pose.position.z == approx(0)
-
-    assert markers.markers[0].scale.x == approx(0.33)
-    assert markers.markers[0].scale.y == approx(0.33)
-    assert markers.markers[1].scale.x == approx(0.225)
-    assert markers.markers[1].scale.y == approx(0.225)
-    assert markers.markers[2].scale.x == approx(0.45)
-    assert markers.markers[2].scale.y == approx(0.45)
-    assert markers.markers[3].scale.x == approx(0.15)
-    assert markers.markers[3].scale.y == approx(0.15)
-    assert markers.markers[4].scale.x == approx(0.25)
-    assert markers.markers[4].scale.y == approx(0.25)
-    assert markers.markers[5].scale.x == approx(0.35)
-    assert markers.markers[5].scale.y == approx(0.35)
-
-def test_map_image(node):
-    img = rospy.wait_for_message('aruco_map/image', Image, timeout=5)
-    assert img.width == 2000
-    assert img.height == 2000
-    assert img.encoding == 'mono8'
--- a/aruco_pose/test/test_parser_pass.test
+++ b/aruco_pose/test/test_parser_pass.test
@@ -1,14 +0,0 @@
-<launch>
-    <node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager" required="true"/>
-
-    <node name="aruco_map" pkg="nodelet" type="nodelet" args="load aruco_pose/aruco_map nodelet_manager" clear_params="true" required="true">
-        <remap from="image_raw" to="main_camera/image_raw"/>
-        <remap from="camera_info" to="main_camera/camera_info"/>
-        <remap from="markers" to="aruco_detect/markers"/>
-        <param name="type" value="map"/>
-        <param name="map" value="$(find aruco_pose)/test/test_parser_pass.txt"/>
-    </node>
-
-    <param name="test_module" value="$(find aruco_pose)/test/test_parser_pass.py"/>
-    <test test-name="test_node_pass" pkg="ros_pytest" type="ros_pytest_runner"/>
-</launch>
--- a/aruco_pose/test/test_parser_pass.txt
+++ b/aruco_pose/test/test_parser_pass.txt
@@ -1,23 +0,0 @@
-# Parser test #1 - correct file
-# 1. Commentary test
-#Commentary text without space after pound sign
-# Commentary text with space after pound sign
-    # Commentary text with spaces before pound sign
-	# Commentary text with tab before pound sign
-			# Text with tabs before pound sign
-# Empty line test:
-
-# All-whitespace line test:
-		    
-# 2. Default coordinates test
-# Fully filled marker description, tab-delimited:
-1	0.33	0	0	0	0	0	0
-# Fully filled marker description, space-delimited:
-2 0.225 1 1 1 0 0 0
-# Default roll, pitch, yaw angles
-3 0.45 1 0 0.5
-# Default roll, pitch, yaw, z
-4 0.15 0 1
-# Inline commentary
-5 0.25 1 0.5# Comment straight after digit
-6 0.35 2.2 0.2 # Comment with a space after digit
--- a/book.json
+++ b/book.json
@@ -4,19 +4,14 @@
    "author": "Copter Express",
    "language": "ru",
    "root": "docs/",
-    "gitbook": "^3.2.2",
    "plugins": [
+        "youtube",
        "richquotes@https://github.com/okalachev/gitbook-plugin-richquotes.git",
        "yametrika",
        "anchors",
        "validate-links",
        "bulk-redirect@https://github.com/okalachev/gitbook-plugin-bulk-redirect.git",
-        "sitemap@https://github.com/okalachev/plugin-sitemap.git",
-        "toolbar@https://github.com/hamishwillee/gitbook-plugin-toolbar.git",
-        "addcssjs",
-        "localized-footer@https://github.com/okalachev/gitbook-plugin-localized-footer.git",
-        "image-zoom@https://github.com/okalachev/gitbook-plugin-image-zoom.git",
-        "language-picker@https://github.com/okalachev/gitbook-plugin-language-picker.git"
+        "sitemap@https://github.com/okalachev/plugin-sitemap.git"
    ],
    "pluginsConfig": {
        "yametrika": {
@@ -28,34 +23,6 @@
        },
        "sitemap": {
            "hostname": "https://clever.copterexpress.com"
-        },
-        "toolbar": {
-            "buttons":
-            [
-                {
-                    "label": "Edit page on github",
-                    "icon": "fa fa-pencil-square-o",
-                    "position" : "left",
-                    "url": "https://github.com/CopterExpress/clever/edit/master/docs/{{filepath_lang}}"
-                },
-                {
-                    "label": "GitHub",
-                    "icon": "fa fa-github",
-                    "position" : "left",
-                    "url": "https://github.com/CopterExpress/clever"
-                }
-            ]
-        },
-        "addcssjs": {
-            "css": ["../clever.css"],
-            "js": ["../clever.js"]
-        },
-        "language-picker": {
-            "languages": [["ru", "Russian"], ["en", "English"]]
-        },
-        "localized-footer": {
-            "hline": false,
-            "filename": "./FOOTER.md"
        }
    }
 }
--- a/builder/assets/99-px4fmu.rules
+++ b/builder/assets/99-px4fmu.rules
--- a/builder/assets/clever.service
+++ b/builder/assets/clever.service
@@ -1,13 +1,14 @@
 [Unit]
 Description=Clever ROS package
 Requires=roscore.service
-After=network.target
+After=roscore.service

 [Service]
 User=pi
-ExecStart=/bin/bash -c ". /home/pi/catkin_ws/devel/setup.sh; \
-                      ROS_HOSTNAME=`hostname`.local exec stdbuf -o L roslaunch clever clever.launch --wait --screen --skip-log-check \
-                      2> >(tee /tmp/clever.err)"
+EnvironmentFile=/lib/systemd/system/roscore.env
+ExecStart=/opt/ros/kinetic/bin/roslaunch clever clever.launch --wait --screen
+Restart=on-failure
+RestartSec=3

 [Install]
 WantedBy=multi-user.target
--- a/builder/assets/hardware_setup.sh
+++ b/builder/assets/hardware_setup.sh
@@ -8,10 +8,6 @@
 #
 # Author: Artem Smirnov <urpylka@gmail.com>
 #
-# Distributed under MIT License (available at https://opensource.org/licenses/MIT).
-# The above copyright notice and this permission notice shall be included in all
-# copies or substantial portions of the Software.
-#

 set -e # Exit immidiately on non-zero result

--- a/builder/assets/init_rpi.sh
+++ b/builder/assets/init_rpi.sh
@@ -8,10 +8,6 @@
 #
 # Author: Artem Smirnov <urpylka@gmail.com>
 #
-# Distributed under MIT License (available at https://opensource.org/licenses/MIT).
-# The above copyright notice and this permission notice shall be included in all
-# copies or substantial portions of the Software.
-#

 set -e # Exit immidiately on non-zero result

@@ -35,15 +31,8 @@ echo_stamp() {
  echo -e ${TEXT}
 }

-NEW_SSID='CLEVER-'$(head -c 100 /dev/urandom | xxd -ps -c 100 | sed -e "s/[^0-9]//g" | cut -c 1-4)
-echo_stamp "Setting SSID to ${NEW_SSID}"
-sudo sed -i.OLD "s/CLEVER/${NEW_SSID}/" /etc/wpa_supplicant/wpa_supplicant.conf
-
-NEW_HOSTNAME=$(echo ${NEW_SSID} | tr '[:upper:]' '[:lower:]')
-echo_stamp "Setting hostname to $NEW_HOSTNAME"
-hostnamectl set-hostname $NEW_HOSTNAME
-sed -i 's/127\.0\.1\.1.*/127.0.1.1\t'${NEW_HOSTNAME}' '${NEW_HOSTNAME}'.local/g' /etc/hosts
-# .local (mdns) hostname added to make it accesable when wlan and ethernet interfaces are down
+echo_stamp "Rename SSID"
+sudo sed -i.OLD "s/CLEVER/CLEVER-$(head -c 100 /dev/urandom | xxd -ps -c 100 | sed -e 's/[^0-9]//g' | cut -c 1-4)/g" /etc/wpa_supplicant/wpa_supplicant.conf

 echo_stamp "Harware setup"
 /root/hardware_setup.sh
--- a/builder/assets/kinetic-rosdep-clever.yaml
+++ b/builder/assets/kinetic-rosdep-clever.yaml
@@ -529,12 +529,6 @@ libogg:
 vl53l1x:
  debian:
    stretch: [ros-kinetic-vl53l1x]
-ws281x:
-  debian:
-    stretch: [ros-kinetic-ws281x]
-led_msgs:
-  debian:
-    stretch: [ros-kinetic-led-msgs]
 interactive_markers:
  debian:
    stretch: [ros-kinetic-interactive-markers]
@@ -547,15 +541,3 @@ tf2_web_republisher:
 image_publisher:
  debian:
    stretch: [ros-kinetic-image-publisher]
-raspberry-kernel-headers:
-  debian:
-    stretch: [raspberry-kernel-headers]
-ddynamic_reconfigure:
-  debian:
-    stretch: [ros-kinetic-ddynamic-reconfigure]
-realsense2_camera:
-  debian:
-    stretch: [ros-kinetic-realsense2-camera]
-ros_pytest:
-  debian:
-    stretch: [ros-kinetic-ros-pytest]
--- a/builder/assets/launch.nanorc
+++ b/builder/assets/launch.nanorc
@@ -1,22 +0,0 @@
-## ROS .launch files (which are actually xml files)
-
-syntax "launch" "\.(launch)$"
-header "<\?xml.*version=.*\?>"
-magic "(XML|SGML) (sub)?document"
-comment "<!--|-->"
-
-# The entire content of the tag:
-color green  start="<" end=">"
-
-# The start and the end of the tag:
-color cyan   "<[^> ]+" ">"
-
-# The strings inside the tag:
-color magenta "\"[^"]*\""
-
-# Comments:
-color yellow start="<!DOCTYPE" end="[/]?>"
-color yellow start="<!--" end="-->"
-
-# Escapes:
-color red    "&[^;]*;"
--- a/builder/assets/pigpiod.service
+++ b/builder/assets/pigpiod.service
@@ -1,8 +0,0 @@
-[Unit]
-Description=Daemon required to control GPIO pins via pigpio
-[Service]
-ExecStart=/usr/bin/pigpiod -l -t 0 -x 0x0FFF3FF0
-ExecStop=/bin/systemctl kill pigpiod
-Type=forking
-[Install]
-WantedBy=multi-user.target
--- a/builder/assets/roscore.env
+++ b/builder/assets/roscore.env
@@ -0,0 +1,10 @@
+ROS_ROOT=/opt/ros/kinetic/share/ros
+ROS_DISTRO=kinetic
+ROS_PACKAGE_PATH=/home/pi/catkin_ws/src:/opt/ros/kinetic/share
+ROS_PORT=11311
+ROS_MASTER_URI=http://localhost:11311
+CMAKE_PREFIX_PATH=/home/pi/catkin_ws/devel:/opt/ros/kinetic
+PATH=/opt/ros/kinetic/bin:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin:/usr/games:/usr/local/games:/snap/bin
+LD_LIBRARY_PATH=/opt/ros/kinetic/lib
+PYTHONPATH=/home/pi/catkin_ws/devel/lib/python2.7/dist-packages:/opt/ros/kinetic/lib/python2.7/dist-packages
+ROS_HOSTNAME=raspberrypi.local
--- a/builder/assets/roscore.service
+++ b/builder/assets/roscore.service
@@ -4,7 +4,8 @@ After=network.target

 [Service]
 User=pi
-ExecStart=/bin/sh -c ". /opt/ros/kinetic/setup.sh; ROS_HOSTNAME=`hostname`.local exec roscore"
+EnvironmentFile=/lib/systemd/system/roscore.env
+ExecStart=/opt/ros/kinetic/bin/roscore
 Restart=on-failure
 RestartSec=3

--- a/builder/assets/rosled.service
+++ b/builder/assets/rosled.service
@@ -0,0 +1,14 @@
+[Unit]
+Description=ROS ws281x support
+Requires=roscore.service
+After=roscore.service
+
+[Service]
+EnvironmentFile=/lib/systemd/system/roscore.env
+ExecStart=/opt/ros/kinetic/bin/roslaunch ros_ws281x clever4.launch --wait --screen
+Restart=on-failure
+RestartSec=3
+TimeoutSec=infinity
+
+[Install]
+WantedBy=multi-user.target
--- a/builder/image-build.sh
+++ b/builder/image-build.sh
@@ -1,21 +1,17 @@
 #! /usr/bin/env bash

 #
-# Script for build the image. Used builder script of the target repo.
+# Script for build the image. Used builder script of the target repo
 # For build: docker run --privileged -it --rm -v /dev:/dev -v $(pwd):/builder/repo smirart/builder
 #
 # Copyright (C) 2018 Copter Express Technologies
 #
 # Author: Artem Smirnov <urpylka@gmail.com>
 #
-# Distributed under MIT License (available at https://opensource.org/licenses/MIT).
-# The above copyright notice and this permission notice shall be included in all
-# copies or substantial portions of the Software.
-#

 set -e # Exit immidiately on non-zero result

-SOURCE_IMAGE="https://downloads.raspberrypi.org/raspbian_lite/images/raspbian_lite-2019-04-09/2019-04-08-raspbian-stretch-lite.zip"
+SOURCE_IMAGE="https://downloads.raspberrypi.org/raspbian_lite/images/raspbian_lite-2018-11-15/2018-11-13-raspbian-stretch-lite.zip"

 export DEBIAN_FRONTEND=${DEBIAN_FRONTEND:='noninteractive'}
 export LANG=${LANG:='C.UTF-8'}
@@ -109,15 +105,14 @@ ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} exec ${SCRIPTS_DIR}'/image-network.
 [[ $(arch) == 'armv7l' ]] && NUMBER_THREADS=1 || NUMBER_THREADS=$(nproc --all)
 # Clever
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/clever.service' '/lib/systemd/system/'
+${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/roscore.env' '/lib/systemd/system/'
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/roscore.service' '/lib/systemd/system/'
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/kinetic-rosdep-clever.yaml' '/etc/ros/rosdep/'
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/ros_python_paths' '/etc/sudoers.d/'
-${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/pigpiod.service' '/lib/systemd/system/'
-${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/launch.nanorc' '/usr/share/nano/'
 # ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/kinetic-ros-clever.rosinstall' '/home/pi/ros_catkin_ws/'
 # Add PX4 udev rules
-${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${REPO_DIR}'/clever/config/99-px4fmu.rules' '/lib/udev/rules.d/'
-# Add rename script
+${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/99-px4fmu.rules' '/lib/udev/rules.d/'
+${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/rosled.service' '/lib/systemd/system/'
 ${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'

--- a/builder/image-init.sh
+++ b/builder/image-init.sh
@@ -1,16 +1,12 @@
 #! /usr/bin/env bash

 #
-# Script for image initialisation
+# Script for initialisation image
 #
 # Copyright (C) 2018 Copter Express Technologies
 #
 # Author: Artem Smirnov <urpylka@gmail.com>
 #
-# Distributed under MIT License (available at https://opensource.org/licenses/MIT).
-# The above copyright notice and this permission notice shall be included in all
-# copies or substantial portions of the Software.
-#

 set -e # Exit immidiately on non-zero result

--- a/builder/image-network.sh
+++ b/builder/image-network.sh
@@ -1,16 +1,12 @@
 #! /usr/bin/env bash

 #
-# Script for network configuration
+# Script for network configure
 #
 # Copyright (C) 2018 Copter Express Technologies
 #
 # Author: Artem Smirnov <urpylka@gmail.com>
 #
-# Distributed under MIT License (available at https://opensource.org/licenses/MIT).
-# The above copyright notice and this permission notice shall be included in all
-# copies or substantial portions of the Software.
-#

 set -e # Exit immidiately on non-zero result

--- a/builder/image-ros.sh
+++ b/builder/image-ros.sh
@@ -8,10 +8,6 @@
 #
 # Author: Artem Smirnov <urpylka@gmail.com>
 #
-# Distributed under MIT License (available at https://opensource.org/licenses/MIT).
-# The above copyright notice and this permission notice shall be included in all
-# copies or substantial portions of the Software.
-#

 set -e # Exit immidiately on non-zero result

@@ -46,10 +42,9 @@ echo_stamp() {
 my_travis_retry() {
  local result=0
  local count=1
-  local max_count=50
-  while [ $count -le $max_count ]; do
+  while [ $count -le 3 ]; do
    [ $result -ne 0 ] && {
-      echo -e "\nThe command \"$@\" failed. Retrying, $count of $max_count.\n" >&2
+      echo -e "\n${ANSI_RED}The command \"$@\" failed. Retrying, $count of 3.${ANSI_RESET}\n" >&2
    }
    # ! { } ignores set -e, see https://stackoverflow.com/a/4073372
    ! { "$@"; result=$?; }
@@ -58,21 +53,21 @@ my_travis_retry() {
    sleep 1
  done

-  [ $count -gt $max_count ] && {
-    echo -e "\nThe command \"$@\" failed $max_count times.\n" >&2
+  [ $count -gt 3 ] && {
+    echo -e "\n${ANSI_RED}The command \"$@\" failed 3 times.${ANSI_RESET}\n" >&2
  }

  return $result
 }

 # TODO: 'kinetic-rosdep-clever.yaml' should add only if we use our repo?
-echo_stamp "Init rosdep"
-my_travis_retry rosdep init
-echo "yaml file:///etc/ros/rosdep/kinetic-rosdep-clever.yaml" >> /etc/ros/rosdep/sources.list.d/20-default.list
-my_travis_retry rosdep update
+echo_stamp "Init rosdep" \
+&& rosdep init \
+&& echo "yaml file:///etc/ros/rosdep/kinetic-rosdep-clever.yaml" >> /etc/ros/rosdep/sources.list.d/20-default.list \
+&& rosdep update

 echo_stamp "Populate rosdep for ROS user"
-my_travis_retry sudo -u pi rosdep update
+sudo -u pi rosdep update

 resolve_rosdep() {
  # TEMPLATE: resolve_rosdep <CATKIN_PATH> <ROS_DISTRO> <OS_DISTRO> <OS_VERSION>
@@ -142,9 +137,9 @@ fi

 export ROS_IP='127.0.0.1' # needed for running tests

-echo_stamp "Reconfiguring Clever repository for simplier unshallowing"
-cd /home/pi/catkin_ws/src/clever
-git config remote.origin.fetch "+refs/heads/*:refs/remotes/origin/*"
+echo_stamp "Adding ros_ws281x ROS package"
+cd /home/pi/catkin_ws/src
+git clone https://github.com/sfalexrog/ros_ws281x

 echo_stamp "Installing CLEVER" \
 && cd /home/pi/catkin_ws/src/clever \
@@ -154,12 +149,17 @@ echo_stamp "Installing CLEVER" \
 && 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 -j2 -DCMAKE_BUILD_TYPE=Release \
+&& catkin_make -j2 -DCMAKE_BUILD_TYPE=Release -DROS_WS2811_REAL_LIB=ON \
+&& 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 "Enabling ROS LED service"
+systemctl enable rosled
+
 echo_stamp "Build CLEVER documentation"
 cd /home/pi/catkin_ws/src/clever
 NPM_CONFIG_UNSAFE_PERM=true npm install gitbook-cli -g
@@ -169,22 +169,18 @@ 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-ws281x \
-    ros-kinetic-opencv3=3.3.19-0stretch \
-    ros-kinetic-rosshow
+    ros-kinetic-opencv3=3.3.19-0stretch

 # TODO move GeographicLib datasets to Mavros debian package
-echo_stamp "Install GeographicLib datasets (needed for mavros)" \
+echo_stamp "Install GeographicLib datasets (needs for mavros)" \
 && wget -qO- https://raw.githubusercontent.com/mavlink/mavros/master/mavros/scripts/install_geographiclib_datasets.sh | bash

-echo_stamp "Running tests"
-catkin_make run_tests && catkin_test_results
-
 echo_stamp "Change permissions for catkin_ws"
 chown -Rf pi:pi /home/pi/catkin_ws

@@ -193,7 +189,7 @@ cat << EOF >> /home/pi/.bashrc
 LANG='C.UTF-8'
 LC_ALL='C.UTF-8'
 ROS_DISTRO='kinetic'
-export ROS_HOSTNAME=\`hostname\`.local
+export ROS_HOSTNAME='raspberrypi.local'
 source /opt/ros/kinetic/setup.bash
 source /home/pi/catkin_ws/devel/setup.bash
 EOF
--- a/builder/image-software.sh
+++ b/builder/image-software.sh
@@ -1,16 +1,12 @@
 #! /usr/bin/env bash

 #
-# Script for installing software to the image.
+# Script for install software to the image.
 #
 # Copyright (C) 2018 Copter Express Technologies
 #
 # Author: Artem Smirnov <urpylka@gmail.com>
 #
-# Distributed under MIT License (available at https://opensource.org/licenses/MIT).
-# The above copyright notice and this permission notice shall be included in all
-# copies or substantial portions of the Software.
-#

 set -e # Exit immidiately on non-zero result

@@ -60,14 +56,14 @@ my_travis_retry() {
 echo_stamp "Install apt keys & repos"

 # TODO: This STDOUT consist 'OK'
-curl http://deb.coex.tech/aptly_repo_signing.key 2> /dev/null | apt-key add -
+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~deb9u4 > /dev/null \
-&& apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654
+&& apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key 421C365BD9FF1F717815A3895523BAEEB01FA116

 echo "deb http://packages.ros.org/ros/ubuntu stretch main" > /etc/apt/sources.list.d/ros-latest.list
-echo "deb http://deb.coex.tech/rpi-ros-kinetic stretch main" > /etc/apt/sources.list.d/rpi-ros-kinetic.list
-echo "deb http://deb.coex.tech/clever stretch main" > /etc/apt/sources.list.d/clever.list
+echo "deb http://repo.coex.space/rpi-ros-kinetic stretch main" > /etc/apt/sources.list.d/rpi-ros-kinetic.list
+echo "deb http://repo.coex.space/clever stretch main" > /etc/apt/sources.list.d/clever.list

 echo_stamp "Update apt cache"

@@ -88,9 +84,9 @@ lsof=4.89+dfsg-0.1 \
 git \
 dnsmasq=2.76-5+rpt1+deb9u1  \
 tmux=2.3-4 \
-vim \
+vim=2:8.0.0197-4+deb9u1 \
 cmake=3.7.2-1 \
-libjpeg8=8d1-2 \
+libjpeg8-dev=8d1-2 \
 tcpdump \
 ltrace \
 libpoco-dev=1.7.6+dfsg1-5+deb9u1 \
@@ -103,23 +99,14 @@ libffi-dev \
 monkey=1.6.9-1 \
 pigpio python-pigpio python3-pigpio \
 i2c-tools \
-espeak espeak-data python-espeak \
 ntpdate \
 python-dev \
 python3-dev \
-mjpg-streamer=2.0 \
 && echo_stamp "Everything was installed!" "SUCCESS" \
 || (echo_stamp "Some packages wasn't installed!" "ERROR"; exit 1)

 echo_stamp "Updating kernel to fix camera bug"
-apt-get install --no-install-recommends -y \
-	raspberrypi-kernel=1.20190819~stretch-1 \
-	raspberrypi-bootloader=1.20190819~stretch-1 \
-	libraspberrypi-bin=1.20190819~stretch-1 \
-	libraspberrypi-dev=1.20190819~stretch-1 \
-	libraspberrypi0=1.20190819~stretch-1 \
-	wireless-regdb=2018.05.09-0~rpt1 \
-	wpasupplicant=2:2.6-21~bpo9~rpt1
+apt-get install --no-install-recommends -y raspberrypi-kernel=1.20190215-1

 # Deny byobu to check available updates
 sed -i "s/updates_available//" /usr/share/byobu/status/status
@@ -152,6 +139,9 @@ mv /etc/monkey/sites/default /etc/monkey/sites/default.orig
 mv /root/monkey /etc/monkey/sites/default
 systemctl enable monkey.service

+echo_stamp "Install paho-mqtt"
+my_travis_retry pip install paho-mqtt
+
 echo_stamp "Install Node.js"
 cd /home/pi
 wget https://nodejs.org/dist/v10.15.0/node-v10.15.0-linux-armv6l.tar.gz
@@ -167,9 +157,6 @@ syntax on
 autocmd BufNewFile,BufRead *.launch set syntax=xml
 EOF

-echo_stamp "Change default keyboard layout to US"
-sed -i 's/XKBLAYOUT="gb"/XKBLAYOUT="us"/g' /etc/default/keyboard
-
 echo_stamp "Attempting to kill dirmngr"
 gpgconf --kill dirmngr
 # dirmngr is only used by apt-key, so we can safely kill it.
--- a/builder/image-validate.sh
+++ b/builder/image-validate.sh
@@ -7,10 +7,6 @@
 #
 # Author: Oleg Kalachev <okalachev@gmail.com>
 #
-# Distributed under MIT License (available at https://opensource.org/licenses/MIT).
-# The above copyright notice and this permission notice shall be included in all
-# copies or substantial portions of the Software.
-#

 set -ex

--- a/builder/test/tests.py
+++ b/builder/test/tests.py
@@ -25,6 +25,6 @@ import pymavlink
 from pymavlink import mavutil
 import rpi_ws281x
 import pigpio
-from espeak import espeak

 print cv2.getBuildInformation()
+
--- a/builder/test/tests.sh
+++ b/builder/test/tests.sh
@@ -28,8 +28,6 @@ monkey --version
 pigpiod -v
 i2cdetect -V
 butterfly -h
-espeak --version
-mjpg_streamer --version

 # ros stuff

@@ -48,4 +46,4 @@ rosversion rosserial
 rosversion usb_cam
 rosversion cv_camera
 rosversion web_video_server
-rosversion rosshow
+rosversion ros_ws281x
--- a/check_assets_size.py
+++ b/check_assets_size.py
@@ -1,34 +0,0 @@
-#!/usr/bin/env python3
-
-import os
-import sys
-
-def human_size(num, suffix='B'):
-    for unit in ['', 'Ki', 'Mi', 'Gi', 'Ti', 'Pi', 'Ei', 'Zi']:
-        if abs(num) < 1024.0:
-            return "%3.1f %s%s" % (num, unit, suffix)
-        num /= 1024.0
-    return "%.1f %s%s" % (num, 'Yi', suffix)
-
-SIZE_LIMIT = 800 * 1024
-EXCLUDE = 'rviz.png', 'ssid.png', 'sitl_docker_demo.png', 'qgc-params.png', 'butterfly.png', \
-    'Clever main.png', 'fpv_3.png', '1_4.png', 'fpv_4.png', 'detal1.png', 'lockradio.png', \
-    'qground.png', 'allElements.png', 'download-log.png', 'explosion.png', 'rqt.png', \
-    'cl3_mountBEC.JPG', 'cl3_mountRpiCamera.JPG', 'clever4-front-black-large.png', \
-    'qgc-battery.png', 'qgc-radio.png', 'qgc-cal-acc.png', 'qgc-esc.png', 'qgc-cal-compass.png', \
-    'qgc.png', 'qgc-parameters.png', 'clever4-front-white-large.png', 'qgc-modes.png', \
-    'qgc-requires-setup.png', 'clever4-front-white.png', 'clever4-kit-white.png', '26_1.png'
-
-code = 0
-
-for root, dirs, files in os.walk('docs/'):
-    for f in files:
-        if f not in EXCLUDE:
-            path = os.path.join(root, f)
-            size = os.path.getsize(path)
-            if size > SIZE_LIMIT:
-                print('\x1b[1;31mFile too large ({}): {}\x1b[0m'.format(human_size(size), path), \
-                    file=sys.stderr)
-                code = 1
-
-sys.exit(code)
--- a/check_unused_assets.py
+++ b/check_unused_assets.py
@@ -1,22 +0,0 @@
-#!/usr/bin/env python3
-
-import os
-import sys
-import subprocess
-
-EXCLUDE = ('clever4-front-white.png', '.DS_Store', 'clever4-front-black-large.png')
-code = 0
-
-os.chdir('./docs')
-
-for root, dirs, files in os.walk('assets'):
-    for f in files:
-        if f not in EXCLUDE:
-            path = os.path.join(root, f)
-            try:
-                subprocess.check_output(['grep', '-F', '-r', path, './ru', './en'])
-            except subprocess.CalledProcessError:
-                print('\x1b[1;31mAssets file {} is not used\x1b[0m'.format(path))
-                code = 1
-
-sys.exit(code)
--- a/clever/CMakeLists.txt
+++ b/clever/CMakeLists.txt
@@ -80,7 +80,6 @@ add_service_files(
  SetVelocity.srv
  SetAttitude.srv
  SetRates.srv
-  SetLEDEffect.srv
 )

 ## Generate actions in the 'action' folder
@@ -163,9 +162,9 @@ add_executable(rc src/rc.cpp)

 add_executable(camera_markers src/camera_markers.cpp)

-add_executable(vpe_publisher src/vpe_publisher.cpp)
+add_executable(frames src/frames.cpp)

-add_executable(led src/led.cpp)
+add_executable(vpe_publisher src/vpe_publisher.cpp)

 target_link_libraries(simple_offboard
  ${catkin_LIBRARIES}
@@ -176,14 +175,12 @@ target_link_libraries(rc ${catkin_LIBRARIES})

 target_link_libraries(camera_markers ${catkin_LIBRARIES})

+target_link_libraries(frames ${catkin_LIBRARIES})
+
 target_link_libraries(vpe_publisher ${catkin_LIBRARIES})

-target_link_libraries(led ${catkin_LIBRARIES})
-
 add_dependencies(simple_offboard clever_generate_messages_cpp)

-add_dependencies(led clever_generate_messages_cpp)
-
 ## Rename C++ executable without prefix
 ## The above recommended prefix causes long target names, the following renames the
 ## target back to the shorter version for ease of user use
@@ -234,19 +231,6 @@ target_link_libraries(clever
 #   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 # )

-# Only install udev rules when building a Debian package
-# FIXME: Other operating systems may have other prefixes
-string(FIND ${CMAKE_INSTALL_PREFIX} "/opt/ros" _PREFIX_INDEX)
-if (${_PREFIX_INDEX} EQUAL 0)
-  message(STATUS "Building as a Debian package - adding udev rules as installable files")
-  install(FILES
-    config/99-px4fmu.rules
-    DESTINATION /lib/udev/rules.d
-  )
-else()
-  message(STATUS "Building in a user workspace - not adding udev rules")
-endif()
-
 #############
 ## Testing ##
 #############
@@ -259,8 +243,3 @@ endif()

 ## 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()
--- a/clever/launch/aruco.launch
+++ b/clever/launch/aruco.launch
@@ -1,7 +1,7 @@
 <launch>
    <arg name="aruco_detect" default="true"/>
-    <arg name="aruco_map" default="false"/>
-    <arg name="aruco_vpe" default="false"/>
+    <arg name="aruco_map" default="true"/>
+    <arg name="aruco_vpe" default="true"/>

    <!-- For additional help go to https://clever.copterexpress.com/aruco.html -->

@@ -9,7 +9,6 @@
    <node name="aruco_detect" pkg="nodelet" if="$(arg aruco_detect)" type="nodelet" args="load aruco_pose/aruco_detect nodelet_manager" output="screen" clear_params="true">
        <remap from="image_raw" to="main_camera/image_raw"/>
        <remap from="camera_info" to="main_camera/camera_info"/>
-        <remap from="map_markers" to="aruco_map/markers" if="$(arg aruco_map)"/>
        <param name="estimate_poses" value="true"/>
        <param name="send_tf" value="true"/>
        <param name="known_tilt" value="map"/>
@@ -21,13 +20,10 @@
        <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="map" value="$(find aruco_pose)/map/nti_novgorod.txt"/>
        <param name="known_tilt" value="map"/>
-        <param name="image_axis" value="true"/>
        <param name="frame_id" value="aruco_map_detected" if="$(arg aruco_vpe)"/>
        <param name="frame_id" value="aruco_map" unless="$(arg aruco_vpe)"/>
-        <param name="markers/frame_id" value="aruco_map"/>
-        <param name="markers/child_frame_id_prefix" value="aruco_"/>
    </node>

    <!-- vpe publisher from aruco markers -->
--- a/clever/launch/clever.launch
+++ b/clever/launch/clever.launch
@@ -5,14 +5,11 @@
    <arg name="web_video_server" default="true"/>
    <arg name="rosbridge" default="true"/>
    <arg name="main_camera" default="true"/>
-    <arg name="optical_flow" default="false"/>
-    <arg name="aruco" default="false"/>
-    <arg name="rangefinder_vl53l1x" default="false"/>
-    <arg name="led" default="false"/>
-    <arg name="rc" default="true"/>
-
-    <!-- log formatting -->
-    <env name="ROSCONSOLE_FORMAT" value="[${severity}] [${time}]: ${logger}: ${message}"/>
+    <arg name="optical_flow" default="true"/>
+    <arg name="aruco" default="true"/>
+    <arg name="rc" default="false"/>
+    <arg name="rangefinder_vl53l1x" default="true"/>
+    <arg name="arduino" default="false"/>

    <!-- mavros -->
    <include file="$(find clever)/launch/mavros.launch">
@@ -24,7 +21,6 @@
    <!-- 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">
        <param name="default_stream_type" value="ros_compressed"/>
-        <param name="publish_rate" value="1.0"/>
    </node>

    <!-- aruco markers -->
@@ -48,7 +44,11 @@
    <node name="simple_offboard" pkg="clever" type="simple_offboard" output="screen" clear_params="true">
        <param name="reference_frames/body" value="map"/>
        <param name="reference_frames/base_link" value="map"/>
-        <param name="reference_frames/navigate_target" value="map"/>
+    </node>
+
+    <!-- Auxiliary frames -->
+    <node name="frames" pkg="clever" type="frames" output="screen">
+        <param name="body/frame_id" value="body"/>
    </node>

    <!-- main camera -->
@@ -61,13 +61,17 @@
    <node name="tf2_web_republisher" pkg="tf2_web_republisher" type="tf2_web_republisher" output="screen" if="$(arg rosbridge)"/>

    <!-- vl53l1x ToF rangefinder -->
-    <node name="rangefinder" pkg="vl53l1x" type="vl53l1x_node" output="screen" if="$(arg rangefinder_vl53l1x)">
+    <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_sub"/> <!-- redirect data to FCU -->
    </node>

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

    <!-- camera is oriented downward, camera cable goes backward [option 1] -->
-    <node pkg="tf2_ros" type="static_transform_publisher" name="main_camera_frame" args="0.05 0 -0.07 -1.5707963 0 3.1415926 base_link main_camera_optical"/>
+    <!-- <node pkg="tf2_ros" type="static_transform_publisher" name="main_camera_frame" args="0.05 0 -0.07 -1.5707963 0 3.1415926 base_link main_camera_optical"/> -->

    <!-- camera is oriented downward, camera cable goes forward  [option 2] -->
-    <!--<node pkg="tf2_ros" type="static_transform_publisher" name="main_camera_frame" args="0.05 0 -0.07 1.5707963 0 3.1415926 base_link main_camera_optical"/>-->
+        <node pkg="tf2_ros" type="static_transform_publisher" name="main_camera_frame" args="-0.04 0 -0.08 1.5707963 0 3.1415926 base_link main_camera_optical"/>

    <!-- camera is oriented upward, camera cable goes backward   [option 3] -->
    <!--<node pkg="tf2_ros" type="static_transform_publisher" name="main_camera_frame" args="0.05 0 0.07 1.5707963 0 0 base_link main_camera_optical"/>-->
@@ -24,7 +24,6 @@
        <param name="rate" value="100"/> <!-- poll rate -->
        <param name="cv_cap_prop_fps" value="40"/> <!-- camera FPS -->
        <param name="capture_delay" value="0.02"/> <!-- approximate delay on frame retrieving -->
-        <param name="rescale_camera_info" value="true"/> <!-- automatically rescale camera calibration info -->

        <!-- camera resolution, NOTE: camera_info file should match it -->
        <param name="image_width" value="320"/>
--- a/clever/launch/mavros.launch
+++ b/clever/launch/mavros.launch
@@ -4,7 +4,6 @@
    <arg name="gcs_bridge" default="tcp"/>
    <arg name="viz" default="true"/>
    <arg name="respawn" default="true"/>
-    <arg name="distance_sensor_remap" default="rangefinder/range"/>

    <node pkg="mavros" type="mavros_node" name="mavros" required="false" clear_params="true" respawn="$(arg respawn)" unless="$(eval fcu_conn == 'none')" respawn_delay="1" output="screen">
        <!-- UART connection -->
@@ -28,9 +27,6 @@
        <!-- basic params -->
        <rosparam command="load" file="$(find clever)/launch/mavros_config.yaml"/>

-        <!-- remap rangefinder -->
-        <remap from="mavros/distance_sensor/rangefinder_sub" to="rangefinder/range"/>
-
        <rosparam param="plugin_whitelist">
            - altitude
            - command
@@ -56,19 +52,11 @@
        </rosparam>
    </node>

-    <!-- remapped distance_sensor config -->
-    <rosparam param="$(arg distance_sensor_remap)" if="$(eval bool(distance_sensor_remap))">
-        subscriber: true
-        id: 1
-        orientation: PITCH_270
-        covariance: 1  # cm
-    </rosparam>
-
    <!-- Rangefinders frame -->
    <node pkg="tf2_ros" type="static_transform_publisher" name="rangefinder_frame" args="0 0 -0.05 0 1.5707963268 0 base_link rangefinder"/>

    <!-- Copter visualization -->
-    <node name="visualization" pkg="mavros_extras" type="visualization" if="$(arg viz)">
+    <node name="copter_visualization" pkg="mavros_extras" type="copter_visualization" if="$(arg viz)">
        <remap to="mavros/local_position/pose" from="local_position"/>
        <remap to="mavros/setpoint_position/local" from="local_setpoint"/>
        <param name="fixed_frame_id" value="map"/>
--- a/clever/launch/mavros_config.yaml
+++ b/clever/launch/mavros_config.yaml
@@ -77,6 +77,9 @@ distance_sensor:
      field_of_view: 0.5
  rangefinder_sub:
      subscriber: true
+      id: 1
+      orientation: PITCH_270
+      covariance: 1  # cm

 # fake_gps
 fake_gps:
--- a/clever/package.xml
+++ b/clever/package.xml
@@ -26,19 +26,17 @@
  <depend>geometry_msgs</depend>
  <depend>sensor_msgs</depend>
  <depend>visualization_msgs</depend>
-  <depend>led_msgs</depend>
  <depend>geographiclib</depend>
  <depend>nodelet</depend>
  <depend>mavros</depend>
  <depend>mavros_extras</depend>
+  <depend>lxml</depend>
  <depend>cv_camera</depend>
  <depend>cv_bridge</depend>
  <depend>opencv3</depend>
+  <depend>mjpg-streamer</depend>
  <depend>rosbridge_server</depend>
  <depend>web_video_server</depend>
-  <depend>tf2_web_republisher</depend>
-  <depend>python-lxml</depend>
-  <exec_depend>python-pymavlink</exec_depend>
  <!-- Use test_depend for packages you need only for testing: -->
  <!--   <test_depend>gtest</test_depend> -->

--- a/clever/requirements.txt
+++ b/clever/requirements.txt
@@ -1,5 +1,6 @@
-flask==1.1.1
+flask==0.12.3
 docopt==0.6.2
 geopy==1.11.0
+pymavlink==2.2.10
 smbus2==0.2.1
 VL53L1X==0.0.2
--- a/clever/src/frames.cpp
+++ b/clever/src/frames.cpp
@@ -0,0 +1,63 @@
+/*
+ * Auxiliary TF frames for CLEVER drone kit:
+ * - Body frame (drone body with zero pitch and roll).
+ * - TODO: REP-0105 `odom` frame emulation: continuous frame without discrete jumps.
+ * - TODO: Terrain frame (base on ALTITUDE message).
+ * - TODO: map_upside_down frame
+ * - TODO: home frame?
+ *
+ * Copyright (C) 2018 Copter Express Technologies
+ *
+ * Author: Oleg Kalachev <okalachev@gmail.com>
+ *
+ * Distributed under MIT License (available at https://opensource.org/licenses/MIT).
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ */
+
+// TODO: consider implementing as a mavros plugin
+
+#include <string>
+#include <memory>
+#include <ros/ros.h>
+#include <tf/transform_datatypes.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include <geometry_msgs/TransformStamped.h>
+#include <geometry_msgs/PoseStamped.h>
+
+using std::string;
+
+static std::shared_ptr<tf2_ros::TransformBroadcaster> br;
+static geometry_msgs::TransformStamped body;
+
+inline void publishBody(const geometry_msgs::PoseStamped& pose)
+{
+	// Get only yaw from pose
+	tf::Quaternion q;
+	q.setRPY(0, 0, tf::getYaw(pose.pose.orientation));
+	tf::quaternionTFToMsg(q, body.transform.rotation);
+
+	body.transform.translation.x = pose.pose.position.x;
+	body.transform.translation.y = pose.pose.position.y;
+	body.transform.translation.z = pose.pose.position.z;
+	body.header.frame_id = pose.header.frame_id;
+	body.header.stamp = pose.header.stamp;
+	br->sendTransform(body);
+}
+
+void poseCallback(const geometry_msgs::PoseStamped& pose)
+{
+	publishBody(pose);
+}
+
+int main(int argc, char **argv) {
+	ros::init(argc, argv, "frames");
+	ros::NodeHandle nh, nh_priv("~");
+
+	nh_priv.param<string>("body/frame_id", body.child_frame_id, "body");
+
+	br = std::make_shared<tf2_ros::TransformBroadcaster>();
+	ros::Subscriber pose_sub = nh.subscribe("mavros/local_position/pose", 1, &poseCallback);
+	ROS_INFO("frames: ready");
+	ros::spin();
+}
--- a/clever/src/led.cpp
+++ b/clever/src/led.cpp
@@ -1,321 +0,0 @@
-/*
- * High level control for the LED strip
- * Indicate flight events with the LED strip
- * Copyright (C) 2019 Copter Express Technologies
- *
- * Author: Oleg Kalachev <okalachev@gmail.com>
- *
- * Distributed under MIT License (available at https://opensource.org/licenses/MIT).
- * The above copyright notice and this permission notice shall be included in all
- * copies or substantial portions of the Software.
- */
-
-#include <ros/ros.h>
-#include <string>
-#include <boost/algorithm/string.hpp>
-
-#include <clever/SetLEDEffect.h>
-#include <led_msgs/SetLEDs.h>
-#include <led_msgs/LEDState.h>
-#include <led_msgs/LEDStateArray.h>
-
-#include <sensor_msgs/BatteryState.h>
-#include <mavros_msgs/State.h>
-#include <rosgraph_msgs/Log.h>
-
-clever::SetLEDEffect::Request current_effect;
-int led_count;
-ros::Timer timer;
-ros::Time start_time;
-double blink_rate, blink_fast_rate, flash_delay, fade_period, wipe_period, rainbow_period;
-double low_battery_threshold;
-bool blink_state;
-led_msgs::SetLEDs set_leds;
-led_msgs::LEDStateArray state, start_state;
-ros::ServiceClient set_leds_srv;
-mavros_msgs::State mavros_state;
-int counter;
-
-void callSetLeds()
-{
-	bool res = set_leds_srv.call(set_leds);
-	if (!res) {
-		ROS_WARN_THROTTLE(5, "Error calling set_leds service");
-	} else if (!set_leds.response.success) {
-		ROS_WARN_THROTTLE(5, "Calling set_leds failed: %s", set_leds.response.message.c_str());
-	}
-}
-
-void rainbow(uint8_t n, uint8_t& r, uint8_t& g, uint8_t& b)
-{
-	if (n < 255 / 3) {
-		r = n * 3;
-		g = 255 - n * 3;
-		b = 0;
-	} else if (n < 255 / 3 * 2) {
-		n -= 255 / 3;
-		r = 255 - n * 3;
-		g = 0;
-		b = n * 3;
-	} else {
-		n -= 255 / 3 * 2;
-		r = 0;
-		g = n * 3;
-		b = 255 - n * 3;
-	}
-}
-
-void fill(uint8_t r, uint8_t g, uint8_t b)
-{
-	set_leds.request.leds.resize(led_count);
-	for (int i = 0; i < led_count; i++) {
-		set_leds.request.leds[i].index = i;
-		set_leds.request.leds[i].r = r;
-		set_leds.request.leds[i].g = g;
-		set_leds.request.leds[i].b = b;
-	}
-	callSetLeds();
-}
-
-void proceed(const ros::TimerEvent& event)
-{
-	counter++;
-	uint8_t r, g, b;
-	set_leds.request.leds.clear();
-	set_leds.request.leds.resize(led_count);
-
-	if (current_effect.effect == "blink" || current_effect.effect == "blink_fast") {
-		blink_state = !blink_state;
-		// toggle all leds
-		if (blink_state) {
-			fill(current_effect.r, current_effect.g, current_effect.b);
-		} else {
-			fill(0, 0, 0);
-		}
-
-	} else if (current_effect.effect == "fade") {
-		// fade all leds from starting state
-		double passed = std::min((event.current_real - start_time).toSec() / fade_period, 1.0);
-		double one_minus_passed = 1 - passed;
-		for (int i = 0; i < led_count; i++) {
-			set_leds.request.leds[i].index = i;
-			set_leds.request.leds[i].r = one_minus_passed * start_state.leds[i].r + passed * current_effect.r;
-			set_leds.request.leds[i].g = one_minus_passed * start_state.leds[i].g + passed * current_effect.g;
-			set_leds.request.leds[i].b = one_minus_passed * start_state.leds[i].b + passed * current_effect.b;
-		}
-		callSetLeds();
-		if (passed >= 1.0) {
-			// fade finished
-			timer.stop();
-		}
-
-	} else if (current_effect.effect == "wipe") {
-		set_leds.request.leds.resize(1);
-		set_leds.request.leds[0].index = counter - 1;
-		set_leds.request.leds[0].r = current_effect.r;
-		set_leds.request.leds[0].g = current_effect.g;
-		set_leds.request.leds[0].b = current_effect.b;
-		callSetLeds();
-		if (counter == led_count) {
-			// wipe finished
-			timer.stop();
-		}
-
-	} else if (current_effect.effect == "rainbow_fill") {
-		rainbow(counter % 255, r, g, b);
-		for (int i = 0; i < led_count; i++) {
-			set_leds.request.leds[i].index = i;
-			set_leds.request.leds[i].r = r;
-			set_leds.request.leds[i].g = g;
-			set_leds.request.leds[i].b = b;
-		}
-		callSetLeds();
-
-	} else if (current_effect.effect == "rainbow") {
-		for (int i = 0; i < led_count; i++) {
-			int pos = (int)round(counter + (255.0 * i / led_count)) % 255;
-			rainbow(pos % 255, r, g, b);
-			set_leds.request.leds[i].index = i;
-			set_leds.request.leds[i].r = r;
-			set_leds.request.leds[i].g = g;
-			set_leds.request.leds[i].b = b;
-		}
-		callSetLeds();
-	}
-}
-
-bool setEffect(clever::SetLEDEffect::Request& req, clever::SetLEDEffect::Response& res)
-{
-	res.success = true;
-
-	if (req.effect == "") {
-		req.effect = "fill";
-	}
-
-	if (req.effect != "flash" && req.effect != "fill" && current_effect.effect == req.effect &&
-	    current_effect.r == req.r && current_effect.g == req.g && current_effect.b == req.b) {
-		res.message = "Effect already set, skip";
-		return true;
-	}
-
-	if (req.effect == "fill") {
-		fill(req.r, req.g, req.b);
-
-	} else if (req.effect == "blink") {
-		timer.setPeriod(ros::Duration(1 / blink_rate), true);
-		timer.start();
-
-	} else if (req.effect == "blink_fast") {
-		timer.setPeriod(ros::Duration(1 / blink_fast_rate), true);
-		timer.start();
-
-	} else if (req.effect == "fade") {
-		timer.setPeriod(ros::Duration(0.05), true);
-		timer.start();
-
-	} else if (req.effect == "wipe") {
-		timer.setPeriod(ros::Duration(wipe_period / led_count), true);
-		timer.start();
-
-	} else if (req.effect == "flash") {
-		ros::Duration delay(flash_delay);
-		fill(0, 0, 0);
-		delay.sleep();
-		fill(req.r, req.g, req.b);
-		delay.sleep();
-		fill(0, 0, 0);
-		delay.sleep();
-		fill(req.r, req.g, req.b);
-		delay.sleep();
-		fill(0, 0, 0);
-		delay.sleep();
-		if (current_effect.effect == "fill"||
-		    current_effect.effect == "fade" ||
-		    current_effect.effect == "wipe") {
-			// restore previous filling
-			for (int i = 0; i < led_count; i++) {
-				fill(current_effect.r, current_effect.g, current_effect.b);
-			}
-			callSetLeds();
-		}
-		return true; // this effect happens only once
-
-	} else if (req.effect == "rainbow_fill") {
-		timer.setPeriod(ros::Duration(rainbow_period / 255), true);
-		timer.start();
-
-	} else if (req.effect == "rainbow") {
-		timer.setPeriod(ros::Duration(rainbow_period / 255), true);
-		timer.start();
-
-	} else {
-		res.message = "Unknown effect: " + req.effect + ". Available effects are fill, fade, wipe, blink, blink_fast, flash, rainbow, rainbow_fill.";
-		ROS_ERROR("%s", res.message.c_str());
-		res.success = false;
-		return true;
-	}
-
-	// set current effect
-	current_effect = req;
-	counter = 0;
-	start_state = state;
-	start_time = ros::Time::now();
-
-	return true;
-}
-
-void handleState(const led_msgs::LEDStateArray& msg)
-{
-	state = msg;
-	led_count = state.leds.size();
-}
-
-bool notify(const std::string& event)
-{
-	if (ros::param::has("~notify/" + event + "/effect") ||
-	    ros::param::has("~notify/" + event + "/r") ||
-	    ros::param::has("~notify/" + event + "/g") ||
-	    ros::param::has("~notify/" + event + "/b")) {
-		ROS_INFO_THROTTLE(5, "led: notify %s", event.c_str());
-		clever::SetLEDEffect effect;
-		effect.request.effect = ros::param::param("~notify/" + event + "/effect", std::string(""));
-		effect.request.r = ros::param::param("~notify/" + event + "/r", 0);
-		effect.request.g = ros::param::param("~notify/" + event + "/g", 0);
-		effect.request.b = ros::param::param("~notify/" + event + "/b", 0);
-		setEffect(effect.request, effect.response);
-	}
-}
-
-void handleMavrosState(const mavros_msgs::State& msg)
-{
-	if (msg.connected && !mavros_state.connected) {
-		notify("connected");
-	} else if (!msg.connected && mavros_state.connected) {
-		notify("disconnected");
-	} else if (msg.armed && !mavros_state.armed) {
-		notify("armed");
-	} else if (!msg.armed && mavros_state.armed) {
-		notify("disarmed");
-	} else if (msg.mode != mavros_state.mode) {
-		// mode changed
-		std::string mode = boost::algorithm::to_lower_copy(msg.mode);
-		if (mode.find(".") != std::string::npos) {
-			// remove the part before "."
-			mode = mode.substr(mode.find(".") + 1);
-		}
-		notify(mode);
-	}
-	mavros_state = msg;
-}
-
-void handleLog(const rosgraph_msgs::Log& log)
-{
-	if (log.level >= rosgraph_msgs::Log::ERROR) {
-		notify("error");
-	}
-}
-
-void handleBattery(const sensor_msgs::BatteryState& msg)
-{
-	for (auto const& voltage : msg.cell_voltage) {
-		if (voltage < low_battery_threshold) {
-			// notify low battery every time
-			notify("low_battery");
-		}
-	}
-}
-
-int main(int argc, char **argv)
-{
-	ros::init(argc, argv, "led");
-	ros::NodeHandle nh, nh_priv("~");
-
-	nh_priv.param("blink_rate", blink_rate, 2.0);
-	nh_priv.param("blink_fast_rate", blink_fast_rate, blink_rate * 2);
-	nh_priv.param("fade_period", fade_period, 0.5);
-	nh_priv.param("wipe_period", wipe_period, 0.5);
-	nh_priv.param("flash_delay", flash_delay, 0.1);
-	nh_priv.param("rainbow_period", rainbow_period, 5.0);
-
-	nh_priv.param("notify/low_battery/threshold", low_battery_threshold, 3.7);
-
-	ros::service::waitForService("set_leds"); // cannot work without set_leds service
-	set_leds_srv = nh.serviceClient<led_msgs::SetLEDs>("set_leds", true);
-
-	// wait for leds count info
-	handleState(*ros::topic::waitForMessage<led_msgs::LEDStateArray>("state", nh));
-
-	auto state_sub = nh.subscribe("state", 1, &handleState);
-
-	auto set_effect = nh.advertiseService("set_effect", &setEffect);
-
-	auto mavros_state_sub = nh.subscribe("/mavros/state", 1, &handleMavrosState);
-	auto battery_sub = nh.subscribe("/mavros/battery", 1, &handleBattery);
-	auto rosout_sub = nh.subscribe("/rosout_agg", 1, &handleLog);
-
-	timer = nh.createTimer(ros::Duration(0), &proceed, false, false);
-
-	ROS_INFO("ready");
-	notify("startup");
-	ros::spin();
-}
--- a/clever/src/optical_flow.cpp
+++ b/clever/src/optical_flow.cpp
@@ -80,7 +80,7 @@ private:
 		flow_.distance = -1; // no distance sensor available
 		flow_.temperature = 0;

-		NODELET_INFO("Optical Flow initialized");
+		ROS_INFO("Optical Flow initialized");
 	}

 	void parseCameraInfo(const sensor_msgs::CameraInfoConstPtr &cinfo) {
--- a/clever/src/selfcheck.py
+++ b/clever/src/selfcheck.py
@@ -1,36 +1,21 @@
 #!/usr/bin/env python
-# coding=utf-8
-
-# Copyright (C) 2018 Copter Express Technologies
-#
-# Author: Oleg Kalachev <okalachev@gmail.com>
-#
-# Distributed under MIT License (available at https://opensource.org/licenses/MIT).
-# The above copyright notice and this permission notice shall be included in all
-# copies or substantial portions of the Software.

 import math
-import subprocess
+from subprocess import Popen, PIPE
 import re
-from collections import OrderedDict
 import traceback
-from threading import Event
-import numpy
 import rospy
-import tf2_ros
-import tf2_geometry_msgs
-from pymavlink import mavutil
 from std_srvs.srv import Trigger
-from sensor_msgs.msg import BatteryState, Image, CameraInfo, NavSatFix, Imu, Range
-from mavros_msgs.msg import State, OpticalFlowRad, Mavlink
+from sensor_msgs.msg import Image, CameraInfo, NavSatFix, Imu, Range
+from mavros_msgs.msg import State, OpticalFlowRad
 from mavros_msgs.srv import ParamGet
-from geometry_msgs.msg import PoseStamped, TwistStamped, PoseWithCovarianceStamped, Vector3Stamped
-from visualization_msgs.msg import MarkerArray as VisualizationMarkerArray
+from geometry_msgs.msg import PoseStamped, TwistStamped, PoseWithCovarianceStamped
 import tf.transformations as t
 from aruco_pose.msg import MarkerArray
-from mavros import mavlink


+# TODO: roscore is running
+# TODO: clever.service is running
 # TODO: check attitude is present
 # TODO: disk free space
 # TODO: map, base_link, body
@@ -43,41 +28,28 @@ from mavros import mavlink
 rospy.init_node('selfcheck')


-tf_buffer = tf2_ros.Buffer()
-tf_listener = tf2_ros.TransformListener(tf_buffer)
-
-
 failures = []
-infos = []
-current_check = None


 def failure(text, *args):
-    msg = text % args
-    rospy.logwarn('%s: %s', current_check, msg)
-    failures.append(msg)
-
-
-def info(text, *args):
-    msg = text % args
-    rospy.loginfo('%s: %s', current_check, msg)
-    infos.append(msg)
+    failures.append(text % args)


 def check(name):
    def inner(fn):
        def wrapper(*args, **kwargs):
            failures[:] = []
-            infos[:] = []
-            global current_check
-            current_check = name
            try:
                fn(*args, **kwargs)
+                for f in failures:
+                    rospy.logwarn('%s: %s', name, f)
            except Exception as e:
+                for f in failures:
+                    rospy.logwarn('%s: %s', name, f)
                traceback.print_exc()
-                rospy.logerr('%s: exception occurred', name)
+                rospy.logwarn('%s: exception occurred', name)
                return
-            if not failures and not infos:
+            if not failures:
                rospy.loginfo('%s: OK', name)
        return wrapper
    return inner
@@ -87,12 +59,7 @@ param_get = rospy.ServiceProxy('mavros/param/get', ParamGet)


 def get_param(name):
-    try:
-        res = param_get(param_id=name)
-    except rospy.ServiceException as e:
-        failure('%s: %s', name, str(e))
-        return None
-
+    res = param_get(param_id=name)
    if not res.success:
        failure('Unable to retrieve PX4 parameter %s', name)
    else:
@@ -101,176 +68,36 @@ def get_param(name):
        return res.value.real


-recv_event = Event()
-link = mavutil.mavlink.MAVLink('', 255, 1)
-mavlink_pub = rospy.Publisher('mavlink/to', Mavlink, queue_size=1)
-mavlink_recv = ''
-
-
-def mavlink_message_handler(msg):
-    global mavlink_recv
-    if msg.msgid == 126:
-        mav_bytes_msg = mavlink.convert_to_bytes(msg)
-        mav_msg = link.decode(mav_bytes_msg)
-        mavlink_recv += ''.join(chr(x) for x in mav_msg.data[:mav_msg.count])
-        if 'nsh>' in mavlink_recv:
-            # Remove the last line, including newline before prompt
-            mavlink_recv = mavlink_recv[:mavlink_recv.find('nsh>') - 1]
-            recv_event.set()
-
-
-mavlink_sub = rospy.Subscriber('mavlink/from', Mavlink, mavlink_message_handler)
-# FIXME: not sleeping here still breaks things
-rospy.sleep(0.5)
-
-
-def mavlink_exec(cmd, timeout=3.0):
-    global mavlink_recv
-    mavlink_recv = ''
-    recv_event.clear()
-    if not cmd.endswith('\n'):
-        cmd += '\n'
-    msg = mavutil.mavlink.MAVLink_serial_control_message(
-        device=mavutil.mavlink.SERIAL_CONTROL_DEV_SHELL,
-        flags=mavutil.mavlink.SERIAL_CONTROL_FLAG_RESPOND | mavutil.mavlink.SERIAL_CONTROL_FLAG_EXCLUSIVE |
-              mavutil.mavlink.SERIAL_CONTROL_FLAG_MULTI,
-        timeout=3,
-        baudrate=0,
-        count=len(cmd),
-        data=map(ord, cmd.ljust(70, '\0')))
-    msg.pack(link)
-    ros_msg = mavlink.convert_to_rosmsg(msg)
-    mavlink_pub.publish(ros_msg)
-    recv_event.wait(timeout)
-    return mavlink_recv
-
-
-BOARD_ROTATIONS = {
-    0: 'no rotation',
-    1: 'yaw 45°',
-    2: 'yaw 90°',
-    3: 'yaw 135°',
-    4: 'yaw 180°',
-    5: 'yaw 225°',
-    6: 'yaw 270°',
-    7: 'yaw 315°',
-    8: 'roll 180°',
-    9: 'roll 180°, yaw 45°',
-    10: 'roll 180°, yaw 90°',
-    11: 'roll 180°, yaw 135°',
-    12: 'pitch 180°',
-    13: 'roll 180°, yaw 225°',
-    14: 'roll 180°, yaw 270°',
-    15: 'roll 180°, yaw 315°',
-    16: 'roll 90°',
-    17: 'roll 90°, yaw 45°',
-    18: 'roll 90°, yaw 90°',
-    19: 'roll 90°, yaw 135°',
-    20: 'roll 270°',
-    21: 'roll 270°, yaw 45°',
-    22: 'roll 270°, yaw 90°',
-    23: 'roll 270°, yaw 135°',
-    24: 'pitch 90°',
-    25: 'pitch 270°',
-    26: 'roll 270°, yaw 270°',
-    27: 'roll 180°, pitch 270°',
-    28: 'pitch 90°, yaw 180',
-    29: 'pitch 90°, roll 90°',
-    30: 'yaw 293°, pitch 68°, roll 90°',
-    31: 'pitch 90°, roll 270°',
-    32: 'pitch 9°, yaw 180°',
-    33: 'pitch 45°',
-    34: 'pitch 315°',
-}
-
-
@check('FCU')
 def check_fcu():
    try:
        state = rospy.wait_for_message('mavros/state', State, timeout=3)
        if not state.connected:
            failure('no connection to the FCU (check wiring)')
-            return
-
-        # Make sure the console is available to us
-        mavlink_exec('\n')
-        version_str = mavlink_exec('ver all')
-        if version_str == '':
-            info('no version data available from SITL')
-
-        r = re.compile(r'^FW (git tag|version): (v?\d\.\d\.\d.*)$')
-        is_clever_firmware = False
-        for ver_line in version_str.split('\n'):
-            match = r.search(ver_line)
-            if match is not None:
-                field, version = match.groups()
-                info('firmware %s: %s' % (field, version))
-                if 'clever' in version:
-                    is_clever_firmware = True
-
-        if not is_clever_firmware:
-            failure('not running Clever PX4 firmware, check http://clever.copterexpress.com/firmware.html')

        est = get_param('SYS_MC_EST_GROUP')
        if est == 1:
-            info('selected estimator: LPE')
+            rospy.loginfo('Selected estimator: LPE')
            fuse = get_param('LPE_FUSION')
            if fuse & (1 << 4):
-                info('LPE_FUSION: land detector fusion is enabled')
+                rospy.loginfo('LPE_FUSION: land detector fusion is enabled')
            else:
-                info('LPE_FUSION: land detector fusion is disabled')
+                rospy.loginfo('LPE_FUSION: land detector fusion is disabled')
            if fuse & (1 << 7):
-                info('LPE_FUSION: barometer fusion is enabled')
+                rospy.loginfo('LPE_FUSION: barometer fusion is enabled')
            else:
-                info('LPE_FUSION: barometer fusion is disabled')
+                rospy.loginfo('LPE_FUSION: barometer fusion is disabled')

        elif est == 2:
-            info('selected estimator: EKF2')
+            rospy.loginfo('Selected estimator: EKF2')
        else:
-            failure('unknown selected estimator: %s', est)
-
-        rot = get_param('SENS_BOARD_ROT')
-        if rot is not None:
-            try:
-                info('board rotation: %s', BOARD_ROTATIONS[rot])
-            except KeyError:
-                failure('unknown board rotation %s', rot)
-
-        cbrk_usb_chk = get_param('CBRK_USB_CHK')
-        if cbrk_usb_chk != 197848:
-            failure('Set parameter CBRK_USB_CHK to 197848 for flying with USB connected')
-
-        try:
-            battery = rospy.wait_for_message('mavros/battery', BatteryState, timeout=3)
-            cell = battery.cell_voltage[0]
-            if cell > 4.3 or cell < 3.0:
-                failure('Incorrect cell voltage: %.2f V, see https://clever.copterexpress.com/power.html', cell)
-            elif cell < 3.7:
-                failure('Critically low cell voltage: %.2f V, recharge battery', cell)
-        except rospy.ROSException:
-            failure('no battery state')
+            failure('Unknown selected estimator: %s', est)

    except rospy.ROSException:
        failure('no MAVROS state (check wiring)')


-def describe_direction(v):
-    if v.x > 0.9:
-        return 'forward'
-    elif v.x < - 0.9:
-        return 'backward'
-    elif v.y > 0.9:
-        return 'left'
-    elif v.y < -0.9:
-        return 'right'
-    elif v.z > 0.9:
-        return 'upward'
-    elif v.z < -0.9:
-        return 'downward'
-    else:
-        return None
-
-
+@check('Camera')
 def check_camera(name):
    try:
        img = rospy.wait_for_message(name + '/image_raw', Image, timeout=1)
@@ -278,98 +105,32 @@ def check_camera(name):
        failure('%s: no images (is the camera connected properly?)', name)
        return
    try:
-        camera_info = rospy.wait_for_message(name + '/camera_info', CameraInfo, timeout=1)
+        info = rospy.wait_for_message(name + '/camera_info', CameraInfo, timeout=1)
    except rospy.ROSException:
        failure('%s: no calibration info', name)
        return

-    if img.width != camera_info.width:
-        failure('%s: calibration width doesn\'t match image width (%d != %d)', name, camera_info.width, img.width)
-    if img.height != camera_info.height:
-        failure('%s: calibration height doesn\'t match image height (%d != %d))', name, camera_info.height, img.height)
-
-    try:
-        optical = Vector3Stamped()
-        optical.header.frame_id = img.header.frame_id
-        optical.vector.z = 1
-        cable = Vector3Stamped()
-        cable.header.frame_id = img.header.frame_id
-        cable.vector.y = 1
-
-        optical = describe_direction(tf_buffer.transform(optical, 'base_link').vector)
-        cable = describe_direction(tf_buffer.transform(cable, 'base_link').vector)
-        if not optical or not cable:
-            info('%s: custom camera orientation detected', name)
-        else:
-            info('camera is oriented %s, camera cable goes %s', optical, cable)
-
-    except tf2_ros.TransformException:
-        failure('cannot transform from base_link to camera frame')
+    if img.width != info.width:
+        failure('%s: calibration width doesn\'t match image width (%d != %d)', name, info.width, img.width)
+    if img.height != info.height:
+        failure('%s: calibration height doesn\'t match image height (%d != %d))', name, info.height, img.height)


-@check('Main camera')
-def check_main_camera():
-    check_camera('main_camera')
-
-
-def is_process_running(binary, exact=False, full=False):
-    try:
-        args = ['pgrep']
-        if exact:
-            args.append('-x')  # match exactly with the command name
-        if full:
-            args.append('-f')  # use full process name to match
-        args.append(binary)
-        subprocess.check_output(args)
-        return True
-    except subprocess.CalledProcessError:
-        return False
-
-
-@check('ArUco markers')
+@check('ArUco detector')
 def check_aruco():
-    if is_process_running('aruco_detect', full=True):
-        info('aruco_detect/length = %g m', rospy.get_param('aruco_detect/length'))
-        known_tilt = rospy.get_param('aruco_detect/known_tilt')
-        if known_tilt == 'map':
-            known_tilt += ' (ALL markers are on the floor)'
-        elif known_tilt == 'map_flipped':
-            known_tilt += ' (ALL markers are on the ceiling)'
-        info('aruco_detector/known_tilt = %s', known_tilt)
-        try:
-            rospy.wait_for_message('aruco_detect/markers', MarkerArray, timeout=1)
-        except rospy.ROSException:
-            failure('no markers detection')
-            return
-    else:
-        info('aruco_detect is not running')
+    try:
+        rospy.wait_for_message('aruco_detect/markers', MarkerArray, timeout=1)
+    except rospy.ROSException:
+        failure('no markers detection')
        return
-
-    if is_process_running('aruco_map', full=True):
-        known_tilt = rospy.get_param('aruco_map/known_tilt')
-        if known_tilt == 'map':
-            known_tilt += ' (marker\'s map is on the floor)'
-        elif known_tilt == 'map_flipped':
-            known_tilt += ' (marker\'s map is on the ceiling)'
-        info('aruco_map/known_tilt = %s', known_tilt)
-
-        try:
-            visualization = rospy.wait_for_message('aruco_map/visualization', VisualizationMarkerArray, timeout=1)
-            info('map has %s markers', len(visualization.markers))
-        except:
-            failure('cannot read aruco_map/visualization topic')
-
-        try:
-            rospy.wait_for_message('aruco_map/pose', PoseWithCovarianceStamped, timeout=1)
-        except rospy.ROSException:
-            failure('no map detection')
-    else:
-        info('aruco_map is not running')
+    try:
+        rospy.wait_for_message('aruco_map/pose', PoseWithCovarianceStamped, timeout=1)
+    except rospy.ROSException:
+        failure('no map detection')


@check('Vision position estimate')
 def check_vpe():
-    vis = None
    try:
        vis = rospy.wait_for_message('mavros/vision_pose/pose', PoseStamped, timeout=1)
    except rospy.ROSException:
@@ -377,11 +138,7 @@ def check_vpe():
            vis = rospy.wait_for_message('mavros/mocap/pose', PoseStamped, timeout=1)
        except rospy.ROSException:
            failure('no VPE or MoCap messages')
-            # check if vpe_publisher is running
-            try:
-                subprocess.check_output(['pgrep', '-x', 'vpe_publisher'])
-            except subprocess.CalledProcessError:
-                return  # it's not running, skip following checks
+            return

    # check PX4 settings
    est = get_param('SYS_MC_EST_GROUP')
@@ -393,30 +150,27 @@ def check_vpe():
        if vision_yaw_w == 0:
            failure('vision yaw weight is zero, change ATT_W_EXT_HDG parameter')
        else:
-            info('Vision yaw weight: %.2f', vision_yaw_w)
+            rospy.loginfo('Vision yaw weight: %.2f', vision_yaw_w)
        fuse = get_param('LPE_FUSION')
        if not fuse & (1 << 2):
-            failure('vision position fusion is disabled, change LPE_FUSION parameter')
+            failure('vision position fusing is disabled, change LPE_FUSION parameter')
        delay = get_param('LPE_VIS_DELAY')
        if delay != 0:
            failure('LPE_VIS_DELAY parameter is %s, but it should be zero', delay)
-        info('LPE_VIS_XY is %.2f m, LPE_VIS_Z is %.2f m', get_param('LPE_VIS_XY'), get_param('LPE_VIS_Z'))
+        rospy.loginfo('LPE_VIS_XY is %.2f m, LPE_VIS_Z is %.2f m', get_param('LPE_VIS_XY'), get_param('LPE_VIS_Z'))
    elif est == 2:
        fuse = get_param('EKF2_AID_MASK')
        if not fuse & (1 << 3):
-            failure('vision position fusion is disabled, change EKF2_AID_MASK parameter')
+            failure('vision position fusing is disabled, change EKF2_AID_MASK parameter')
        if not fuse & (1 << 4):
-            failure('vision yaw fusion is disabled, change EKF2_AID_MASK parameter')
+            failure('vision yaw fusing is disabled, change EKF2_AID_MASK parameter')
        delay = get_param('EKF2_EV_DELAY')
        if delay != 0:
            failure('EKF2_EV_DELAY is %.2f, but it should be zero', delay)
-        info('EKF2_EVA_NOISE is %.3f, EKF2_EVP_NOISE is %.3f',
+        rospy.loginfo('EKF2_EVA_NOISE is %.3f, EKF2_EVP_NOISE is %.3f',
            get_param('EKF2_EVA_NOISE'),
            get_param('EKF2_EVP_NOISE'))

-    if not vis:
-        return
-
    # check vision pose and estimated pose inconsistency
    try:
        pose = rospy.wait_for_message('mavros/local_position/pose', PoseStamped, timeout=1)
@@ -486,7 +240,7 @@ def check_velocity():
            failure('vertical velocity estimation is %.2f m/s; is copter staying still?' % vert)

        angular = velocity.twist.angular
-        ANGULAR_VELOCITY_LIMIT = 0.1
+        ANGULAR_VELOCITY_LIMIT = 0.01
        if abs(angular.x) > ANGULAR_VELOCITY_LIMIT:
            failure('pitch rate estimation is %.2f rad/s (%.2f deg/s); is copter staying still?',
                    angular.x, math.degrees(angular.x))
@@ -522,32 +276,32 @@ def check_optical_flow():
        if est == 1:
            fuse = get_param('LPE_FUSION')
            if not fuse & (1 << 1):
-                failure('optical flow fusion is disabled, change LPE_FUSION parameter')
+                failure('optical flow fusing is disabled, change LPE_FUSION parameter')
            if not fuse & (1 << 1):
                failure('flow gyro compensation is disabled, change LPE_FUSION parameter')
            scale = get_param('LPE_FLW_SCALE')
-            if not numpy.isclose(scale, 1.0):
+            if scale != 0:
                failure('LPE_FLW_SCALE parameter is %.2f, but it should be 1.0', scale)

-            info('LPE_FLW_QMIN is %s, LPE_FLW_R is %.4f, LPE_FLW_RR is %.4f, SENS_FLOW_MINHGT is %.3f, SENS_FLOW_MAXHGT is %.3f',
-                          get_param('LPE_FLW_QMIN'),
-                          get_param('LPE_FLW_R'),
-                          get_param('LPE_FLW_RR'),
-                          get_param('SENS_FLOW_MINHGT'),
-                          get_param('SENS_FLOW_MAXHGT'))
+            rospy.loginfo('LPE_FLW_QMIN is %s, LPE_FLW_R is %.4f, LPE_FLW_RR is %.4f, SENS_FLOW_MINHGT is %.3f, SENS_FLOW_MAXHGT is %.3f',
+                get_param('LPE_FLW_QMIN'),
+                get_param('LPE_FLW_R'),
+                get_param('LPE_FLW_RR'),
+                get_param('SENS_FLOW_MINHGT'),
+                get_param('SENS_FLOW_MAXHGT'))
        elif est == 2:
            fuse = get_param('EKF2_AID_MASK')
            if not fuse & (1 << 1):
-                failure('optical flow fusion is disabled, change EKF2_AID_MASK parameter')
+                failure('optical flow fusing is disabled, change EKF2_AID_MASK parameter')
            delay = get_param('EKF2_OF_DELAY')
            if delay != 0:
                failure('EKF2_OF_DELAY is %.2f, but it should be zero', delay)
-            info('EKF2_OF_QMIN is %s, EKF2_OF_N_MIN is %.4f, EKF2_OF_N_MAX is %.4f, SENS_FLOW_MINHGT is %.3f, SENS_FLOW_MAXHGT is %.3f',
-                          get_param('EKF2_OF_QMIN'),
-                          get_param('EKF2_OF_N_MIN'),
-                          get_param('EKF2_OF_N_MAX'),
-                          get_param('SENS_FLOW_MINHGT'),
-                          get_param('SENS_FLOW_MAXHGT'))
+            rospy.loginfo('EKF2_OF_QMIN is %s, EKF2_OF_N_MIN is %.4f, EKF2_OF_N_MAX is %.4f, SENS_FLOW_MINHGT is %.3f, SENS_FLOW_MAXHGT is %.3f',
+                get_param('EKF2_OF_QMIN'),
+                get_param('EKF2_OF_N_MIN'),
+                get_param('EKF2_OF_N_MAX'),
+                get_param('SENS_FLOW_MINHGT'),
+                get_param('SENS_FLOW_MAXHGT'))

    except rospy.ROSException:
        failure('no optical flow data (from Raspberry)')
@@ -558,13 +312,13 @@ def check_rangefinder():
    # TODO: check FPS!
    rng = False
    try:
-        rospy.wait_for_message('rangefinder/range', Range, timeout=4)
+        rospy.wait_for_message('mavros/distance_sensor/rangefinder_sub', Range, timeout=0.5)
        rng = True
    except rospy.ROSException:
        failure('no rangefinder data from Raspberry')

    try:
-        rospy.wait_for_message('mavros/distance_sensor/rangefinder', Range, timeout=4)
+        rospy.wait_for_message('mavros/distance_sensor/rangefinder', Range, timeout=0.5)
        rng = True
    except rospy.ROSException:
        failure('no rangefinder data from PX4')
@@ -576,26 +330,28 @@ def check_rangefinder():
    if est == 1:
        fuse = get_param('LPE_FUSION')
        if not fuse & (1 << 5):
-            info('"pub agl as lpos down" in LPE_FUSION is disabled, NOT operating over flat surface')
+            rospy.loginfo('"pub agl as lpos down" in LPE_FUSION is disabled, NOT operating over flat surface')
        else:
-            info('"pub agl as lpos down" in LPE_FUSION is enabled, operating over flat surface')
+            rospy.loginfo('"pub agl as lpos down" in LPE_FUSION is enabled, operating over flat surface')

    elif est == 2:
        hgt = get_param('EKF2_HGT_MODE')
        if hgt != 2:
-            info('EKF2_HGT_MODE != Range sensor, NOT operating over flat surface')
+            rospy.loginfo('EKF2_HGT_MODE != Range sensor, NOT operating over flat surface')
        else:
-            info('EKF2_HGT_MODE = Range sensor, operating over flat surface')
+            rospy.loginfo('EKF2_HGT_MODE = Range sensor, operating over flat surface')
        aid = get_param('EKF2_RNG_AID')
        if aid != 1:
-            info('EKF2_RNG_AID != 1, range sensor aiding disabled')
+            rospy.loginfo('EKF2_RNG_AID != 1, range sensor aiding disabled')
        else:
-            info('EKF2_RNG_AID = 1, range sensor aiding enabled')
+            rospy.loginfo('EKF2_RNG_AID = 1, range sensor aiding enabled')


@check('Boot duration')
 def check_boot_duration():
-    output = subprocess.check_output('systemd-analyze')
+    proc = Popen('systemd-analyze', stdout=PIPE)
+    proc.wait()
+    output = proc.communicate()[0]
    r = re.compile(r'([\d\.]+)s$')
    duration = float(r.search(output).groups()[0])
    if duration > 15:
@@ -606,7 +362,9 @@ def check_boot_duration():
 def check_cpu_usage():
    WHITELIST = 'nodelet',
    CMD = "top -n 1 -b -i | tail -n +8 | awk '{ printf(\"%-8s\\t%-8s\\t%-8s\\n\", $1, $9, $12); }'"
-    output = subprocess.check_output(CMD, shell=True)
+    proc = Popen(CMD, stdout=PIPE, shell=True)
+    proc.wait()
+    output = proc.communicate()[0]
    processes = output.split('\n')
    for process in processes:
        if not process:
@@ -618,83 +376,13 @@ def check_cpu_usage():
                    cpu.strip(), cmd.strip(), pid.strip())


-@check('clever.service')
-def check_clever_service():
-    try:
-        output = subprocess.check_output('systemctl show -p ActiveState --value clever.service'.split(),
-                                         stderr=subprocess.STDOUT)
-    except subprocess.CalledProcessError as e:
-        failure('systemctl returned %s: %s', e.returncode, e.output)
-        return
-    if 'inactive' in output:
-        failure('service is not running, try sudo systemctl restart clever')
-        return
-    elif 'failed' in output:
-        failure('service failed to run, check your launch-files')
-
-    r = re.compile(r'^(.*)\[(FATAL|ERROR)\] \[\d+.\d+\]: (.*?)(\x1b(.*))?$')
-    error_count = OrderedDict()
-    try:
-        for line in open('/tmp/clever.err', 'r'):
-            node_error = r.search(line)
-            if node_error:
-                msg = node_error.groups()[1] + ': ' + node_error.groups()[2]
-                if msg in error_count:
-                    error_count[msg] += 1
-                else:
-                    error_count.update({msg: 1})
-            else:
-                error_count.update({line.strip(): 1})
-
-        for error in error_count:
-            if error_count[error] == 1:
-                failure(error)
-            else:
-                failure('%s (%d)', error, error_count[error])
-
-    except IOError as e:
-        failure('%s', e)
-
-
-@check('Image')
-def check_image():
-    try:
-        info('version: %s', open('/etc/clever_version').read().strip())
-    except IOError:
-        info('no /etc/clever_version file, not the Clever image?')
-
-
-@check('Preflight status')
-def check_preflight_status():
-    # Make sure the console is available to us
-    mavlink_exec('\n')
-    cmdr_output = mavlink_exec('commander check')
-    if cmdr_output == '':
-        failure('no data from FCU')
-        return
-    cmdr_lines = cmdr_output.split('\n')
-    r = re.compile(r'^(.*)(Preflight|Prearm) check: (.*)')
-    for line in cmdr_lines:
-        if 'WARN' in line:
-            failure(line[line.find(']') + 2:])
-            continue
-        match = r.search(line)
-        if match is not None:
-            check_status = match.groups()[2]
-            if check_status != 'OK':
-                failure(' '.join([match.groups()[1], 'check:', check_status]))
-
-
 def selfcheck():
-    check_image()
-    check_clever_service()
    check_fcu()
    check_imu()
    check_local_position()
    check_velocity()
    check_global_position()
-    check_preflight_status()
-    check_main_camera()
+    check_camera('main_camera')
    check_aruco()
    check_simpleoffboard()
    check_optical_flow()
--- a/clever/src/simple_offboard.cpp
+++ b/clever/src/simple_offboard.cpp
@@ -20,7 +20,6 @@
 #include <tf2/utils.h>
 #include <tf2_ros/transform_listener.h>
 #include <tf2_ros/transform_broadcaster.h>
-#include <tf2_ros/static_transform_broadcaster.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 #include <std_srvs/Trigger.h>
 #include <geometry_msgs/PoseStamped.h>
@@ -55,8 +54,6 @@ using mavros_msgs::Thrust;

 // tf2
 tf2_ros::Buffer tf_buffer;
-std::shared_ptr<tf2_ros::TransformBroadcaster> transform_broadcaster;
-std::shared_ptr<tf2_ros::StaticTransformBroadcaster> static_transform_broadcaster;

 // Parameters
 string local_frame;
@@ -73,7 +70,6 @@ ros::Duration global_position_timeout;
 ros::Duration battery_timeout;
 float default_speed;
 bool auto_release;
-bool land_only_in_offboard, nav_from_sp;
 std::map<string, string> reference_frames;

 // Publishers
@@ -91,7 +87,6 @@ AttitudeTarget att_raw_msg;
 Thrust thrust_msg;
 TwistStamped rates_msg;
 TransformStamped target;
-geometry_msgs::TransformStamped body;

 // State
 PoseStamped nav_start;
@@ -102,7 +97,6 @@ float setpoint_yaw_rate;
 float nav_speed;
 bool busy = false;
 bool wait_armed = false;
-bool nav_from_sp_flag = false;

 enum setpoint_type_t {
 	NONE,
@@ -126,50 +120,18 @@ TwistStamped velocity;
 NavSatFix global_position;
 BatteryState battery;

-// Common subscriber callback template that stores message to the variable
+// Common subcriber callback template that stores message to the variable
 template<typename T, T& STORAGE>
 void handleMessage(const T& msg)
 {
 	STORAGE = msg;
 }

-void handleState(const mavros_msgs::State& s)
-{
-	state = s;
-	if (s.mode != "OFFBOARD") {
-		// flight intercepted
-		nav_from_sp_flag = false;
-	}
-}
-
-inline void publishBodyFrame()
-{
-	if (body.child_frame_id.empty()) return;
-
-	tf::Quaternion q;
-	q.setRPY(0, 0, tf::getYaw(local_position.pose.orientation));
-	tf::quaternionTFToMsg(q, body.transform.rotation);
-
-	body.transform.translation.x = local_position.pose.position.x;
-	body.transform.translation.y = local_position.pose.position.y;
-	body.transform.translation.z = local_position.pose.position.z;
-	body.header.frame_id = local_position.header.frame_id;
-	body.header.stamp = local_position.header.stamp;
-	transform_broadcaster->sendTransform(body);
-}
-
-void handleLocalPosition(const PoseStamped& pose)
-{
-	local_position = pose;
-	publishBodyFrame();
-	// TODO: terrain?, home?
-}
-
 // wait for transform without interrupting publishing setpoints
 inline bool waitTransform(const string& target, const string& source,
-                          const ros::Time& stamp, const ros::Duration& timeout) // editorconfig-checker-disable-line
+                          const ros::Time& stamp, const ros::Duration& timeout)
 {
-	ros::Rate r(100);
+	ros::Rate r(10);
 	auto start = ros::Time::now();
 	while (ros::ok()) {
 		if (ros::Time::now() - start > timeout) return false;
@@ -213,29 +175,31 @@ bool getTelemetry(GetTelemetry::Request& req, GetTelemetry::Response& res)
 		res.mode = state.mode;
 	}

-	try {
-		waitTransform(req.frame_id, fcu_frame, stamp, telemetry_transform_timeout);
-		auto transform = tf_buffer.lookupTransform(req.frame_id, fcu_frame, stamp);
-		res.x = transform.transform.translation.x;
-		res.y = transform.transform.translation.y;
-		res.z = transform.transform.translation.z;
+	waitTransform(local_frame, req.frame_id, stamp, telemetry_transform_timeout);

-		double yaw, pitch, roll;
-		tf2::getEulerYPR(transform.transform.rotation, yaw, pitch, roll);
-		res.yaw = yaw;
-		res.pitch = pitch;
-		res.roll = roll;
-	} catch (const tf2::TransformException& e) {
-		ROS_DEBUG("%s", e.what());
+	if (!TIMEOUT(local_position, local_position_timeout)) {
+		try {
+			// transform pose
+			PoseStamped pose;
+			tf_buffer.transform(local_position, pose, req.frame_id);
+			res.x = pose.pose.position.x;
+			res.y = pose.pose.position.y;
+			res.z = pose.pose.position.z;
+
+			// Tait-Bryan angles, order z-y-x
+			double yaw, pitch, roll;
+			tf2::getEulerYPR(pose.pose.orientation, yaw, pitch, roll);
+			res.yaw = yaw;
+			res.pitch = pitch;
+			res.roll = roll;
+		} catch (const tf2::TransformException& e) {}
 	}

 	if (!TIMEOUT(velocity, velocity_timeout)) {
 		try {
 			// transform velocity
-			waitTransform(req.frame_id, fcu_frame, velocity.header.stamp, telemetry_transform_timeout);
 			Vector3Stamped vec, vec_out;
-			vec.header.stamp = velocity.header.stamp;
-			vec.header.frame_id = velocity.header.frame_id;
+			vec.header = velocity.header;
 			vec.vector = velocity.twist.linear;
 			tf_buffer.transform(vec, vec_out, req.frame_id);

@@ -273,7 +237,7 @@ void offboardAndArm()

 	if (state.mode != "OFFBOARD") {
 		auto start = ros::Time::now();
-		ROS_INFO("switch to OFFBOARD");
+		ROS_INFO("simple_offboard: switch to OFFBOARD");
 		static mavros_msgs::SetMode sm;
 		sm.request.custom_mode = "OFFBOARD";

@@ -298,7 +262,7 @@ void offboardAndArm()

 	if (!state.armed) {
 		ros::Time start = ros::Time::now();
-		ROS_INFO("arming");
+		ROS_INFO("simple_offboard: arming");
 		mavros_msgs::CommandBool srv;
 		srv.request.value = true;
 		if (!arming.call(srv)) {
@@ -361,10 +325,6 @@ PoseStamped globalToLocal(double lat, double lon)
 	x_offset = distance * sin(azimuth_radians);
 	y_offset = distance * cos(azimuth_radians);

-	if (!waitTransform(local_frame, fcu_frame, global_position.header.stamp, ros::Duration(0.2))) {
-		throw std::runtime_error("No local position");
-	}
-
 	auto local = tf_buffer.lookupTransform(local_frame, fcu_frame, global_position.header.stamp);

 	PoseStamped pose;
@@ -398,7 +358,19 @@ void publish(const ros::Time stamp)
 		}

 	} catch (const tf2::TransformException& e) {
-		ROS_WARN_THROTTLE(10, "can't transform");
+		ROS_WARN_THROTTLE(10, "simple_offboard: can't transform");
+	}
+
+	if (!target.child_frame_id.empty()) {
+		if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == POSITION) {
+			static tf2_ros::TransformBroadcaster tf_broadcaster;
+			target.header = setpoint_position_transformed.header;
+			target.transform.translation.x = setpoint_position_transformed.pose.position.x;
+			target.transform.translation.y = setpoint_position_transformed.pose.position.y;
+			target.transform.translation.z = setpoint_position_transformed.pose.position.z;
+			target.transform.rotation = setpoint_position_transformed.pose.orientation;
+			tf_broadcaster.sendTransform(target);
+		}
 	}

 	if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL) {
@@ -482,12 +454,10 @@ inline void checkState()
 		throw std::runtime_error("No connection to FCU, https://clever.copterexpress.com/connection.html");
 }

-#define ENSURE_FINITE(var) { if (!std::isfinite(var)) throw std::runtime_error(#var " argument cannot be NaN or Inf"); }
-
 bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, float vy, float vz,
-           float pitch, float roll, float yaw, float pitch_rate, float roll_rate, float yaw_rate, // editorconfig-checker-disable-line
-           float lat, float lon, float thrust, float speed, string frame_id, bool auto_arm, // editorconfig-checker-disable-line
-           uint8_t& success, string& message) // editorconfig-checker-disable-line
+           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();

@@ -495,20 +465,6 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl
 		if (busy)
 			throw std::runtime_error("Busy");

-		ENSURE_FINITE(x);
-		ENSURE_FINITE(y);
-		ENSURE_FINITE(z);
-		ENSURE_FINITE(vx);
-		ENSURE_FINITE(vy);
-		ENSURE_FINITE(vz);
-		ENSURE_FINITE(pitch);
-		ENSURE_FINITE(roll);
-		ENSURE_FINITE(pitch_rate);
-		ENSURE_FINITE(roll_rate);
-		ENSURE_FINITE(lat);
-		ENSURE_FINITE(lon);
-		ENSURE_FINITE(thrust);
-
 		busy = true;

 		// Checks
@@ -558,9 +514,7 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl

 		if (sp_type == NAVIGATE_GLOBAL) {
 			// Calculate x and from lat and lot in request's frame
-			auto pose_local = globalToLocal(lat, lon);
-			pose_local.header.stamp = stamp; // TODO: fix
-			auto xy_in_req_frame = tf_buffer.transform(pose_local, frame_id);
+			auto xy_in_req_frame = tf_buffer.transform(globalToLocal(lat, lon), frame_id);
 			x = xy_in_req_frame.pose.position.x;
 			y = xy_in_req_frame.pose.position.y;
 		}
@@ -568,20 +522,10 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl
 		// Everything fine - switch setpoint type
 		setpoint_type = sp_type;

-		if (sp_type != NAVIGATE && sp_type != NAVIGATE_GLOBAL) {
-			nav_from_sp_flag = false;
-		}
-
 		if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL) {
 			// starting point
-			if (nav_from_sp && nav_from_sp_flag) {
-				message = "Navigating from current setpoint";
-				nav_start = position_msg;
-			} else {
-				nav_start = local_position;
-			}
+			nav_start = local_position;
 			nav_speed = speed;
-			nav_from_sp_flag = true;
 		}

 		// if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == POSITION || sp_type == VELOCITY) {
@@ -643,19 +587,6 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl
 		publish(stamp); // calculate initial transformed messages first
 		setpoint_timer.start();

-		// publish target frame
-		if (!target.child_frame_id.empty()) {
-			if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == POSITION) {
-				target.header.frame_id = setpoint_position.header.frame_id;
-				target.header.stamp = stamp;
-				target.transform.translation.x = setpoint_position.pose.position.x;
-				target.transform.translation.y = setpoint_position.pose.position.y;
-				target.transform.translation.z = setpoint_position.pose.position.z;
-				target.transform.rotation = setpoint_position.pose.orientation;
-				static_transform_broadcaster->sendTransform(target);
-			}
-		}
-
 		if (auto_arm) {
 			offboardAndArm();
 			wait_armed = false;
@@ -669,7 +600,7 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl

 	} catch (const std::exception& e) {
 		message = e.what();
-		ROS_INFO("%s", message.c_str());
+		ROS_INFO("simple_offboard: %s", message.c_str());
 		busy = false;
 		return true;
 	}
@@ -713,12 +644,6 @@ bool land(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)

 		checkState();

-		if (land_only_in_offboard) {
-			if (state.mode != "OFFBOARD") {
-				throw std::runtime_error("Copter is not in OFFBOARD mode");
-			}
-		}
-
 		static mavros_msgs::SetMode sm;
 		sm.request.custom_mode = "AUTO.LAND";

@@ -745,7 +670,7 @@ bool land(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)

 	} catch (const std::exception& e) {
 		res.message = e.what();
-		ROS_INFO("%s", e.what());
+		ROS_INFO("simple_offboard: %s", e.what());
 		busy = false;
 		return true;
 	}
@@ -757,18 +682,13 @@ int main(int argc, char **argv)
 	ros::NodeHandle nh, nh_priv("~");

 	tf2_ros::TransformListener tf_listener(tf_buffer);
-	transform_broadcaster = std::make_shared<tf2_ros::TransformBroadcaster>();
-	static_transform_broadcaster = std::make_shared<tf2_ros::StaticTransformBroadcaster>();

 	// Params
 	nh.param<string>("mavros/local_position/tf/frame_id", local_frame, "map");
 	nh.param<string>("mavros/local_position/tf/child_frame_id", fcu_frame, "base_link");
 	nh_priv.param("target_frame", target.child_frame_id, string("navigate_target"));
 	nh_priv.param("auto_release", auto_release, true);
-	nh_priv.param("land_only_in_offboard", land_only_in_offboard, true);
-	nh_priv.param("nav_from_sp", nav_from_sp, true);
 	nh_priv.param("default_speed", default_speed, 0.5f);
-	nh_priv.param<string>("body_frame", body.child_frame_id, "body");
 	nh_priv.getParam("reference_frames", reference_frames);

 	state_timeout = ros::Duration(nh_priv.param("state_timeout", 3.0));
@@ -788,12 +708,12 @@ int main(int argc, char **argv)
 	set_mode = nh.serviceClient<mavros_msgs::SetMode>("mavros/set_mode");

 	// Telemetry subscribers
-	auto state_sub = nh.subscribe("mavros/state", 1, &handleState);
-	auto velocity_sub = nh.subscribe("mavros/local_position/velocity_body", 1, &handleMessage<TwistStamped, velocity>);
+	auto state_sub = nh.subscribe("mavros/state", 1, &handleMessage<mavros_msgs::State, state>);
+	auto local_position_sub = nh.subscribe("mavros/local_position/pose", 1, &handleMessage<PoseStamped, local_position>);
+	auto velocity_sub = nh.subscribe("mavros/local_position/velocity", 1, &handleMessage<TwistStamped, velocity>);
 	auto global_position_sub = nh.subscribe("mavros/global_position/global", 1, &handleMessage<NavSatFix, global_position>);
 	auto battery_sub = nh.subscribe("mavros/battery", 1, &handleMessage<BatteryState, battery>);
 	auto statustext_sub = nh.subscribe("mavros/statustext/recv", 1, &handleMessage<mavros_msgs::StatusText, statustext>);
-	auto local_position_sub = nh.subscribe("mavros/local_position/pose", 1, &handleLocalPosition);

 	// Setpoint publishers
 	position_pub = nh.advertise<PoseStamped>("mavros/setpoint_position/local", 1);
@@ -821,6 +741,6 @@ int main(int argc, char **argv)
 	position_raw_msg.coordinate_frame = PositionTarget::FRAME_LOCAL_NED;
 	rates_msg.header.frame_id = fcu_frame;

-	ROS_INFO("ready");
+	ROS_INFO("simple_offboard: ready");
 	ros::spin();
 }
--- a/clever/src/vpe_publisher.cpp
+++ b/clever/src/vpe_publisher.cpp
@@ -40,7 +40,7 @@ void publishZero(const ros::TimerEvent& e)

 	if (e.current_real - pose.header.stamp < publish_zero_timout) { // have local position
 		if (got_local_pos.isZero()) {
-			ROS_INFO("got local position");
+			ROS_INFO("vpe_publisher: got local position");
 			got_local_pos = e.current_real;
 		}

@@ -50,7 +50,7 @@ void publishZero(const ros::TimerEvent& e)
 		got_local_pos = ros::Time(0);
 	}

-	ROS_INFO_THROTTLE(10, "publish zero");
+	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;
@@ -91,7 +91,7 @@ void callback(const T& msg)
 				// offset.header.frame_id = vpe.header.frame_id;
 				offset.child_frame_id = offset_frame_id;
 				br.sendTransform(offset);
-				ROS_INFO("offset reset");
+				ROS_INFO("vpe_publisher: offset reset");
 			}
 			// apply the offset
 			tf2::doTransform(vpe, vpe, offset);
@@ -102,7 +102,7 @@ void callback(const T& msg)
 		vpe_pub.publish(vpe);

 	} catch (const tf2::TransformException& e) {
-		ROS_WARN_THROTTLE(5, "%s", e.what());
+		ROS_WARN_THROTTLE(5, "vpe_publisher: %s", e.what());
 	}
 }

@@ -119,9 +119,9 @@ int main(int argc, char **argv) {
 	offset_timeout = ros::Duration(nh_priv.param("offset_timeout", 3.0));

 	if (!frame_id.empty()) {
-		ROS_INFO("using data from TF");
+		ROS_INFO("vpe_publisher: using data from TF");
 	} else {
-		ROS_INFO("using data topic");
+		ROS_INFO("vpe_publisher: using data topic");
 	}

 	auto pose_sub = nh_priv.subscribe<PoseStamped>("pose", 1, &callback);
@@ -139,6 +139,6 @@ int main(int argc, char **argv) {
 		local_position_sub = nh.subscribe("mavros/local_position/pose", 1, &localPositionCallback);
 	}

-	ROS_INFO("ready");
+	ROS_INFO("vpe_publisher: ready");
 	ros::spin();
 }
--- a/clever/srv/SetLEDEffect.srv
+++ b/clever/srv/SetLEDEffect.srv
@@ -1,7 +0,0 @@
-string effect
-uint8 r
-uint8 g
-uint8 b
---
-bool success
-string message
--- a/clever/test/basic.py
+++ b/clever/test/basic.py
@@ -1,29 +0,0 @@
-#!/usr/bin/env python
-import rospy
-import pytest
-from mavros_msgs.msg import State
-
-@pytest.fixture()
-def node():
-    return rospy.init_node('clever_test', anonymous=True)
-
-def test_state(node):
-    state = rospy.wait_for_message('mavros/state', State, timeout=10)
-    assert state.connected == False
-    assert state.armed == False
-    assert state.guided == False
-    assert state.mode == ''
-
-def test_simple_offboard_services_available():
-    rospy.wait_for_service('get_telemetry', timeout=5)
-    rospy.wait_for_service('navigate', timeout=5)
-    rospy.wait_for_service('navigate_global', timeout=5)
-    rospy.wait_for_service('set_position', timeout=5)
-    rospy.wait_for_service('set_velocity', timeout=5)
-    rospy.wait_for_service('set_attitude', timeout=5)
-    rospy.wait_for_service('set_rates', timeout=5)
-    rospy.wait_for_service('land', timeout=5)
-
-def test_web_video_server(node):
-    import urllib2
-    urllib2.urlopen("http://localhost:8080").read()
--- a/clever/test/basic.test
+++ b/clever/test/basic.test
@@ -1,41 +0,0 @@
-<launch>
-    <!-- Verify all the required nodes basically work -->
-
-    <node pkg="mavros" type="mavros_node" name="mavros" required="true" output="screen">
-        <param name="fcu_url" value="udp://@127.0.1:14557"/>
-        <rosparam command="load" file="$(find clever)/launch/mavros_config.yaml"/>
-    </node>
-
-    <node name="visualization" pkg="mavros_extras" type="visualization" required="true">
-        <remap to="mavros/local_position/pose" from="local_position"/>
-        <remap to="mavros/setpoint_position/local" from="local_setpoint"/>
-        <param name="fixed_frame_id" value="map"/>
-        <param name="child_frame_id" value="base_link"/>
-        <param name="marker_scale" value="1"/>
-        <param name="max_track_size" value="20"/>
-        <param name="num_rotors" value="4"/>
-    </node>
-
-    <node name="web_video_server" pkg="web_video_server" type="web_video_server" required="true" output="screen">
-        <param name="default_stream_type" value="ros_compressed"/>
-        <param name="publish_rate" value="1.0"/>
-    </node>
-
-    <node pkg="tf2_ros" type="static_transform_publisher" name="map_flipped_frame" args="0 0 0 3.1415926 3.1415926 0 map map_flipped" required="true"/>
-
-    <node name="simple_offboard" pkg="clever" type="simple_offboard" required="true" output="screen">
-        <param name="reference_frames/body" value="map"/>
-        <param name="reference_frames/base_link" value="map"/>
-    </node>
-
-    <node name="tf2_web_republisher" pkg="tf2_web_republisher" type="tf2_web_republisher" required="true"/>
-
-    <node name="rc" pkg="clever" type="rc" required="true" output="screen"/>
-
-    <node pkg="clever" name="led_effect" type="led" ns="led" clear_params="true" output="screen" required="true">
-        <rosparam param="notify">startup: { r: 255, g: 255, b: 255 }</rosparam>
-    </node>
-
-    <param name="test_module" value="$(find clever)/test/basic.py"/>
-    <test test-name="basic_test" pkg="ros_pytest" type="ros_pytest_runner"/>
-</launch>
--- a/clever/www/js/gcs.js
+++ b/clever/www/js/gcs.js
@@ -3,7 +3,7 @@ var titleEl = document.querySelector('title');
 var modeEl = document.querySelector('.mode');
 var batteryEl = document.querySelector('.battery');

-var url = 'ws://' + location.hostname + ':9090';
+var url = 'ws://' + location.host + ':9090';
 var ros = new ROSLIB.Ros({ url: url });

 function speak(txt) {
--- a/clever/www/js/viz.js
+++ b/clever/www/js/viz.js
@@ -1,5 +1,5 @@
 var ros = new ROSLIB.Ros({
-	url : 'ws://' + location.hostname + ':9090'
+	url : 'ws://' + location.host + ':9090'
 });

 var titleEl = document.querySelector('title');
--- a/docs/LICENSE
+++ b/docs/LICENSE
@@ -1,437 +0,0 @@
-Attribution-NonCommercial-ShareAlike 4.0 International
-
-=======================================================================
-
-Creative Commons Corporation ("Creative Commons") is not a law firm and
-does not provide legal services or legal advice. Distribution of
-Creative Commons public licenses does not create a lawyer-client or
-other relationship. Creative Commons makes its licenses and related
-information available on an "as-is" basis. Creative Commons gives no
-warranties regarding its licenses, any material licensed under their
-terms and conditions, or any related information. Creative Commons
-disclaims all liability for damages resulting from their use to the
-fullest extent possible.
-
-Using Creative Commons Public Licenses
-
-Creative Commons public licenses provide a standard set of terms and
-conditions that creators and other rights holders may use to share
-original works of authorship and other material subject to copyright
-and certain other rights specified in the public license below. The
-following considerations are for informational purposes only, are not
-exhaustive, and do not form part of our licenses.
-
-     Considerations for licensors: Our public licenses are
-     intended for use by those authorized to give the public
-     permission to use material in ways otherwise restricted by
-     copyright and certain other rights. Our licenses are
-     irrevocable. Licensors should read and understand the terms
-     and conditions of the license they choose before applying it.
-     Licensors should also secure all rights necessary before
-     applying our licenses so that the public can reuse the
-     material as expected. Licensors should clearly mark any
-     material not subject to the license. This includes other CC-
-     licensed material, or material used under an exception or
-     limitation to copyright. More considerations for licensors:
-    wiki.creativecommons.org/Considerations_for_licensors
-
-     Considerations for the public: By using one of our public
-     licenses, a licensor grants the public permission to use the
-     licensed material under specified terms and conditions. If
-     the licensor's permission is not necessary for any reason--for
-     example, because of any applicable exception or limitation to
-     copyright--then that use is not regulated by the license. Our
-     licenses grant only permissions under copyright and certain
-     other rights that a licensor has authority to grant. Use of
-     the licensed material may still be restricted for other
-     reasons, including because others have copyright or other
-     rights in the material. A licensor may make special requests,
-     such as asking that all changes be marked or described.
-     Although not required by our licenses, you are encouraged to
-     respect those requests where reasonable. More considerations
-     for the public:
-    wiki.creativecommons.org/Considerations_for_licensees
-
-=======================================================================
-
-Creative Commons Attribution-NonCommercial-ShareAlike 4.0 International
-Public License
-
-By exercising the Licensed Rights (defined below), You accept and agree
-to be bound by the terms and conditions of this Creative Commons
-Attribution-NonCommercial-ShareAlike 4.0 International Public License
-("Public License"). To the extent this Public License may be
-interpreted as a contract, You are granted the Licensed Rights in
-consideration of Your acceptance of these terms and conditions, and the
-Licensor grants You such rights in consideration of benefits the
-Licensor receives from making the Licensed Material available under
-these terms and conditions.
-
-
-Section 1 -- Definitions.
-
-  a. Adapted Material means material subject to Copyright and Similar
-     Rights that is derived from or based upon the Licensed Material
-     and in which the Licensed Material is translated, altered,
-     arranged, transformed, or otherwise modified in a manner requiring
-     permission under the Copyright and Similar Rights held by the
-     Licensor. For purposes of this Public License, where the Licensed
-     Material is a musical work, performance, or sound recording,
-     Adapted Material is always produced where the Licensed Material is
-     synched in timed relation with a moving image.
-
-  b. Adapter's License means the license You apply to Your Copyright
-     and Similar Rights in Your contributions to Adapted Material in
-     accordance with the terms and conditions of this Public License.
-
-  c. BY-NC-SA Compatible License means a license listed at
-     creativecommons.org/compatiblelicenses, approved by Creative
-     Commons as essentially the equivalent of this Public License.
-
-  d. Copyright and Similar Rights means copyright and/or similar rights
-     closely related to copyright including, without limitation,
-     performance, broadcast, sound recording, and Sui Generis Database
-     Rights, without regard to how the rights are labeled or
-     categorized. For purposes of this Public License, the rights
-     specified in Section 2(b)(1)-(2) are not Copyright and Similar
-     Rights.
-
-  e. Effective Technological Measures means those measures that, in the
-     absence of proper authority, may not be circumvented under laws
-     fulfilling obligations under Article 11 of the WIPO Copyright
-     Treaty adopted on December 20, 1996, and/or similar international
-     agreements.
-
-  f. Exceptions and Limitations means fair use, fair dealing, and/or
-     any other exception or limitation to Copyright and Similar Rights
-     that applies to Your use of the Licensed Material.
-
-  g. License Elements means the license attributes listed in the name
-     of a Creative Commons Public License. The License Elements of this
-     Public License are Attribution, NonCommercial, and ShareAlike.
-
-  h. Licensed Material means the artistic or literary work, database,
-     or other material to which the Licensor applied this Public
-     License.
-
-  i. Licensed Rights means the rights granted to You subject to the
-     terms and conditions of this Public License, which are limited to
-     all Copyright and Similar Rights that apply to Your use of the
-     Licensed Material and that the Licensor has authority to license.
-
-  j. Licensor means the individual(s) or entity(ies) granting rights
-     under this Public License.
-
-  k. NonCommercial means not primarily intended for or directed towards
-     commercial advantage or monetary compensation. For purposes of
-     this Public License, the exchange of the Licensed Material for
-     other material subject to Copyright and Similar Rights by digital
-     file-sharing or similar means is NonCommercial provided there is
-     no payment of monetary compensation in connection with the
-     exchange.
-
-  l. Share means to provide material to the public by any means or
-     process that requires permission under the Licensed Rights, such
-     as reproduction, public display, public performance, distribution,
-     dissemination, communication, or importation, and to make material
-     available to the public including in ways that members of the
-     public may access the material from a place and at a time
-     individually chosen by them.
-
-  m. Sui Generis Database Rights means rights other than copyright
-     resulting from Directive 96/9/EC of the European Parliament and of
-     the Council of 11 March 1996 on the legal protection of databases,
-     as amended and/or succeeded, as well as other essentially
-     equivalent rights anywhere in the world.
-
-  n. You means the individual or entity exercising the Licensed Rights
-     under this Public License. Your has a corresponding meaning.
-
-
-Section 2 -- Scope.
-
-  a. License grant.
-
-       1. Subject to the terms and conditions of this Public License,
-          the Licensor hereby grants You a worldwide, royalty-free,
-          non-sublicensable, non-exclusive, irrevocable license to
-          exercise the Licensed Rights in the Licensed Material to:
-
-            a. reproduce and Share the Licensed Material, in whole or
-               in part, for NonCommercial purposes only; and
-
-            b. produce, reproduce, and Share Adapted Material for
-               NonCommercial purposes only.
-
-       2. Exceptions and Limitations. For the avoidance of doubt, where
-          Exceptions and Limitations apply to Your use, this Public
-          License does not apply, and You do not need to comply with
-          its terms and conditions.
-
-       3. Term. The term of this Public License is specified in Section
-          6(a).
-
-       4. Media and formats; technical modifications allowed. The
-          Licensor authorizes You to exercise the Licensed Rights in
-          all media and formats whether now known or hereafter created,
-          and to make technical modifications necessary to do so. The
-          Licensor waives and/or agrees not to assert any right or
-          authority to forbid You from making technical modifications
-          necessary to exercise the Licensed Rights, including
-          technical modifications necessary to circumvent Effective
-          Technological Measures. For purposes of this Public License,
-          simply making modifications authorized by this Section 2(a)
-          (4) never produces Adapted Material.
-
-       5. Downstream recipients.
-
-            a. Offer from the Licensor -- Licensed Material. Every
-               recipient of the Licensed Material automatically
-               receives an offer from the Licensor to exercise the
-               Licensed Rights under the terms and conditions of this
-               Public License.
-
-            b. Additional offer from the Licensor -- Adapted Material.
-               Every recipient of Adapted Material from You
-               automatically receives an offer from the Licensor to
-               exercise the Licensed Rights in the Adapted Material
-               under the conditions of the Adapter's License You apply.
-
-            c. No downstream restrictions. You may not offer or impose
-               any additional or different terms or conditions on, or
-               apply any Effective Technological Measures to, the
-               Licensed Material if doing so restricts exercise of the
-               Licensed Rights by any recipient of the Licensed
-               Material.
-
-       6. No endorsement. Nothing in this Public License constitutes or
-          may be construed as permission to assert or imply that You
-          are, or that Your use of the Licensed Material is, connected
-          with, or sponsored, endorsed, or granted official status by,
-          the Licensor or others designated to receive attribution as
-          provided in Section 3(a)(1)(A)(i).
-
-  b. Other rights.
-
-       1. Moral rights, such as the right of integrity, are not
-          licensed under this Public License, nor are publicity,
-          privacy, and/or other similar personality rights; however, to
-          the extent possible, the Licensor waives and/or agrees not to
-          assert any such rights held by the Licensor to the limited
-          extent necessary to allow You to exercise the Licensed
-          Rights, but not otherwise.
-
-       2. Patent and trademark rights are not licensed under this
-          Public License.
-
-       3. To the extent possible, the Licensor waives any right to
-          collect royalties from You for the exercise of the Licensed
-          Rights, whether directly or through a collecting society
-          under any voluntary or waivable statutory or compulsory
-          licensing scheme. In all other cases the Licensor expressly
-          reserves any right to collect such royalties, including when
-          the Licensed Material is used other than for NonCommercial
-          purposes.
-
-
-Section 3 -- License Conditions.
-
-Your exercise of the Licensed Rights is expressly made subject to the
-following conditions.
-
-  a. Attribution.
-
-       1. If You Share the Licensed Material (including in modified
-          form), You must:
-
-            a. retain the following if it is supplied by the Licensor
-               with the Licensed Material:
-
-                 i. identification of the creator(s) of the Licensed
-                    Material and any others designated to receive
-                    attribution, in any reasonable manner requested by
-                    the Licensor (including by pseudonym if
-                    designated);
-
-                ii. a copyright notice;
-
-               iii. a notice that refers to this Public License;
-
-                iv. a notice that refers to the disclaimer of
-                    warranties;
-
-                 v. a URI or hyperlink to the Licensed Material to the
-                    extent reasonably practicable;
-
-            b. indicate if You modified the Licensed Material and
-               retain an indication of any previous modifications; and
-
-            c. indicate the Licensed Material is licensed under this
-               Public License, and include the text of, or the URI or
-               hyperlink to, this Public License.
-
-       2. You may satisfy the conditions in Section 3(a)(1) in any
-          reasonable manner based on the medium, means, and context in
-          which You Share the Licensed Material. For example, it may be
-          reasonable to satisfy the conditions by providing a URI or
-          hyperlink to a resource that includes the required
-          information.
-       3. If requested by the Licensor, You must remove any of the
-          information required by Section 3(a)(1)(A) to the extent
-          reasonably practicable.
-
-  b. ShareAlike.
-
-     In addition to the conditions in Section 3(a), if You Share
-     Adapted Material You produce, the following conditions also apply.
-
-       1. The Adapter's License You apply must be a Creative Commons
-          license with the same License Elements, this version or
-          later, or a BY-NC-SA Compatible License.
-
-       2. You must include the text of, or the URI or hyperlink to, the
-          Adapter's License You apply. You may satisfy this condition
-          in any reasonable manner based on the medium, means, and
-          context in which You Share Adapted Material.
-
-       3. You may not offer or impose any additional or different terms
-          or conditions on, or apply any Effective Technological
-          Measures to, Adapted Material that restrict exercise of the
-          rights granted under the Adapter's License You apply.
-
-
-Section 4 -- Sui Generis Database Rights.
-
-Where the Licensed Rights include Sui Generis Database Rights that
-apply to Your use of the Licensed Material:
-
-  a. for the avoidance of doubt, Section 2(a)(1) grants You the right
-     to extract, reuse, reproduce, and Share all or a substantial
-     portion of the contents of the database for NonCommercial purposes
-     only;
-
-  b. if You include all or a substantial portion of the database
-     contents in a database in which You have Sui Generis Database
-     Rights, then the database in which You have Sui Generis Database
-     Rights (but not its individual contents) is Adapted Material,
-     including for purposes of Section 3(b); and
-
-  c. You must comply with the conditions in Section 3(a) if You Share
-     all or a substantial portion of the contents of the database.
-
-For the avoidance of doubt, this Section 4 supplements and does not
-replace Your obligations under this Public License where the Licensed
-Rights include other Copyright and Similar Rights.
-
-
-Section 5 -- Disclaimer of Warranties and Limitation of Liability.
-
-  a. UNLESS OTHERWISE SEPARATELY UNDERTAKEN BY THE LICENSOR, TO THE
-     EXTENT POSSIBLE, THE LICENSOR OFFERS THE LICENSED MATERIAL AS-IS
-     AND AS-AVAILABLE, AND MAKES NO REPRESENTATIONS OR WARRANTIES OF
-     ANY KIND CONCERNING THE LICENSED MATERIAL, WHETHER EXPRESS,
-     IMPLIED, STATUTORY, OR OTHER. THIS INCLUDES, WITHOUT LIMITATION,
-     WARRANTIES OF TITLE, MERCHANTABILITY, FITNESS FOR A PARTICULAR
-     PURPOSE, NON-INFRINGEMENT, ABSENCE OF LATENT OR OTHER DEFECTS,
-     ACCURACY, OR THE PRESENCE OR ABSENCE OF ERRORS, WHETHER OR NOT
-     KNOWN OR DISCOVERABLE. WHERE DISCLAIMERS OF WARRANTIES ARE NOT
-     ALLOWED IN FULL OR IN PART, THIS DISCLAIMER MAY NOT APPLY TO YOU.
-
-  b. TO THE EXTENT POSSIBLE, IN NO EVENT WILL THE LICENSOR BE LIABLE
-     TO YOU ON ANY LEGAL THEORY (INCLUDING, WITHOUT LIMITATION,
-     NEGLIGENCE) OR OTHERWISE FOR ANY DIRECT, SPECIAL, INDIRECT,
-     INCIDENTAL, CONSEQUENTIAL, PUNITIVE, EXEMPLARY, OR OTHER LOSSES,
-     COSTS, EXPENSES, OR DAMAGES ARISING OUT OF THIS PUBLIC LICENSE OR
-     USE OF THE LICENSED MATERIAL, EVEN IF THE LICENSOR HAS BEEN
-     ADVISED OF THE POSSIBILITY OF SUCH LOSSES, COSTS, EXPENSES, OR
-     DAMAGES. WHERE A LIMITATION OF LIABILITY IS NOT ALLOWED IN FULL OR
-     IN PART, THIS LIMITATION MAY NOT APPLY TO YOU.
-
-  c. The disclaimer of warranties and limitation of liability provided
-     above shall be interpreted in a manner that, to the extent
-     possible, most closely approximates an absolute disclaimer and
-     waiver of all liability.
-
-
-Section 6 -- Term and Termination.
-
-  a. This Public License applies for the term of the Copyright and
-     Similar Rights licensed here. However, if You fail to comply with
-     this Public License, then Your rights under this Public License
-     terminate automatically.
-
-  b. Where Your right to use the Licensed Material has terminated under
-     Section 6(a), it reinstates:
-
-       1. automatically as of the date the violation is cured, provided
-          it is cured within 30 days of Your discovery of the
-          violation; or
-
-       2. upon express reinstatement by the Licensor.
-
-     For the avoidance of doubt, this Section 6(b) does not affect any
-     right the Licensor may have to seek remedies for Your violations
-     of this Public License.
-
-  c. For the avoidance of doubt, the Licensor may also offer the
-     Licensed Material under separate terms or conditions or stop
-     distributing the Licensed Material at any time; however, doing so
-     will not terminate this Public License.
-
-  d. Sections 1, 5, 6, 7, and 8 survive termination of this Public
-     License.
-
-
-Section 7 -- Other Terms and Conditions.
-
-  a. The Licensor shall not be bound by any additional or different
-     terms or conditions communicated by You unless expressly agreed.
-
-  b. Any arrangements, understandings, or agreements regarding the
-     Licensed Material not stated herein are separate from and
-     independent of the terms and conditions of this Public License.
-
-
-Section 8 -- Interpretation.
-
-  a. For the avoidance of doubt, this Public License does not, and
-     shall not be interpreted to, reduce, limit, restrict, or impose
-     conditions on any use of the Licensed Material that could lawfully
-     be made without permission under this Public License.
-
-  b. To the extent possible, if any provision of this Public License is
-     deemed unenforceable, it shall be automatically reformed to the
-     minimum extent necessary to make it enforceable. If the provision
-     cannot be reformed, it shall be severed from this Public License
-     without affecting the enforceability of the remaining terms and
-     conditions.
-
-  c. No term or condition of this Public License will be waived and no
-     failure to comply consented to unless expressly agreed to by the
-     Licensor.
-
-  d. Nothing in this Public License constitutes or may be interpreted
-     as a limitation upon, or waiver of, any privileges and immunities
-     that apply to the Licensor or You, including from the legal
-     processes of any jurisdiction or authority.
-
-=======================================================================
-
-Creative Commons is not a party to its public
-licenses. Notwithstanding, Creative Commons may elect to apply one of
-its public licenses to material it publishes and in those instances
-will be considered the “Licensor.” The text of the Creative Commons
-public licenses is dedicated to the public domain under the CC0 Public
-Domain Dedication. Except for the limited purpose of indicating that
-material is shared under a Creative Commons public license or as
-otherwise permitted by the Creative Commons policies published at
-creativecommons.org/policies, Creative Commons does not authorize the
-use of the trademark "Creative Commons" or any other trademark or logo
-of Creative Commons without its prior written consent including,
-without limitation, in connection with any unauthorized modifications
-to any of its public licenses or any other arrangements,
-understandings, or agreements concerning use of licensed material. For
-the avoidance of doubt, this paragraph does not form part of the
-public licenses.
-
-Creative Commons may be contacted at creativecommons.org.
--- a/docs/assets/0.gif
+++ b/docs/assets/0.gif
--- a/docs/assets/3d_drone_1.jpg
+++ b/docs/assets/3d_drone_1.jpg
--- a/docs/assets/4/01.png
+++ b/docs/assets/4/01.png
--- a/docs/assets/4/02.png
+++ b/docs/assets/4/02.png
--- a/docs/assets/4/03.png
+++ b/docs/assets/4/03.png
--- a/docs/assets/4/03_1.png
+++ b/docs/assets/4/03_1.png
--- a/docs/assets/4/04.png
+++ b/docs/assets/4/04.png
--- a/docs/assets/4/05.png
+++ b/docs/assets/4/05.png
--- a/docs/assets/4/07.png
+++ b/docs/assets/4/07.png
--- a/docs/assets/4/07_1.png
+++ b/docs/assets/4/07_1.png
--- a/docs/assets/4/08.png
+++ b/docs/assets/4/08.png
--- a/docs/assets/4/08_1.png
+++ b/docs/assets/4/08_1.png
--- a/docs/assets/4/09.png
+++ b/docs/assets/4/09.png
--- a/docs/assets/4/10.png
+++ b/docs/assets/4/10.png
--- a/docs/assets/4/10_1.png
+++ b/docs/assets/4/10_1.png
--- a/docs/assets/4/12_1.png
+++ b/docs/assets/4/12_1.png
--- a/docs/assets/4/13.png
+++ b/docs/assets/4/13.png
--- a/docs/assets/4/13_1.png
+++ b/docs/assets/4/13_1.png
--- a/docs/assets/4/14.png
+++ b/docs/assets/4/14.png
--- a/docs/assets/4/15.png
+++ b/docs/assets/4/15.png
--- a/docs/assets/4/16.png
+++ b/docs/assets/4/16.png
--- a/docs/assets/4/18.png
+++ b/docs/assets/4/18.png
--- a/docs/assets/4/18_1.png
+++ b/docs/assets/4/18_1.png
--- a/docs/assets/4/19.png
+++ b/docs/assets/4/19.png
--- a/docs/assets/4/19_1.png
+++ b/docs/assets/4/19_1.png
--- a/docs/assets/4/20.png
+++ b/docs/assets/4/20.png
--- a/docs/assets/4/20_1.png
+++ b/docs/assets/4/20_1.png
--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
Oleg Kalachev	67877457b1	Add a marker to nti novgorod map	2019-04-10 18:42:08 +03:00
Oleg Kalachev	ebd92b7d85	docs: edit nti2019.md	2019-04-09 20:44:27 +03:00
Oleg Kalachev	8b144f2355	Edit NTI markers	2019-04-09 20:07:08 +03:00
Oleg Kalachev	90dfa4c473	Edit nti_novgirod markers map	2019-04-09 16:32:43 +03:00
Oleg Kalachev	af019f51f5	docs: small fix for markdownlint	2019-04-08 20:34:00 +03:00
Oleg Kalachev	b4492d16fa	Set correct camera frame by default for nti	2019-04-08 20:31:00 +03:00
Oleg Kalachev	d5cbe0c266	Expand nti novgorod aruco map	2019-04-08 20:30:36 +03:00
Oleg Kalachev	cd9fcb0595	aruco.launch: change default map to nti_novgorod.txt	2019-04-08 17:29:25 +03:00
Oleg Kalachev	f90c1a6329	Change some default for NTI 2019	2019-04-08 17:23:52 +03:00
Oleg Kalachev	c63e4265d6	aruco_map: add nti_novgorod map	2019-04-08 17:22:14 +03:00
Oleg Kalachev	60c97d2318	Merge branch 'master' into nti2019	2019-04-08 17:21:11 +03:00
sfalexrog	8dec500702	builder: Add ros_ws281x package	2019-04-07 17:50:31 +03:00
sfalexrog	09a8f702a7	docs: Add note about LED strip	2019-04-06 23:57:21 +03:00
sfalexrog	bcefb03f04	builder: Install paho-mqtt system-wide	2019-04-06 23:23:04 +03:00
sfalexrog	bc1ceb2fa0	Add annotated MQTT sample	2019-04-06 23:20:35 +03:00