Merge branch 'master' into nti2019

.markdownlint.json

@@ -24,8 +24,6 @@
             "Python",
             "C++",
             "PX4",
-            "px4.io",
-            "logs.px4.io",
             "QGroundControl",
             "QGC",
             "WireShark",
@@ -40,10 +38,6 @@
             "RPi",
             "Linux",
             "Windows",
-            "Docker",
-            "Travis",
-            "travis-ci.org",
-            "travis-ci.com",
             "macOS",
             "iOS",
             "Android",

.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:
@@ -64,18 +55,19 @@ jobs:
         - markdownlint docs
         - 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

README.md

@@ -10,7 +10,7 @@ Copter Express has implemented a large number of different autonomous drone proj
 
 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,59 +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
-```
-
-## Running
-
-Enable systemd service `roscore` (if not running):
-
-```bash
-sudo systemctl enable /home/<username>/catkin_ws/src/clever/deploy/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
-```
-
-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
 ```
+
+### Dependencies
+
+[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`

aruco_pose/CMakeLists.txt

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

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

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	

aruco_pose/map/nti_novgorod.txt

@@ -0,0 +1,6 @@
+34	0.33	0	0	0	0	0	0
+37	0.33	0.74	0	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.79	-0.15	1.20	0	0	0
+29	0.33	2.46	0.65	1.20	0	0	0

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>

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>
@@ -69,13 +67,11 @@ private:
 	Mat camera_matrix_, dist_coeffs_;
 	geometry_msgs::TransformStamped transform_;
 	geometry_msgs::PoseWithCovarianceStamped pose_;
-	vector<geometry_msgs::TransformStamped> markers_transforms_;
 	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_;
 
@@ -104,8 +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<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();
 
@@ -130,7 +124,6 @@ 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();
 		publishMapImage();
 		vis_markers_pub_.publish(vis_array_);
 
@@ -277,50 +270,10 @@ 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 == '#') {
-				ROS_DEBUG("aruco_map: 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
-				ROS_FATAL("aruco_map: Malformed input: %s", line.c_str());
-				ros::shutdown();
-				throw std::runtime_error("Malformed input");
-			}
-
-			if (!(s >> id >> length >> x >> y)) {
-				ROS_ERROR("aruco_map: 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)) {
-				ROS_DEBUG("aruco_map: No z coordinate provided for marker %d, assuming 0", id);
-				z = 0;
-			}
-			if (!(s >> yaw)) {
-				ROS_DEBUG("aruco_map: No yaw provided for marker %d, assuming 0", id);
-				yaw = 0;
-			}
-			if (!(s >> pitch)) {
-				ROS_DEBUG("aruco_map: No pitch provided for marker %d, assuming 0", id);
-				pitch = 0;
-			}
-			if (!(s >> roll)) {
-				ROS_DEBUG("aruco_map: No roll provided for marker %d, assuming 0", id);
-				roll = 0;
-			}
 			addMarker(id, length, x, y, z, yaw, pitch, roll);
 		}
 
@@ -386,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) {
-			ROS_ERROR("aruco_map: 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) {
-			ROS_ERROR("aruco_map: Marker id %d is already in the map", id);
-			return;
-		}
 		// Create transform
 		tf::Quaternion q;
 		q.setRPY(roll, pitch, yaw);
@@ -428,15 +368,6 @@ 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 visualization marker
 		visualization_msgs::Marker marker;
 		marker.header.frame_id = transform_.child_frame_id;
@@ -467,13 +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 publishMapImage()
 	{
 		cv::Size size(image_width_, image_height_);

aruco_pose/src/draw.cpp

@@ -87,7 +87,6 @@ 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);
 

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.

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>

aruco_pose/test/basic.py

@@ -1,131 +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) == 4
-    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[3].id == 3
-    assert markers.markers[3].length == approx(0.1)
-    assert markers.markers[3].pose.position.x == approx(-0.1805169666)
-    assert markers.markers[3].pose.position.y == approx(-0.200697302327)
-    assert markers.markers[3].pose.position.z == approx(0.585767514823)
-    assert markers.markers[3].pose.orientation.x == approx(-0.961738074009)
-    assert markers.markers[3].pose.orientation.y == approx(-0.0375180244707)
-    assert markers.markers[3].pose.orientation.z == approx(-0.0115387773672)
-    assert markers.markers[3].pose.orientation.w == approx(0.271144115664)
-    assert markers.markers[3].c1.x == approx(129.557723999)
-    assert markers.markers[3].c1.y == approx(49.557723999)
-    assert markers.markers[3].c2.x == approx(223.442276001)
-    assert markers.markers[3].c2.y == approx(49.557723999)
-    assert markers.markers[3].c3.x == approx(223.442276001)
-    assert markers.markers[3].c3.y == approx(143.442276001)
-    assert markers.markers[3].c4.x == approx(129.557723999)
-    assert markers.markers[3].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[2].id == 4
-    assert markers.markers[2].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')
 
-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(self):
+        pose = rospy.wait_for_message('aruco_map/pose', PoseWithCovarianceStamped, timeout=5)
+        self.assertEqual(pose.header.frame_id, 'main_camera_optical')
+        self.assertAlmostEqual(pose.pose.pose.position.x, -0.629167753342, places=4)
+        self.assertAlmostEqual(pose.pose.pose.position.y, 0.293822650809, places=4)
+        self.assertAlmostEqual(pose.pose.pose.position.z, 2.12641343155, places=4)
+        self.assertAlmostEqual(pose.pose.pose.orientation.x, -0.998383794799, places=4)
+        self.assertAlmostEqual(pose.pose.pose.orientation.y, -5.20919098575e-06, places=4)
+        self.assertAlmostEqual(pose.pose.pose.orientation.z, -0.0300861070302, places=4)
+        self.assertAlmostEqual(pose.pose.pose.orientation.w, 0.0482143590507, places=4)
 
-def test_map_markers_frames(node, tf_buffer):
-    stamp = rospy.get_rostime()
-    timeout = rospy.Duration(5)
+    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')
 
-    marker_1 = tf_buffer.lookup_transform('aruco_map', 'aruco_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_visualization(self):
+        vis = rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5)
 
-    marker_4 = tf_buffer.lookup_transform('aruco_map', 'aruco_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_map_debug(self):
+        img = rospy.wait_for_message('aruco_map/debug', Image, timeout=5)
 
-    marker_12 = tf_buffer.lookup_transform('aruco_map', 'aruco_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_transforms(self):
+    #     pass
 
-def test_visualization(node):
-    vis = rospy.wait_for_message('aruco_detect/visualization', VisMarkerArray, timeout=5)
-    assert len(vis.markers) == 9
 
-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_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)

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

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

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)

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>

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

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)

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>

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!

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

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>

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

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'

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>

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

book.json

@@ -4,7 +4,6 @@
     "author": "Copter Express",
     "language": "ru",
     "root": "docs/",
-    "gitbook": "^3.2.2",
     "plugins": [
         "youtube",
         "richquotes@https://github.com/okalachev/gitbook-plugin-richquotes.git",

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
 

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
 

builder/assets/kinetic-rosdep-clever.yaml

@@ -541,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]

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

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

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'}
@@ -113,10 +109,10 @@ ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/roscore
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/roscore.service' '/lib/systemd/system/'
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/kinetic-rosdep-clever.yaml' '/etc/ros/rosdep/'
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/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/kinetic-ros-clever.rosinstall' '/home/pi/ros_catkin_ws/'
 # Add PX4 udev rules
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/99-px4fmu.rules' '/lib/udev/rules.d/'
+${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/rosled.service' '/lib/systemd/system/'
 # Add rename script
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/clever_rename' '/usr/local/bin/clever_rename'
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} exec ${SCRIPTS_DIR}'/image-ros.sh' ${REPO_URL} ${IMAGE_VERSION} false false ${NUMBER_THREADS}

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
 

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
 

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,21 +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-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
 

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
 
@@ -63,7 +59,7 @@ echo_stamp "Install apt keys & repos"
 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://repo.coex.space/rpi-ros-kinetic stretch main" > /etc/apt/sources.list.d/rpi-ros-kinetic.list
@@ -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,24 +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 \
-python-systemd \
-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.20190517-1 \
-	raspberrypi-bootloader=1.20190517-1 \
-	libraspberrypi-bin=1.20190517-1 \
-	libraspberrypi-dev=1.20190517-1 \
-	libraspberrypi0=1.20190517-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
@@ -153,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
@@ -168,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.

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
 

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()
+

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

clever/CMakeLists.txt

@@ -162,6 +162,8 @@ add_executable(rc src/rc.cpp)
 
 add_executable(camera_markers src/camera_markers.cpp)
 
+add_executable(frames src/frames.cpp)
+
 add_executable(vpe_publisher src/vpe_publisher.cpp)
 
 target_link_libraries(simple_offboard
@@ -173,6 +175,8 @@ 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})
 
 add_dependencies(simple_offboard clever_generate_messages_cpp)
@@ -239,8 +243,3 @@ target_link_libraries(clever
 
 ## Add folders to be run by python nosetests
 # catkin_add_nosetests(test)
-
-if (CATKIN_ENABLE_TESTING)
-  find_package(rostest REQUIRED)
-  add_rostest(test/basic.test)
-endif()

clever/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 -->
 
@@ -24,8 +24,6 @@
         <param name="known_tilt" value="map"/>
         <param name="frame_id" value="aruco_map_detected" if="$(arg aruco_vpe)"/>
         <param name="frame_id" value="aruco_map" unless="$(arg aruco_vpe)"/>
-        <param name="markers/frame_id" value="aruco_map"/>
-        <param name="markers/child_frame_id_prefix" value="aruco_map_"/>
     </node>
 
     <!-- vpe publisher from aruco markers -->

clever/launch/clever.launch

@@ -5,10 +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="rc" default="true"/>
-    <arg name="rangefinder_vl53l1x" default="false"/>
+    <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">
@@ -20,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 -->
@@ -46,6 +46,11 @@
         <param name="reference_frames/base_link" value="map"/>
     </node>
 
+    <!-- Auxiliary frames -->
+    <node name="frames" pkg="clever" type="frames" output="screen">
+        <param name="body/frame_id" value="body"/>
+    </node>
+
     <!-- main camera -->
     <include file="$(find clever)/launch/main_camera.launch" if="$(arg main_camera)"/>
 
@@ -58,9 +63,15 @@
     <!-- vl53l1x ToF rangefinder -->
     <node name="vl53l1x" pkg="vl53l1x" type="vl53l1x_node" output="screen" if="$(arg rangefinder_vl53l1x)">
         <param name="frame_id" value="rangefinder"/>
+        <param name="offset" value="-0.05"/>
         <remap from="~range" to="mavros/distance_sensor/rangefinder_sub"/> <!-- redirect data to FCU -->
     </node>
 
     <!-- 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>

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

clever/launch/mavros.launch

@@ -56,7 +56,7 @@
     <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"/>

clever/package.xml

@@ -30,14 +30,13 @@
   <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> -->
 

clever/requirements.txt

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

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();
+}

clever/src/selfcheck.py

@@ -1,35 +1,21 @@
 #!/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.
-
 import math
-import subprocess
+from subprocess import Popen, PIPE
 import re
 import traceback
-from threading import Event
-import numpy
 import rospy
-from systemd import journal
-import tf2_ros
-import tf2_geometry_msgs
-from pymavlink import mavutil
 from std_srvs.srv import Trigger
 from sensor_msgs.msg import Image, CameraInfo, NavSatFix, Imu, Range
-from mavros_msgs.msg import State, OpticalFlowRad, Mavlink
+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
@@ -42,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
@@ -86,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:
@@ -100,116 +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
-
-
 @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)
+            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)
@@ -217,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:
@@ -316,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')
@@ -332,29 +150,26 @@ 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',
-                      get_param('EKF2_EVA_NOISE'),
-                      get_param('EKF2_EVP_NOISE'))
-
-    if not vis:
-        return
+        rospy.loginfo('EKF2_EVA_NOISE is %.3f, EKF2_EVP_NOISE is %.3f',
+            get_param('EKF2_EVA_NOISE'),
+            get_param('EKF2_EVP_NOISE'))
 
     # check vision pose and estimated pose inconsistency
     try:
@@ -425,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))
@@ -461,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)')
@@ -497,13 +312,13 @@ def check_rangefinder():
     # TODO: check FPS!
     rng = False
     try:
-        rospy.wait_for_message('mavros/distance_sensor/rangefinder_sub', 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')
@@ -515,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:
@@ -545,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:
@@ -557,68 +376,13 @@ def check_cpu_usage():
                     cpu.strip(), cmd.strip(), pid.strip())
 
 
-@check('clever.service')
-def check_clever_service():
-    output = subprocess.check_output('systemctl show -p ActiveState --value clever.service'.split())
-    if 'inactive' in output:
-        failure('clever.service is not running, try sudo systemctl restart clever')
-        return
-    j = journal.Reader()
-    j.this_boot()
-    j.add_match(_SYSTEMD_UNIT='clever.service')
-    j.add_disjunction()
-    j.add_match(UNIT='clever.service')
-    node_errors = []
-    r = re.compile(r'^(.*)\[(FATAL|ERROR)\] \[\d+.\d+\]: (.*)$')
-    for event in j:
-        msg = event['MESSAGE']
-        if ('Stopped Clever ROS package' in msg) or ('Started Clever ROS package' in msg):
-            node_errors = []
-        elif ('[ERROR]' in msg) or ('[FATAL]' in msg):
-            msg = r.search(msg).groups()[2]
-            if msg in node_errors:
-                continue
-            node_errors.append(msg)
-    for error in node_errors:
-        failure(error)
-
-
-@check('Image')
-def check_image():
-    info('version: %s', open('/etc/clever_version').read().strip())
-
-
-@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()

clever/src/simple_offboard.cpp

@@ -54,7 +54,6 @@ using mavros_msgs::Thrust;
 
 // tf2
 tf2_ros::Buffer tf_buffer;
-std::shared_ptr<tf2_ros::TransformBroadcaster> transform_broadcaster;
 
 // Parameters
 string local_frame;
@@ -71,7 +70,6 @@ ros::Duration global_position_timeout;
 ros::Duration battery_timeout;
 float default_speed;
 bool auto_release;
-bool land_only_in_offboard;
 std::map<string, string> reference_frames;
 
 // Publishers
@@ -89,7 +87,6 @@ AttitudeTarget att_raw_msg;
 Thrust thrust_msg;
 TwistStamped rates_msg;
 TransformStamped target;
-geometry_msgs::TransformStamped body;
 
 // State
 PoseStamped nav_start;
@@ -123,36 +120,13 @@ 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;
 }
 
-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)
@@ -389,12 +363,13 @@ void publish(const ros::Time stamp)
 
 	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;
-			transform_broadcaster->sendTransform(target);
+			tf_broadcaster.sendTransform(target);
 		}
 	}
 
@@ -669,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";
 
@@ -713,16 +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>();
 
 	// 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("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));
@@ -743,11 +709,11 @@ int main(int argc, char **argv)
 
 	// Telemetry subscribers
 	auto state_sub = nh.subscribe("mavros/state", 1, &handleMessage<mavros_msgs::State, state>);
+	auto local_position_sub = nh.subscribe("mavros/local_position/pose", 1, &handleMessage<PoseStamped, local_position>);
 	auto velocity_sub = nh.subscribe("mavros/local_position/velocity", 1, &handleMessage<TwistStamped, velocity>);
 	auto 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);

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

clever/test/basic.test

@@ -1,37 +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"/>
-
-    <param name="test_module" value="$(find clever)/test/basic.py"/>
-    <test test-name="basic_test" pkg="ros_pytest" type="ros_pytest_runner"/>
-</launch>