Run with QTWEBENGINE_DISABLE_SANDBOX=1

https://github.com/actions/runner-images/issues/6002

.gitattributes

@@ -3,6 +3,7 @@ roslib.js linguist-vendored
 eventemitter2.js linguist-vendored
 ros3d.js linguist-vendored
 three.min.js linguist-vendored
+json-to-pretty-yaml.js linguist-vendored
 aruco_pose/vendor/* linguist-vendored
 blockly/* linguist-vendored
 highlight/* linguist-vendored

.github/workflows/docs-docker.yaml

@@ -0,0 +1,23 @@
+name: Build docs Docker image
+
+on:
+  workflow_dispatch:
+  push:
+    branches: [ '*' ]
+
+jobs:
+  docs-docker:
+    runs-on: ubuntu-22.04
+    steps:
+      - uses: actions/checkout@v2
+
+      - name: Build Docker image
+        run: docker build -t clover-docs:latest docs
+
+      - name: Test Docker image
+        run: docker run -t --rm -v `pwd`:/clover clover-docs:latest
+
+      - name: Show results
+        run: |
+          ls -lh _book
+          ls -lh *.pdf

.github/workflows/docs.yml

@@ -4,16 +4,25 @@ on:
   push:
     branches: [ '*' ]
   pull_request:
-    branches: [ master ]
+    branches: [ '*' ]
+
+permissions:
+  contents: read
+  pages: write
+  id-token: write
+
+concurrency:
+  group: "pages"
+  cancel-in-progress: true
+
+defaults:
+  run:
+    shell: bash
 
 jobs:
   docs:
-    runs-on: ubuntu-18.04
+    runs-on: ubuntu-22.04
     steps:
-      - name: Cancel previous runs
-        uses: styfle/cancel-workflow-action@0.9.1
-        with:
-          access_token: ${{ github.token }}
       - uses: actions/checkout@v2
       - name: Use Node.js
         uses: actions/setup-node@v1
@@ -38,7 +47,11 @@ jobs:
          gitbook install
          gitbook build
       - name: Generate PDF
-        if: ${{ github.event_name == 'push' && github.ref == 'refs/heads/master' }}
+        id: generate-pdf
+        env:
+          GITBOOK_SKIP_PDF: ${{ secrets.GITBOOK_SKIP_PDF }}
+        continue-on-error: ${{ env.GITBOOK_SKIP_PDF != '' }}
+        if: ${{ github.event_name == 'push' }}
         run: |
           for i in 1 2 3 4; do gitbook pdf ./ _book/clover.pdf && break || sleep 1; done
           sudo apt-get -q install ghostscript
@@ -47,11 +60,27 @@ jobs:
           rm _book/clover_ru.pdf && mv _book/clover_ru_compressed.pdf _book/clover_ru.pdf
           rm _book/clover_en.pdf && mv _book/clover_en_compressed.pdf _book/clover_en.pdf
           ls -lah _book/clover*.pdf
-      - name: Deploy
+          echo '::set-output name=GITBOOK_PDF_OK::1'
-        uses: JamesIves/github-pages-deploy-action@4.1.3
+      - name: Download older PDFs
-        if: ${{ github.event_name == 'push' && github.ref == 'refs/heads/master' }}
+        if: ${{ !steps.generate-pdf.outputs.GITBOOK_PDF_OK }}
+        run: |
+          rm -f _book/clover*.pdf
+          wget --no-verbose https://clover.coex.tech/clover_ru.pdf -P _book/
+          wget --no-verbose https://clover.coex.tech/clover_en.pdf -P _book/
+      - name: Upload artifact
+        # if: ${{ github.event_name == 'push' && github.ref == 'refs/heads/master' }}
+        uses: actions/upload-pages-artifact@v1
         with:
-          branch: gh-pages
+          path: _book
-          folder: _book
+
-          clean: true
+  deploy-docs:
-          single-commit: true # to avoid multiple copies of large pdf files
+    if: ${{ github.event_name == 'push' && github.ref == 'refs/heads/master' }}
+    environment:
+      name: github-pages
+      url: ${{ steps.deployment.outputs.page_url }}
+    runs-on: ubuntu-latest
+    needs: docs
+    steps:
+      - name: Deploy to GitHub Pages
+        id: deployment
+        uses: actions/deploy-pages@v1

aruco_pose/CMakeLists.txt

@@ -83,11 +83,10 @@ add_message_files(
 )
 
 ## Generate services in the 'srv' folder
-# add_service_files(
+add_service_files(
-#   FILES
+  FILES
-#   Service1.srv
+  SetMarkers.srv
-#   Service2.srv
+)
-# )
 
 ## Generate actions in the 'action' folder
 # add_action_files(
@@ -120,6 +119,7 @@ generate_messages(
 ## Generate dynamic reconfigure parameters in the 'cfg' folder
 generate_dynamic_reconfigure_options(
   cfg/Detector.cfg
+  cfg/Map.cfg
 )
 
 ###################################

aruco_pose/README.md

@@ -51,6 +51,7 @@ It's recommended to run it within the same nodelet manager with the camera nodel
 
 * `image_raw` (*sensor_msgs/Image*) – camera image
 * `camera_info` (*sensor_msgs/CameraInfo*) – camera calibration info
+* `map_markers` (*aruco_pose/MarkerArray*) – list of markers to disable TF transform publishing
 
 #### Published
 
@@ -74,6 +75,7 @@ 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)
+* `~image_axis` – whether debug image should contain axis (default: true)
 * `~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:
@@ -97,6 +99,7 @@ See examples in [`map`](map/) directory.
 #### Published
 
 * `~pose` (*geometry_msgs/PoseWithCovarianceStamped*) – estimated map pose
+* `~map` (*aruco_pose/MarkerArray*) – list of markers in the loaded map
 * `~image` (*sensor_msgs/Image*) – planarized map image
 * `~visualization` (*visualization_msgs/MarkerArray*) – markers map visualization for rviz
 * `~debug` (*sensor_msgs/Image*) – debug image with detected markers and map axis

aruco_pose/cfg/Detector.cfg

@@ -10,6 +10,8 @@ gen = ParameterGenerator()
 
 gen.add("enabled", bool_t, 0, "if detection enabled", True)
 
+gen.add("length", double_t, 0, "markers' side length", min=0, max=10)
+
 gen.add("adaptiveThreshConstant",                 double_t, 0,
         "Constant for adaptive thresholding before finding contours",
          p.adaptiveThreshConstant, 0, 100)

aruco_pose/cfg/Map.cfg

@@ -0,0 +1,14 @@
+#!/usr/bin/env python
+PACKAGE = "aruco_pose"
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+gen.add("enabled", bool_t, 0, "if map detection enabled", True)
+
+gen.add("map", str_t, 0, "full path for the map file")
+
+gen.add("image_axis", bool_t, 0, "debug image axis", default=True)
+
+exit(gen.generate(PACKAGE, "aruco_pose", "Map"))

aruco_pose/map/cmit.txt

@@ -1,3 +1,4 @@
+# id	length	x	y	z	rot_z	rot_y	rot_x
 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	

aruco_pose/map/office.txt

@@ -1,3 +1,4 @@
+# id	length	x	y	z	rot_z	rot_y	rot_x
 107	0.33	0	0	0	0	0	0
 106	0.33	0.77	0	0	0	0	0
 105	0.33	0	0.77	0	0	0	0

aruco_pose/map/office_ceiling.txt

@@ -1,3 +1,4 @@
+# id	length	x	y	z	rot_z	rot_y	rot_x
 14	0.365	0.000	0.0	0	0	0	0
 15	0.365	1.335	0.0	0	0	0	0
 30	0.365	2.865	0.0	0	0	0	0

aruco_pose/src/aruco_detect.cpp

@@ -48,6 +48,7 @@
 #include <aruco_pose/Marker.h>
 #include <aruco_pose/MarkerArray.h>
 #include <aruco_pose/DetectorConfig.h>
+#include <aruco_pose/SetMarkers.h>
 
 #include "utils.h"
 #include <memory>
@@ -69,8 +70,10 @@ private:
 	image_transport::CameraSubscriber img_sub_;
 	ros::Publisher markers_pub_, vis_markers_pub_;
 	ros::Subscriber map_markers_sub_;
+	ros::ServiceServer set_markers_srv_;
 	bool estimate_poses_, send_tf_, auto_flip_;
 	double length_;
+	ros::Duration transform_timeout_;
 	std::unordered_map<int, double> length_override_;
 	std::string frame_id_prefix_, known_tilt_;
 	Mat camera_matrix_, dist_coeffs_;
@@ -97,6 +100,7 @@ public:
 			ros::shutdown();
 		}
 		readLengthOverride(nh_priv_);
+		transform_timeout_ = ros::Duration(nh_priv_.param("transform_timeout", 0.02));
 
 		known_tilt_ = nh_priv_.param<std::string>("known_tilt", "");
 		auto_flip_ = nh_priv_.param("auto_flip", false);
@@ -114,6 +118,8 @@ public:
 		dyn_srv_ = std::make_shared<dynamic_reconfigure::Server<aruco_pose::DetectorConfig>>(nh_priv_);
 		dyn_srv_->setCallback(std::bind(&ArucoDetect::paramCallback, this, std::placeholders::_1, std::placeholders::_2));
 
+		set_markers_srv_ = nh_priv_.advertiseService("set_length_override", &ArucoDetect::setMarkers, 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);
@@ -172,7 +178,7 @@ private:
 				if (!known_tilt_.empty()) {
 					try {
 						snap_to = tf_buffer_->lookupTransform(msg->header.frame_id, known_tilt_,
-						                                      msg->header.stamp, ros::Duration(0.02));
+						                                      msg->header.stamp, transform_timeout_);
 					} catch (const tf2::TransformException& e) {
 						NODELET_WARN_THROTTLE(5, "can't snap: %s", e.what());
 					}
@@ -346,6 +352,29 @@ private:
 		}
 	}
 
+	bool setMarkers(aruco_pose::SetMarkers::Request& req, aruco_pose::SetMarkers::Response& res)
+	{
+		for (auto const& marker : req.markers) {
+			if (marker.id > 999) {
+				res.message = "Invalid marker id: " + std::to_string(marker.id);
+				ROS_ERROR("%s", res.message.c_str());
+				return true;
+			}
+			if (!std::isfinite(marker.length) || marker.length <= 0) {
+				res.message = "Invalid marker " + std::to_string(marker.id) + " length: " + std::to_string(marker.length);
+				ROS_ERROR("%s", res.message.c_str());
+				return true;
+			}
+		}
+
+		for (auto const& marker : req.markers) {
+			length_override_[marker.id] = marker.length;
+		}
+
+		res.success = true;
+		return true;
+	}
+
 	void mapMarkersCallback(const aruco_pose::MarkerArray& msg)
 	{
 		map_markers_ids_.clear();
@@ -356,7 +385,8 @@ private:
 
 	void paramCallback(aruco_pose::DetectorConfig &config, uint32_t level)
 	{
-		enabled_ = config.enabled;
+		enabled_ = config.enabled && config.length > 0;
+		length_ = config.length;
 		parameters_->adaptiveThreshConstant = config.adaptiveThreshConstant;
 		parameters_->adaptiveThreshWinSizeMin = config.adaptiveThreshWinSizeMin;
 		parameters_->adaptiveThreshWinSizeMax = config.adaptiveThreshWinSizeMax;

aruco_pose/src/aruco_map.cpp

@@ -19,11 +19,13 @@
 #include <vector>
 #include <fstream>
 #include <algorithm>
+#include <memory>
 #include <ros/ros.h>
 #include <nodelet/nodelet.h>
 #include <pluginlib/class_list_macros.h>
 #include <image_transport/image_transport.h>
 #include <cv_bridge/cv_bridge.h>
+#include <dynamic_reconfigure/server.h>
 #include <tf/transform_datatypes.h>
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
@@ -41,6 +43,7 @@
 
 #include <aruco_pose/MarkerArray.h>
 #include <aruco_pose/Marker.h>
+#include <aruco_pose/MapConfig.h>
 
 #include <opencv2/opencv.hpp>
 #include <opencv2/aruco.hpp>
@@ -74,6 +77,9 @@ private:
 	tf2_ros::StaticTransformBroadcaster static_br_;
 	tf2_ros::Buffer tf_buffer_;
 	tf2_ros::TransformListener tf_listener_{tf_buffer_};
+	std::shared_ptr<dynamic_reconfigure::Server<aruco_pose::MapConfig>> dyn_srv_;
+	bool enabled_ = true;
+	std::string type_;
 	visualization_msgs::MarkerArray vis_array_;
 	std::string known_tilt_, map_, markers_frame_, markers_parent_frame_;
 	int image_width_, image_height_, image_margin_;
@@ -89,15 +95,14 @@ 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);
+		markers_pub_ = nh_priv_.advertise<aruco_pose::MarkerArray>("map", 1, true);
 
 		board_ = cv::makePtr<cv::aruco::Board>();
 		board_->dictionary = cv::aruco::getPredefinedDictionary(
 			                 static_cast<cv::aruco::PREDEFINED_DICTIONARY_NAME>(nh_priv_.param("dictionary", 2)));
 		camera_matrix_ = cv::Mat::zeros(3, 3, CV_64F);
 
-		std::string type, map;
+		type_ = nh_priv_.param<std::string>("type", "map");
-		type = nh_priv_.param<std::string>("type", "map");
 		transform_.child_frame_id = nh_priv_.param<std::string>("frame_id", "aruco_map");
 		known_tilt_ = nh_priv_.param<std::string>("known_tilt", "");
 		auto_flip_ = nh_priv_.param("auto_flip", false);
@@ -110,13 +115,13 @@ public:
 
 		// createStripLine();
 
-		if (type == "map") {
+		if (type_ == "map") {
-			param(nh_priv_, "map", map);
+			map_ = nh_priv_.param<std::string>("map" , "");
-			loadMap(map);
+			loadMap(map_);
-		} else if (type == "gridboard") {
+		} else if (type_ == "gridboard") {
 			createGridBoard(nh_priv_);
 		} else {
-			NODELET_FATAL("unknown type: %s", type.c_str());
+			NODELET_FATAL("unknown type: %s", type_.c_str());
 			ros::shutdown();
 		}
 
@@ -124,10 +129,7 @@ public:
 		vis_markers_pub_ = nh_priv_.advertise<visualization_msgs::MarkerArray>("visualization", 1, true);
 		debug_pub_ = it_priv.advertise("debug", 1);
 
-		publishMarkersFrames();
+		publishMap();
-		publishMarkers();
-		publishMapImage();
-		vis_markers_pub_.publish(vis_array_);
 
 		image_sub_.subscribe(nh_, "image_raw", 1);
 		info_sub_.subscribe(nh_, "camera_info", 1);
@@ -136,6 +138,12 @@ 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));
 
+		dyn_srv_ = std::make_shared<dynamic_reconfigure::Server<aruco_pose::MapConfig>>(nh_priv_);
+		dynamic_reconfigure::Server<aruco_pose::MapConfig>::CallbackType cb;
+
+		cb = std::bind(&ArucoMap::paramCallback, this, std::placeholders::_1, std::placeholders::_2);
+		dyn_srv_->setCallback(cb);
+
 		NODELET_INFO("ready");
 	}
 
@@ -143,6 +151,9 @@ public:
 	              const sensor_msgs::CameraInfoConstPtr& cinfo,
 	              const aruco_pose::MarkerArrayConstPtr& markers)
 	{
+		if (!enabled_) return;
+		if (markers->markers.empty()) return; // map not loaded
+
 		int valid = 0;
 		int count = markers->markers.size();
 		std::vector<int> ids;
@@ -268,9 +279,17 @@ publish_debug:
 		std::ifstream f(filename);
 		std::string line;
 
+		clearMarkers();
+
+		if (map_ == "") {
+			NODELET_INFO("No map loaded");
+			return;
+		}
+
 		if (!f.good()) {
-			NODELET_FATAL("%s - %s", strerror(errno), filename.c_str());
+			NODELET_ERROR("%s - %s", strerror(errno), filename.c_str());
-			ros::shutdown();
+			map_ = "";
+			return;
 		}
 
 		while (std::getline(f, line)) {
@@ -296,9 +315,10 @@ publish_debug:
 				s.putback(first);
 			} else {
 				// Probably garbage data; inform user and throw an exception, possibly killing nodelet
-				NODELET_FATAL("Malformed input: %s", line.c_str());
+				NODELET_ERROR("Malformed input: %s", line.c_str());
-				ros::shutdown();
+				map_ = "";
-				throw std::runtime_error("Malformed input");
+				clearMarkers();
+				return;
 			}
 
 			if (!(s >> id >> length >> x >> y)) {
@@ -329,6 +349,14 @@ publish_debug:
 		NODELET_INFO("loading %s complete (%d markers)", filename.c_str(), static_cast<int>(board_->ids.size()));
 	}
 
+	void publishMap()
+	{
+		publishMarkersFrames();
+		publishMarkers();
+		publishMapImage();
+		vis_markers_pub_.publish(vis_array_);
+	}
+
 	void createGridBoard(ros::NodeHandle& nh)
 	{
 		NODELET_INFO("generate gridboard");
@@ -370,6 +398,15 @@ publish_debug:
 		}
 	}
 
+	void clearMarkers()
+	{
+		board_->ids.clear();
+		board_->objPoints.clear();
+		markers_.markers.clear();
+		vis_array_.markers.clear();
+		markers_transforms_.clear();
+	}
+
 	// void createStripLine()
 	// {
 	// 	visualization_msgs::Marker marker;
@@ -509,6 +546,22 @@ publish_debug:
 		msg.image = image;
 		img_pub_.publish(msg.toImageMsg());
 	}
+
+	void paramCallback(aruco_pose::MapConfig &config, uint32_t level)
+	{
+		// https://github.com/CopterExpress/clover/commit/2cd334c474e3ed04ef65ca1ba7f08ab535a3dc6d#diff-942723f9452c398ae93f1a91427f9a7b614be5e5871f8a3e590f324d804f0d58R356
+		enabled_ = config.enabled;
+		if (type_ == "map" && config.map != map_) {
+			map_ = config.map;
+			loadMap(map_);
+			publishMap();
+		}
+
+		if (config.image_axis != image_axis_) {
+			image_axis_ = config.image_axis;
+			publishMapImage();
+		}
+	}
 };
 
 PLUGINLIB_EXPORT_CLASS(ArucoMap, nodelet::Nodelet)

aruco_pose/srv/SetMarkers.srv

@@ -0,0 +1,7 @@
+# * Add or change markers in the map
+# * Change markers' properties, e. g. lengths
+
+Marker[] markers # if length or pose is nan - remove from map
+---
+bool success
+string message

aruco_pose/test/basic.py

@@ -143,7 +143,7 @@ def test_map_image(node):
     assert img.encoding in ('mono8', 'rgb8')
 
 def test_map_markers(node):
-    markers = rospy.wait_for_message('aruco_map/markers', MarkerArray, timeout=5)
+    markers = rospy.wait_for_message('aruco_map/map', MarkerArray, timeout=5)
     assert markers.markers[0].id == 1
     assert markers.markers[1].id == 2
     assert markers.markers[2].id == 3

aruco_pose/test/test_node_failure.py

@@ -2,6 +2,7 @@ import rospy
 import pytest
 
 from visualization_msgs.msg import MarkerArray as VisMarkerArray
+from aruco_pose.msg import MarkerArray
 
 
 @pytest.fixture
@@ -9,5 +10,5 @@ def node():
     return rospy.init_node('aruco_pose_test', anonymous=True)
 
 def test_node_failure(node):
-    with pytest.raises(rospy.exceptions.ROSException):
+    assert rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5).markers == []
-        rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5)
+    assert rospy.wait_for_message('aruco_map/map', MarkerArray, timeout=5).markers == []

builder/test/tests.sh

@@ -60,7 +60,7 @@ rosversion image_view
 
 # validate some versions
 [[ $(rosversion cv_camera) == "0.5.1" ]] # patched version with init fix
-[[ $(rosversion ws281x) == "0.0.12" ]]
+[[ $(rosversion ws281x) == "0.0.13" ]]
 
 # validate examples are present
 [[ $(ls /home/pi/examples/*) ]]

clover/CMakeLists.txt

@@ -17,6 +17,7 @@ find_package(catkin REQUIRED COMPONENTS
   message_generation
   geometry_msgs
   sensor_msgs
+  led_msgs
   geographic_msgs
   tf
   tf2
@@ -24,6 +25,7 @@ find_package(catkin REQUIRED COMPONENTS
   tf2_ros
   image_transport
   cv_bridge
+  dynamic_reconfigure
 )
 
 list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/cmake")
@@ -126,10 +128,9 @@ generate_messages(
 ##     and list every .cfg file to be processed
 
 ## Generate dynamic reconfigure parameters in the 'cfg' folder
-# generate_dynamic_reconfigure_options(
+generate_dynamic_reconfigure_options(
-#   cfg/DynReconf1.cfg
+  cfg/Flow.cfg
-#   cfg/DynReconf2.cfg
+)
-# )
 
 ###################################
 ## catkin specific configuration ##
@@ -211,6 +212,8 @@ add_dependencies(clover_led ${PROJECT_NAME}_generate_messages_cpp)
 
 add_dependencies(shell ${PROJECT_NAME}_generate_messages_cpp)
 
+add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencfg)
+
 ## 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

clover/cfg/Flow.cfg

@@ -0,0 +1,10 @@
+#!/usr/bin/env python
+PACKAGE = "clover"
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+gen.add("enabled", bool_t, 0, "if optical flow enabled", True)
+
+exit(gen.generate(PACKAGE, "clover", "Flow"))

clover/launch/aruco.launch

@@ -8,16 +8,20 @@
 
     <!-- For additional help go to https://clover.coex.tech/aruco -->
 
+    <arg name="force_init" default="false"/>
+    <arg name="disable" default="false"/> <!-- only force init -->
+
     <!-- aruco_detect: detect aruco markers, estimate poses -->
-    <node name="aruco_detect" pkg="nodelet" if="$(arg aruco_detect)" type="nodelet" args="load aruco_pose/aruco_detect main_camera_nodelet_manager" output="screen" clear_params="true" respawn="true">
+    <node name="aruco_detect" pkg="nodelet" if="$(eval aruco_detect and not disable)" type="nodelet" args="load aruco_pose/aruco_detect main_camera_nodelet_manager" output="screen" clear_params="true" respawn="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"/>
+        <remap from="map_markers" to="aruco_map/map"/>
         <param name="estimate_poses" value="true"/>
         <param name="send_tf" value="true"/>
         <param name="known_tilt" value="map" if="$(eval placement == 'floor')"/>
         <param name="known_tilt" value="map_flipped" if="$(eval placement == 'ceiling')"/>
         <param name="length" value="$(arg length)"/>
+        <param name="transform_timeout" value="0.1"/>
         <!-- aruco detector parameters -->
         <param name="cornerRefinementMethod" value="2"/> <!-- contour refinement -->
         <param name="minMarkerPerimeterRate" value="0.075"/> <!-- 0.075 for 320x240, 0.0375 for 640x480 -->
@@ -26,7 +30,7 @@
     </node>
 
     <!-- aruco_map: estimate aruco map pose -->
-    <node name="aruco_map" pkg="nodelet" type="nodelet" if="$(arg aruco_map)" args="load aruco_pose/aruco_map main_camera_nodelet_manager" output="screen" clear_params="true" respawn="true">
+    <node name="aruco_map" pkg="nodelet" type="nodelet" if="$(eval aruco_map and not disable)" args="load aruco_pose/aruco_map main_camera_nodelet_manager" output="screen" clear_params="true" respawn="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"/>
@@ -41,11 +45,11 @@
     </node>
 
     <!-- vpe publisher from aruco markers -->
-    <node name="vpe_publisher" pkg="clover" type="vpe_publisher" if="$(arg aruco_vpe)" output="screen" clear_params="true">
+    <node name="vpe_publisher" pkg="clover" type="vpe_publisher" if="$(eval aruco_vpe or force_init)" output="screen" clear_params="true">
-        <remap from="~pose_cov" to="aruco_map/pose"/>
+        <remap from="~pose_cov" to="aruco_map/pose" if="$(arg aruco_vpe)"/>
         <remap from="~vpe" to="mavros/vision_pose/pose"/>
-        <param name="frame_id" value="aruco_map_detected"/>
+        <param name="frame_id" value="aruco_map_detected" if="$(arg aruco_vpe)"/>
-        <param name="publish_zero" value="true"/>
+        <param name="force_init" value="$(arg force_init)"/>
         <param name="offset_frame_id" value="aruco_map"/>
     </node>
 </launch>

clover/launch/clover.launch

@@ -12,6 +12,7 @@
     <arg name="led" default="true"/>
     <arg name="blocks" default="false"/>
     <arg name="rc" default="false"/>
+    <arg name="force_init" value="true"/> <!-- force estimator to init by publishing zero pose -->
 
     <arg name="simulator" default="false"/> <!-- flag that we are operating on a simulated drone -->
 
@@ -33,7 +34,10 @@
     </node>
 
     <!-- aruco markers -->
-    <include file="$(find clover)/launch/aruco.launch" if="$(arg aruco)"/>
+    <include file="$(find clover)/launch/aruco.launch" if="$(eval aruco or force_init)">
+        <arg name="force_init" value="$(arg force_init)"/>
+        <arg name="disable" value="$(eval not aruco)"/>
+    </include>
 
     <!-- optical flow -->
     <node pkg="nodelet" type="nodelet" name="optical_flow" args="load clover/optical_flow main_camera_nodelet_manager" if="$(arg optical_flow)" clear_params="true" output="screen" respawn="true">

clover/launch/mavros.launch

@@ -39,7 +39,7 @@
         <rosparam command="load" file="$(find clover)/launch/mavros_config.yaml"/>
 
         <!-- remap rangefinder -->
-        <remap from="mavros/distance_sensor/rangefinder_sub" to="rangefinder/range"/>
+        <remap from="mavros/distance_sensor/rangefinder_sub" to="$(arg distance_sensor_remap)" if="$(eval bool(distance_sensor_remap))"/>
 
         <rosparam param="plugin_whitelist">
             - altitude

clover/launch/mavros_config.yaml

@@ -78,6 +78,9 @@ distance_sensor:
       field_of_view: 0.5
   rangefinder_sub:
       subscriber: true
+      id: 1
+      orientation: PITCH_270
+      covariance: 1  # cm
 
 # fake_gps
 fake_gps:

clover/launch/simulator.launch

@@ -0,0 +1,4 @@
+<launch>
+    <!-- shurtcut for running the simulation (`roslaunch clover simulator.launch`) -->
+    <include file="$(find clover_simulation)/launch/simulator.launch"/>
+</launch>

clover/package.xml

@@ -39,6 +39,7 @@
   <depend>tf2_web_republisher</depend>
   <depend condition="$ROS_PYTHON_VERSION == 2">python-lxml</depend>
   <depend condition="$ROS_PYTHON_VERSION == 3">python3-lxml</depend>
+  <depend>dynamic_reconfigure</depend>
   <exec_depend>python-pymavlink</exec_depend>
   <!-- Use test_depend for packages you need only for testing: -->
   <!--   <test_depend>gtest</test_depend> -->

clover/src/autotest/autotest_aruco.py

@@ -0,0 +1,87 @@
+#!/usr/bin/env python3
+
+import rospy
+import math
+import signal
+import sys
+import dynamic_reconfigure.client
+from clover import srv
+from std_srvs.srv import Trigger
+from sensor_msgs.msg import Range
+from aruco_pose.msg import MarkerArray
+from util import handle_response
+
+rospy.init_node('autotest_aruco', disable_signals=True) # disable signals to allow interrupting with ctrl+c
+
+flow_client = dynamic_reconfigure.client.Client('optical_flow')
+get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry)
+navigate = handle_response(rospy.ServiceProxy('navigate', srv.Navigate))
+land = handle_response(rospy.ServiceProxy('land', Trigger))
+
+def interrupt(sig, frame):
+    print('\nInterrupted, landing...')
+    land()
+    sys.exit(0)
+
+signal.signal(signal.SIGINT, interrupt)
+
+def print_current_map_position():
+    telem = get_telemetry()
+    dist = rospy.wait_for_message('rangefinder/range', Range).range
+    print('Map position:\tx={:.1f}\ty={:.1f}\tz={:.1f}\tyaw={:.1f}\tdist={:.2f}'.format(telem.x, telem.y, telem.z, telem.yaw, dist))
+
+def navigate_wait(x=0, y=0, z=0, yaw=float('nan'), yaw_rate=0, speed=0.5, \
+        frame_id='body', tolerance=0.2, auto_arm=False):
+
+    res = navigate(x=x, y=y, z=z, yaw=yaw, yaw_rate=yaw_rate, speed=speed, \
+        frame_id=frame_id, auto_arm=auto_arm)
+
+    if not res.success:
+        return res
+
+    while not rospy.is_shutdown():
+        telem = get_telemetry(frame_id='navigate_target')
+        if math.sqrt(telem.x ** 2 + telem.y ** 2 + telem.z ** 2) < tolerance:
+            return res
+        rospy.sleep(0.2)
+
+markers = rospy.wait_for_message('aruco_map/map', MarkerArray, timeout=3)
+left = min(marker.pose.position.x for marker in markers.markers)
+bottom = min(marker.pose.position.y for marker in markers.markers)
+width = max(marker.pose.position.x for marker in markers.markers)
+height = max(marker.pose.position.y for marker in markers.markers)
+center_x = left + width / 2
+center_y = bottom + height / 2
+
+print('Map rect: %g %g - %g %g' % (left, bottom, width, height))
+
+input('Take off and hover 1 m [enter] ')
+navigate_wait(x=0, y=0, z=1, frame_id='body', auto_arm=True)
+print_current_map_position()
+
+input('Go to corner %g %g 1.5 speed 1 [enter] ' % (width, height))
+navigate_wait(x=width, y=height, z=1.5, speed=1, frame_id='aruco_map')
+print_current_map_position()
+
+input('Go to center %g %g 1.5 speed 5 [enter] ' % (center_x, center_y))
+navigate_wait(x=center_x, y=center_y, z=1.5, speed=5, frame_id='aruco_map')
+print_current_map_position()
+
+input('Disable optical flow and keep hovering [enter] ')
+flow_client.update_configuration({'enabled': False})
+rospy.sleep(5)
+
+input('Enable optical flow back [enter] ')
+flow_client.update_configuration({'enabled': True})
+
+input('Go to side 1 %g 2 heading top [enter] ' % (center_y))
+navigate_wait(x=1, y=center_y, z=2, yaw=1.57, frame_id='aruco_map')
+print_current_map_position()
+
+marker_id = markers.markers[0].id
+input('Go to marker %d z=1.5 [enter] ' % marker_id)
+navigate_wait(x=0, y=0, z=1.5, yaw=0, frame_id='aruco_%d' % marker_id)
+print_current_map_position()
+
+input('Perform landing [enter] ')
+land()

clover/src/autotest/autotest_flight.py

@@ -0,0 +1,100 @@
+#!/usr/bin/env python3
+
+import rospy
+import math
+from math import nan
+import signal
+import sys
+from clover import srv
+from std_srvs.srv import Trigger
+from sensor_msgs.msg import Range
+from util import handle_response
+
+rospy.init_node('autotest_flight', disable_signals=True) # disable signals to allow interrupting with ctrl+c
+
+get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry)
+navigate = handle_response(rospy.ServiceProxy('navigate', srv.Navigate))
+navigate_global = handle_response(rospy.ServiceProxy('navigate_global', srv.NavigateGlobal))
+set_position = handle_response(rospy.ServiceProxy('set_position', srv.SetPosition))
+set_velocity = handle_response(rospy.ServiceProxy('set_velocity', srv.SetVelocity))
+set_attitude = handle_response(rospy.ServiceProxy('set_attitude', srv.SetAttitude))
+set_rates = handle_response(rospy.ServiceProxy('set_rates', srv.SetRates))
+land = handle_response(rospy.ServiceProxy('land', Trigger))
+
+def interrupt(sig, frame):
+    print('\nInterrupted, landing...')
+    land()
+    sys.exit(0)
+
+signal.signal(signal.SIGINT, interrupt)
+
+def navigate_wait(x=0, y=0, z=0, yaw=nan, yaw_rate=0, speed=0.5, \
+        frame_id='body', tolerance=0.2, auto_arm=False):
+
+    res = navigate(x=x, y=y, z=z, yaw=yaw, yaw_rate=yaw_rate, speed=speed, \
+        frame_id=frame_id, auto_arm=auto_arm)
+
+    if not res.success:
+        return res
+
+    while not rospy.is_shutdown():
+        telem = get_telemetry(frame_id='navigate_target')
+        if math.sqrt(telem.x ** 2 + telem.y ** 2 + telem.z ** 2) < tolerance:
+            return res
+        rospy.sleep(0.2)
+
+def print_distance():
+    dist = rospy.wait_for_message('rangefinder/range', Range).range
+    print('Distance: {:.2f}'.format(dist))
+
+input('Take off and hover 1 m [enter] ')
+navigate_wait(z=1, frame_id='body', auto_arm=True)
+print_distance()
+start = get_telemetry()
+
+input('Fly forward 2 m [enter] ')
+navigate_wait(x=2, frame_id='navigate_target')
+print_distance()
+
+input('Climb 0.5 m [enter] ')
+navigate_wait(z=0.5, frame_id='navigate_target')
+print_distance()
+
+input('Rotate left 90° [enter] ')
+navigate(yaw=math.pi / 2, frame_id='navigate_target')
+rospy.sleep(3)
+
+input('Use set_velocity to fly forward 2 m speed 1 [enter]')
+set_velocity(vx=1, vy=0.0, vz=0, frame_id='body')
+rospy.sleep(2)
+set_position(frame_id='body')
+
+input('Rotate right 90° [enter] ')
+navigate(yaw=-math.pi / 2, frame_id='navigate_target')
+rospy.sleep(3)
+
+input('Use set_attitude to fly backwards [enter]')
+set_attitude(pitch=-0.3, roll=0, yaw=0, thrust=0.5, frame_id='body')
+rospy.sleep(0.3)
+set_position(frame_id='body')
+
+input('Use set_attitude to fly right [enter]')
+set_attitude(pitch=0, roll=0.3, yaw=0, thrust=0.5, frame_id='body')
+rospy.sleep(0.5)
+set_position(frame_id='body')
+
+input('Use set_rates to fly right [enter]')
+set_rates(roll_rate=1.2, thrust=0.5)
+rospy.sleep(0.4)
+set_position(frame_id='body')
+
+input('Rotate 360° to the right using yaw_rate [enter]')
+set_position(x=nan, y=nan, z=nan, frame_id='body', yaw=nan, yaw_rate=-1)
+rospy.sleep(2 * math.pi)
+set_position(frame_id='body')
+
+input('Return to start point [enter]')
+navigate_wait(x=start.x, y=start.y, z=start.z, yaw=start.yaw, speed=1, frame_id='map')
+
+input('Land [enter]')
+land()

clover/src/autotest/autotest_led.py

@@ -0,0 +1,72 @@
+#!/usr/bin/env python3
+
+import rospy
+import functools
+from clover.srv import SetLEDEffect
+from led_msgs.srv import SetLEDs
+from led_msgs.msg import LEDStateArray, LEDState
+from util import handle_response
+
+rospy.init_node('autotest_led', disable_signals=True)
+
+set_leds = handle_response(rospy.ServiceProxy('led/set_leds', SetLEDs))
+set_effect = handle_response(rospy.ServiceProxy('led/set_effect', SetLEDEffect))
+
+led_count = len(rospy.wait_for_message('led/state', LEDStateArray, timeout=10).leds)
+print('LED count =', led_count)
+
+print('== Testing effects ==')
+
+input('Fill red [enter] ')
+set_effect(r=255, g=0, b=0)
+
+input('Fill green [enter] ')
+set_effect(r=0, g=100, b=0)
+
+input('Blink white [enter] ')
+set_effect(effect='blink', r=255, g=255, b=255)
+rospy.sleep(3)
+
+input('Blink fast violet [enter] ')
+set_effect(effect='blink_fast', r=220, g=20, b=250)
+rospy.sleep(3)
+
+input('Fade to blue [enter] ')
+set_effect(effect='fade', r=0, g=0, b=255)
+
+input('Wipe to yellow [enter] ')
+set_effect(effect='wipe', r=255, g=255, b=40)
+
+input('Flash red [enter] ')
+set_effect(effect='flash', r=255, g=0, b=0)
+rospy.sleep(1)
+
+input('Rainbow [enter] ')
+set_effect(effect='rainbow')
+rospy.sleep(4)
+
+input('Rainbow fill [enter] ')
+set_effect(effect='rainbow_fill')
+rospy.sleep(4)
+
+input('Turn off [enter] ')
+set_effect()
+
+print('== Testing low-level control ==')
+
+input('Fill orange [enter] ')
+set_leds(leds=[LEDState(index=i, r=245, g=155, b=0) for i in range(led_count)])
+
+input('Fill blue gradient [enter] ')
+set_leds(leds=[LEDState(index=i, r=0, g=20, b=int(255 * i / led_count)) for i in range(led_count)])
+
+input('Animate green dot [enter] ')
+set_effect()
+for i in range(led_count):
+    if i > 0:
+        set_leds(leds=[LEDState(index=i - 1, r=0, g=0, b=0)])
+    set_leds(leds=[LEDState(index=i, r=0, g=255, b=0)])
+    rospy.sleep(0.05)
+
+input('Turn off [enter] ')
+set_effect()

clover/src/autotest/util.py

@@ -0,0 +1,11 @@
+import functools
+
+# decorator to handle response and print error message
+def handle_response(fn):
+    @functools.wraps(fn)
+    def wrapper(*args, **kwargs):
+        res = fn(*args, **kwargs)
+        if not res.success:
+            print('\033[91mError:\033[0m {}'.format(res.message))
+        return res
+    return wrapper

clover/src/led.cpp

@@ -31,7 +31,6 @@ ros::Time start_time;
 double blink_rate, blink_fast_rate, flash_delay, fade_period, wipe_period, rainbow_period;
 double low_battery_threshold;
 std::vector<std::string> error_ignore;
-bool blink_state;
 led_msgs::SetLEDs set_leds;
 led_msgs::LEDStateArray state, start_state;
 ros::ServiceClient set_leds_srv;
@@ -87,9 +86,8 @@ void proceed(const ros::TimerEvent& event)
 	set_leds.request.leds.resize(led_count);
 
 	if (current_effect.effect == "blink" || current_effect.effect == "blink_fast") {
-		blink_state = !blink_state;
+		// enable on odd counter
-		// toggle all leds
+		if (counter % 2 != 0) {
-		if (blink_state) {
 			fill(current_effect.r, current_effect.g, current_effect.b);
 		} else {
 			fill(0, 0, 0);
@@ -222,6 +220,7 @@ bool setEffect(clover::SetLEDEffect::Request& req, clover::SetLEDEffect::Respons
 	counter = 0;
 	start_state = state;
 	start_time = ros::Time::now();
+	proceed({ .current_real = start_time });
 
 	return true;
 }

clover/src/optical_flow.cpp

@@ -22,11 +22,13 @@
 #include <tf2/utils.h>
 #include <tf2_ros/transform_listener.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <dynamic_reconfigure/server.h>
 #include <mavros_msgs/OpticalFlowRad.h>
 #include <sensor_msgs/Imu.h>
 #include <geometry_msgs/Vector3Stamped.h>
 #include <geometry_msgs/PointStamped.h>
 #include <geometry_msgs/TwistStamped.h>
+#include <clover/FlowConfig.h>
 
 using cv::Mat;
 
@@ -38,6 +40,7 @@ public:
 	{}
 
 private:
+	bool enabled_;
 	ros::Publisher flow_pub_, velo_pub_, shift_pub_;
 	ros::Time prev_stamp_;
 	std::string fcu_frame_id_, local_frame_id_;
@@ -54,6 +57,7 @@ private:
 	std::unique_ptr<tf2_ros::TransformListener> tf_listener_;
 	bool calc_flow_gyro_;
 	float flow_gyro_default_;
+	std::shared_ptr<dynamic_reconfigure::Server<clover::FlowConfig>> dyn_srv_;
 
 	void onInit()
 	{
@@ -83,6 +87,12 @@ private:
 
 		img_sub_ = it.subscribeCamera("image_raw", 1, &OpticalFlow::flow, this);
 
+		dyn_srv_ = std::make_shared<dynamic_reconfigure::Server<clover::FlowConfig>>(nh_priv);
+		dynamic_reconfigure::Server<clover::FlowConfig>::CallbackType cb;
+
+		cb = std::bind(&OpticalFlow::paramCallback, this, std::placeholders::_1, std::placeholders::_2);
+		dyn_srv_->setCallback(cb);
+
 		NODELET_INFO("Optical Flow initialized");
 	}
 
@@ -109,6 +119,8 @@ private:
 
 	void flow(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cinfo)
 	{
+		if (!enabled_) return;
+
 		parseCameraInfo(cinfo);
 
 		auto img = cv_bridge::toCvShare(msg, "mono8")->image;
@@ -264,6 +276,14 @@ publish_debug:
 
 		return flow;
 	}
+
+	void paramCallback(clover::FlowConfig &config, uint32_t level)
+	{
+		enabled_ = config.enabled;
+		if (!enabled_) {
+			prev_ = Mat(); // clear previous frame
+		}
+	}
 };
 
 PLUGINLIB_EXPORT_CLASS(OpticalFlow, nodelet::Nodelet)

clover/src/rc.cpp

@@ -91,7 +91,7 @@ private:
 	void fakeGCSThread()
 	{
 		// Awful workaround for fixing PX4 not sending STATUSTEXTs
-		// if there is no GCS hearbeats.
+		// if there is no GCS heartbeats.
 		// TODO: use timer
 		// TODO: remove, when PX4 get this fixed.
 		ros::Publisher mavlink_pub = nh.advertise<mavros_msgs::Mavlink>("mavlink/to", 1);

clover/src/selfcheck.py

@@ -30,6 +30,7 @@ from visualization_msgs.msg import MarkerArray as VisualizationMarkerArray
 import tf.transformations as t
 from aruco_pose.msg import MarkerArray
 from mavros import mavlink
+import locale
 
 
 # TODO: check attitude is present
@@ -45,6 +46,8 @@ rospy.init_node('selfcheck')
 
 os.environ['ROSCONSOLE_FORMAT']='[${severity}]: ${message}'
 
+# use user's locale to convert numbers, etc
+locale.setlocale(locale.LC_ALL, '')
 
 tf_buffer = tf2_ros.Buffer()
 tf_listener = tf2_ros.TransformListener(tf_buffer)
@@ -195,24 +198,27 @@ def check_fcu():
             failure('no connection to the FCU (check wiring)')
             return
 
+        clover_tag = re.compile(r'-cl[oe]ver\.\d+$')
+        clover_fw = False
+
         # 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.*)$')
+        for line in version_str.split('\n'):
-        is_clover_firmware = False
+            if line.startswith('FW version: '):
-        for ver_line in version_str.split('\n'):
+                info(line[len('FW version: '):])
-            match = r.search(ver_line)
+            elif line.startswith('FW git tag: '): # only Clover's firmware
-            if match is not None:
+                tag = line[len('FW git tag: '):]
-                field, version = match.groups()
+                clover_fw = clover_tag.search(tag)
-                info('firmware %s: %s' % (field, version))
+                info(tag)
-                if 'clover' in version or 'clever' in version:
+            elif line.startswith('HW arch: '):
-                    is_clover_firmware = True
+                info(line[len('HW arch: '):])
 
-        if not is_clover_firmware:
+        if not clover_fw:
-            failure('not running Clover PX4 firmware, https://clover.coex.tech/firmware')
+            info('not Clover PX4 firmware, check https://clover.coex.tech/firmware')
 
         est = get_param('SYS_MC_EST_GROUP')
         if est == 1:
@@ -330,7 +336,7 @@ def is_process_running(binary, exact=False, full=False):
         if exact:
             args.append('-x')  # match exactly with the command name
         if full:
-            args.append('-f')  # use full process name to match
+            args.append('-f')  # use full command line (including arguments) to match
         args.append(binary)
         subprocess.check_output(args)
         return True
@@ -528,6 +534,8 @@ def check_global_position():
         rospy.wait_for_message('mavros/global_position/global', NavSatFix, timeout=1)
     except rospy.ROSException:
         info('no global position')
+        if get_param('SYS_MC_EST_GROUP') == 2 and (get_param('EKF2_AID_MASK') & (1 << 0)):
+            failure('enabled GPS fusion may suppress vision position aiding')
 
 
 @check('Optical flow')
@@ -635,13 +643,16 @@ def check_cpu_usage():
             continue
         pid, cpu, cmd = process.split('\t')
 
-        if cmd.strip() not in WHITELIST and float(cpu) > 30:
+        if cmd.strip() not in WHITELIST and locale.atof(cpu) > 30:
             failure('high CPU usage (%s%%) detected: %s (PID %s)',
                     cpu.strip(), cmd.strip(), pid.strip())
 
 
 @check('clover.service')
 def check_clover_service():
+    if not os.path.exists('/etc/clover_version'):
+        return # Don't check not on Clover's image
+
     try:
         output = subprocess.check_output('systemctl show -p ActiveState --value clover.service'.split(),
                                          stderr=subprocess.STDOUT).decode()
@@ -697,6 +708,10 @@ def check_image():
 
 @check('Preflight status')
 def check_preflight_status():
+    if is_process_running('px4', exact=True):
+        info('can\'t check in SITL')
+        return
+
     # Make sure the console is available to us
     mavlink_exec('\n')
     cmdr_output = mavlink_exec('commander check')
@@ -718,6 +733,10 @@ def check_preflight_status():
 
 @check('Network')
 def check_network():
+    if not os.path.exists('/etc/clover_version'):
+        # TODO:
+        return # Don't check not on Clover's image
+
     ros_hostname = os.environ.get('ROS_HOSTNAME', '').strip()
 
     if not ros_hostname:
@@ -740,6 +759,14 @@ def check_network():
 
 @check('RPi health')
 def check_rpi_health():
+    try:
+        import shutil
+        total, used, free = shutil.disk_usage('/')
+        if free < 1024 * 1024 * 1024:
+            failure('disk space is less than 1 GB; consider removing logs (~/.ros/log/)')
+    except Exception as e:
+        info('could not check the disk free space: %s', str(e))
+
     # `vcgencmd get_throttled` output codes taken from
     # https://github.com/raspberrypi/documentation/blob/JamesH65-patch-vcgencmd-vcdbg-docs/raspbian/applications/vcgencmd.md#get_throttled
     # TODO: support more base platforms?

clover/src/simple_offboard.cpp

@@ -61,6 +61,7 @@ std::shared_ptr<tf2_ros::TransformBroadcaster> transform_broadcaster;
 std::shared_ptr<tf2_ros::StaticTransformBroadcaster> static_transform_broadcaster;
 
 // Parameters
+string mavros;
 string local_frame;
 string fcu_frame;
 ros::Duration transform_timeout;
@@ -149,6 +150,9 @@ void handleState(const mavros_msgs::State& s)
 inline void publishBodyFrame()
 {
 	if (body.child_frame_id.empty()) return;
+	if (!body.header.stamp.isZero() && body.header.stamp == local_position.header.stamp) {
+		return; // avoid TF_REPEATED_DATA warnings
+	}
 
 	tf::Quaternion q;
 	q.setRPY(0, 0, tf::getYaw(local_position.pose.orientation));
@@ -690,7 +694,7 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl
 		// }
 
 		if (sp_type == POSITION || sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == VELOCITY || sp_type == ATTITUDE) {
-			// destination point and/or yaw
+			// destination point and/or attitude
 			PoseStamped ps;
 			ps.header.frame_id = frame_id;
 			ps.header.stamp = stamp;
@@ -699,7 +703,12 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl
 			ps.pose.position.z = z;
 			ps.pose.orientation.w = 1.0; // Ensure quaternion is always valid
 
-			if (std::isnan(yaw)) {
+			if (sp_type == ATTITUDE) {
+				ps.pose.position.x = 0;
+				ps.pose.position.y = 0;
+				ps.pose.position.z = 0;
+				ps.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);
+			} else if (std::isnan(yaw)) {
 				setpoint_yaw_type = YAW_RATE;
 				setpoint_yaw_rate = yaw_rate;
 			} else if (std::isinf(yaw) && yaw > 0) {
@@ -861,8 +870,9 @@ int main(int argc, char **argv)
 	static_transform_broadcaster = std::make_shared<tf2_ros::StaticTransformBroadcaster>();
 
 	// Params
-	nh.param<string>("mavros/local_position/tf/frame_id", local_frame, "map");
+	nh_priv.param("mavros", mavros, string("mavros")); // for case of using multiple connections
-	nh.param<string>("mavros/local_position/tf/child_frame_id", fcu_frame, "base_link");
+	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("setpoint", setpoint.child_frame_id, string("setpoint"));
 	nh_priv.param("auto_release", auto_release, true);
@@ -894,25 +904,25 @@ int main(int argc, char **argv)
 	arming_timeout = ros::Duration(nh_priv.param("arming_timeout", 4.0));
 
 	// Service clients
-	arming = nh.serviceClient<mavros_msgs::CommandBool>("mavros/cmd/arming");
+	arming = nh.serviceClient<mavros_msgs::CommandBool>(mavros + "/cmd/arming");
-	set_mode = nh.serviceClient<mavros_msgs::SetMode>("mavros/set_mode");
+	set_mode = nh.serviceClient<mavros_msgs::SetMode>(mavros + "/set_mode");
 
 	// Telemetry subscribers
-	auto state_sub = nh.subscribe("mavros/state", 1, &handleState);
+	auto state_sub = nh.subscribe(mavros + "/state", 1, &handleState);
-	auto velocity_sub = nh.subscribe("mavros/local_position/velocity_body", 1, &handleMessage<TwistStamped, velocity>);
+	auto velocity_sub = nh.subscribe(mavros + "/local_position/velocity_body", 1, &handleMessage<TwistStamped, velocity>);
-	auto global_position_sub = nh.subscribe("mavros/global_position/global", 1, &handleMessage<NavSatFix, global_position>);
+	auto 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 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 statustext_sub = nh.subscribe(mavros + "/statustext/recv", 1, &handleMessage<mavros_msgs::StatusText, statustext>);
-	auto manual_control_sub = nh.subscribe("mavros/manual_control/control", 1, &handleMessage<mavros_msgs::ManualControl, manual_control>);
+	auto manual_control_sub = nh.subscribe(mavros + "/manual_control/control", 1, &handleMessage<mavros_msgs::ManualControl, manual_control>);
-	auto local_position_sub = nh.subscribe("mavros/local_position/pose", 1, &handleLocalPosition);
+	auto local_position_sub = nh.subscribe(mavros + "/local_position/pose", 1, &handleLocalPosition);
 
 	// Setpoint publishers
-	position_pub = nh.advertise<PoseStamped>("mavros/setpoint_position/local", 1);
+	position_pub = nh.advertise<PoseStamped>(mavros + "/setpoint_position/local", 1);
-	position_raw_pub = nh.advertise<PositionTarget>("mavros/setpoint_raw/local", 1);
+	position_raw_pub = nh.advertise<PositionTarget>(mavros + "/setpoint_raw/local", 1);
-	attitude_pub = nh.advertise<PoseStamped>("mavros/setpoint_attitude/attitude", 1);
+	attitude_pub = nh.advertise<PoseStamped>(mavros + "/setpoint_attitude/attitude", 1);
-	attitude_raw_pub = nh.advertise<AttitudeTarget>("mavros/setpoint_raw/attitude", 1);
+	attitude_raw_pub = nh.advertise<AttitudeTarget>(mavros + "/setpoint_raw/attitude", 1);
-	rates_pub = nh.advertise<TwistStamped>("mavros/setpoint_attitude/cmd_vel", 1);
+	rates_pub = nh.advertise<TwistStamped>(mavros + "/setpoint_attitude/cmd_vel", 1);
-	thrust_pub = nh.advertise<Thrust>("mavros/setpoint_attitude/thrust", 1);
+	thrust_pub = nh.advertise<Thrust>(mavros + "/setpoint_attitude/thrust", 1);
 
 	 // Service servers
 	auto gt_serv = nh.advertiseService("get_telemetry", &getTelemetry);

clover/src/vpe_publisher.cpp

@@ -33,14 +33,14 @@ ros::Subscriber local_position_sub;
 ros::Timer zero_timer;
 PoseStamped vpe, pose;
 ros::Time got_local_pos(0);
-ros::Duration publish_zero_timout, publish_zero_duration, offset_timeout;
+ros::Duration publish_zero_timeout, publish_zero_duration, offset_timeout;
 TransformStamped offset;
 
 void publishZero(const ros::TimerEvent& e)
 {
-	if (e.current_real - vpe.header.stamp < publish_zero_timout) return; // have vpe
+	if (!vpe.header.stamp.isZero() && e.current_real - vpe.header.stamp < publish_zero_timeout) return; // have vpe
 
-	if (e.current_real - pose.header.stamp < publish_zero_timout) { // have local position
+	if (!pose.header.stamp.isZero() && e.current_real - pose.header.stamp < publish_zero_timeout) { // have local position
 		if (got_local_pos.isZero()) {
 			ROS_INFO("got local position");
 			got_local_pos = e.current_real;
@@ -124,8 +124,8 @@ int main(int argc, char **argv) {
 
 	nh_priv.param<string>("frame_id", frame_id, "");
 	nh_priv.param<string>("offset_frame_id", offset_frame_id, "");
-	nh_priv.param<string>("mavros/local_position/frame_id", local_frame_id, "map");
+	nh.param<string>("mavros/local_position/frame_id", local_frame_id, "map");
-	nh_priv.param<string>("mavros/local_position/tf/child_frame_id", child_frame_id, "base_link");
+	nh.param<string>("mavros/local_position/tf/child_frame_id", child_frame_id, "base_link");
 	offset_timeout = ros::Duration(nh_priv.param("offset_timeout", 3.0));
 
 	if (!frame_id.empty()) {
@@ -141,11 +141,11 @@ int main(int argc, char **argv) {
 	vpe_pub = nh_priv.advertise<PoseStamped>("vpe", 1);
 	//vpe_cov_pub = nh_priv_.advertise<PoseStamped>("pose_cov_pub", 1);
 
-	if (nh_priv.param("publish_zero", false)) {
+	if (nh_priv.param("force_init", false) || nh_priv.param("publish_zero", false)) { // publish_zero is old name
 		// publish zero to initialize the local position
 		zero_timer = nh.createTimer(ros::Duration(0.1), &publishZero);
-		publish_zero_timout = ros::Duration(nh_priv.param("publish_zero_timout", 5.0));
+		publish_zero_timeout = ros::Duration(nh_priv.param("force_init_timeout", 5.0));
-		publish_zero_duration = ros::Duration(nh_priv.param("publish_zero_duration", 5.0));
+		publish_zero_duration = ros::Duration(nh_priv.param("force_init_duration", 5.0));
 		local_position_sub = nh.subscribe("mavros/local_position/pose", 1, &localPositionCallback);
 	}
 

clover/test/basic.py

@@ -33,3 +33,29 @@ def test_web_video_server(node):
         # Python 3
         import urllib.request as urllib
     urllib.urlopen("http://localhost:8080").read()
+
+def test_blocks(node):
+    rospy.wait_for_service('clover_blocks/run', timeout=5)
+    rospy.wait_for_service('clover_blocks/stop', timeout=5)
+    rospy.wait_for_service('clover_blocks/load', timeout=5)
+    rospy.wait_for_service('clover_blocks/store', timeout=5)
+
+    from std_msgs.msg import String
+    from clover_blocks.srv import Run
+
+    def wait_print():
+        try:
+            wait_print.result = rospy.wait_for_message('clover_blocks/print', String, timeout=5).data
+        except Exception as e:
+            wait_print.result = str(e)
+
+    import threading
+    t = threading.Thread(target=wait_print)
+    t.start()
+    rospy.sleep(0.1)
+
+    run = rospy.ServiceProxy('clover_blocks/run', Run)
+    assert run(code='print("test")').success == True
+
+    t.join()
+    assert wait_print.result == 'test'

clover/test/basic.test

@@ -23,10 +23,7 @@
 
     <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="clover" type="simple_offboard" required="true" output="screen">
+    <node name="simple_offboard" pkg="clover" 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"/>
 
@@ -38,6 +35,8 @@
         <rosparam param="notify">startup: { r: 255, g: 255, b: 255 }</rosparam>
     </node>
 
+    <node name="clover_blocks" pkg="clover_blocks" type="clover_blocks" output="screen" required="true"/>
+
     <param name="test_module" value="$(find clover)/test/basic.py"/>
     <test test-name="basic_test" pkg="ros_pytest" type="ros_pytest_runner"/>
 </launch>

clover_blocks/README.md

@@ -19,7 +19,7 @@ The frontend files are located in [`www`](./www/) subdirectory. The frontend app
 ### Services
 
 * `~run` ([*clover_blocks/Run*](srv/Run.srv)) – run Blockly-generated program (in Python).
-* `~stop` ([*std_srvs/Trigger*](http://docs.ros.org/melodic/api/std_srvs/html/srv/Trigger.html)) – terminate the running program.
+* `~stop` ([*std_srvs/Trigger*](http://docs.ros.org/noetic/api/std_srvs/html/srv/Trigger.html)) – terminate the running program.
 * `~store` ([*clover_blocks/load*](srv/Store.srv)) – store a user program (to `<package_path>/programs` by default).
 * `~load` ([*clover_blocks/load*](srv/Load.srv)) – load all the stored programs.
 
@@ -45,11 +45,11 @@ http://<hostname>/clover_blocks/?navigate_tolerance=0.5&sleep_time=0.1
 
 #### Published
 
-* `~running` ([*std_msgs/Bool*](http://docs.ros.org/melodic/api/std_msgs/html/msg/Bool.html)) – indicates if the program is currently running.
+* `~running` ([*std_msgs/Bool*](http://docs.ros.org/noetic/api/std_msgs/html/msg/Bool.html)) – indicates if the program is currently running.
-* `~block` ([*std_msgs/String*](http://docs.ros.org/melodic/api/std_msgs/html/msg/String.html)) – current executing block (maximum topic rate is limited).
+* `~block` ([*std_msgs/String*](http://docs.ros.org/noetic/api/std_msgs/html/msg/String.html)) – current executing block (maximum topic rate is limited).
-* `~error` ([*std_msgs/String*](http://docs.ros.org/melodic/api/std_msgs/html/msg/String.html)) – user program errors and exceptions.
+* `~error` ([*std_msgs/String*](http://docs.ros.org/noetic/api/std_msgs/html/msg/String.html)) – user program errors and exceptions.
 * `~prompt` ([*clover_blocks/Prompt*](msg/Prompt.msg)) – user input request (includes random request ID string).
 
 This topic is published from the frontend side:
 
-* `~prompt/<request_id>` ([*std_msgs/String*](http://docs.ros.org/melodic/api/std_msgs/html/msg/String.html)) – user input response.
+* `~prompt/<request_id>` ([*std_msgs/String*](http://docs.ros.org/noetic/api/std_msgs/html/msg/String.html)) – user input response.

clover_description/urdf/clover/clover4_physics.xacro

@@ -35,7 +35,7 @@
   <xacro:property name="sqrt2" value="1.4142135623730951" />
   <xacro:property name="rotor_drag_coefficient" value="1.75e-04" />
   <xacro:property name="rolling_moment_coefficient" value="0.000001" />
-  <xacro:property name="color" value="$(arg visual_material)" />
+  <xacro:property name="color" value="DarkGrey" />
 
   <!-- Property Blocks -->
   <!-- Clover body inertia -->

clover_description/urdf/leds/led_strip.xacro

@@ -64,6 +64,12 @@
         <!-- <gazebo>
             <static>true</static>
         </gazebo> -->
+        <gazebo>
+            <plugin name="${name}_ros_controller" filename="libsim_leds_controller.so">
+                <robotNamespace></robotNamespace>
+                <ledCount>${led_count}</ledCount>
+            </plugin>
+        </gazebo>
     </xacro:macro>
 
 </robot>

clover_simulation/CMakeLists.txt

@@ -37,6 +37,14 @@ target_compile_options(sim_leds PRIVATE -std=c++11)
 
 add_dependencies(sim_leds ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
+add_library(sim_leds_controller src/sim_leds_controller.cpp)
+
+target_include_directories(sim_leds_controller PRIVATE ${catkin_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS})
+target_link_libraries(sim_leds_controller PRIVATE ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES})
+target_compile_options(sim_leds_controller PRIVATE -std=c++11)
+
+add_dependencies(sim_leds_controller ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
 # Gazebo throttling camera plugin
 # for some reason, CMake does not support per-target link directories, and Gazebo does not put
 # CameraPlugin into ${GAZEBO_LIBRARIES}

clover_simulation/airframes/4500_clover

@@ -7,30 +7,31 @@
 
 . ${R}etc/init.d-posix/airframes/10016_iris # base on iris
 
-param set ATT_W_EXT_HDG 0.5
+param set-default ATT_W_EXT_HDG 0.5
-param set ATT_EXT_HDG_M 1
+param set-default ATT_EXT_HDG_M 1 # 0 = none, 1 = vision, 2 = mocap
 
-param set COM_DISARM_LAND 1.0
+param set-default COM_DISARM_LAND 1.0
+param set-default COM_RCL_EXCEPT 4 # enable offboard flights without rc
 
-param set LPE_FLW_SCALE 1.0
+param set-default LPE_FLW_SCALE 1.0
-param set LPE_FLW_R 0.2
+param set-default LPE_FLW_R 0.2
-param set LPE_FLW_RR 0.0
+param set-default LPE_FLW_RR 0.0
-param set LPE_FLW_QMIN 10
+param set-default LPE_FLW_QMIN 10
-param set LPE_VIS_DELAY 0.0
+param set-default LPE_VIS_DELAY 0.0
-param set LPE_VIS_Z 0.1
+param set-default LPE_VIS_Z 0.1
-param set LPE_FUSION 86
+param set-default LPE_FUSION 86 # flow + vis + land detector + gyro comp
 
-param set SENS_FLOW_ROT 0
+param set-default SENS_FLOW_ROT 0
-param set SENS_FLOW_MINHGT 0.01
+param set-default SENS_FLOW_MINHGT 0.0
-param set SENS_FLOW_MAXHGT 4.0
+param set-default SENS_FLOW_MAXHGT 4.0
-param set SENS_FLOW_MAXR 10.0
+param set-default SENS_FLOW_MAXR 10.0
 
-param set EKF2_AID_MASK 27 # gps + flow + vis pos + vis yaw
+param set-default EKF2_AID_MASK 26 # flow + vis pos + vis yaw
-param set EKF2_OF_DELAY 0
+param set-default EKF2_OF_DELAY 0
-param set EKF2_OF_QMIN 10
+param set-default EKF2_OF_QMIN 10
-param set EKF2_OF_N_MIN 0.05
+param set-default EKF2_OF_N_MIN 0.05
-param set EKF2_OF_N_MAX 0.2
+param set-default EKF2_OF_N_MAX 0.2
-param set EKF2_HGT_MODE 2
+param set-default EKF2_HGT_MODE 2 # 0 = baro, 1 = gps, 2 = range, 3 = vision
-param set EKF2_EVA_NOISE 0.1
+param set-default EKF2_EVA_NOISE 0.1
-param set EKF2_EVP_NOISE 0.1
+param set-default EKF2_EVP_NOISE 0.1
-param set EKF2_EV_DELAY 0
+param set-default EKF2_EV_DELAY 0

clover_simulation/launch/simulator.launch

@@ -21,7 +21,7 @@
     </include>
 
     <!-- PX4 instance -->
-    <node name="sitl_$(arg mav_id)" pkg="px4" type="px4" output="screen" args="$(find px4)/ROMFS/px4fmu_common -s etc/init.d-posix/rcS -i $(arg mav_id)" unless="$(eval type == 'none')">
+    <node name="sitl_$(arg mav_id)" pkg="px4" type="px4" output="screen" required="true" args="$(find px4)/ROMFS/px4fmu_common -s etc/init.d-posix/rcS -i $(arg mav_id)" unless="$(eval type == 'none')">
         <env name="PX4_SIM_MODEL" value="$(arg vehicle)"/>
         <env name="PX4_ESTIMATOR" value="$(arg est)"/>
     </node>
@@ -36,7 +36,7 @@
         <arg name="use_clover_physics" value="$(arg use_clover_physics)"/>
     </include>
 
-    <node name="jmavsim" pkg="px4" type="jmavsim_run.sh" output="screen" if="$(eval type == 'jmavsim')">
+    <node name="jmavsim" pkg="px4" required="true" type="jmavsim_run.sh" output="screen" if="$(eval type == 'jmavsim')">
         <env name="HEADLESS" value="1" if="$(eval not gui)"/>
     </node>
 

clover_simulation/src/sim_leds.cpp

@@ -49,14 +49,9 @@ private:
 
 	std::unique_ptr<ros::NodeHandle> nh;
 
-	ros::ServiceServer setLedsSrv;
-	// Note: LED state should only be published by the /gazebo node
-	led_msgs::LEDStateArray ledState;
-	ros::Publisher statePublisher;
 	// LED state will be read from the topic to avoid creating more services
 	ros::Subscriber stateSubscriber;
 
-	bool setLeds(led_msgs::SetLEDs::Request& req, led_msgs::SetLEDs::Response& resp);
 	void handleLedsMsg(const led_msgs::LEDStateArrayConstPtr& leds);
 
 public:
@@ -73,16 +68,8 @@ public:
 		ROS_INFO_NAMED(("LedController_" + robotNamespace).c_str(), "LedController has started (as %s)", role == Role::Client ? "client" : "server");
 
 		nh.reset(new ros::NodeHandle(robotNamespace));
-		if (role == Role::Server)
+
-		{
-			setLedsSrv = nh->advertiseService("led/set_leds", &LedController::setLeds, this);
-			statePublisher = nh->advertise<led_msgs::LEDStateArray>("led/state", 1, true);
-		}
-		else
-		{
-			// LED state should be published to the "led/state" topic, so we grab our data there
 		stateSubscriber = nh->subscribe<led_msgs::LEDStateArray>("led/state", 1, &LedController::handleLedsMsg, this);
-		}
 	};
 
 	~LedController()
@@ -96,13 +83,9 @@ public:
 		std::lock_guard<std::mutex> lock(registryMutex);
 		if (totalLeds > 0) {
 			registeredLeds.resize(totalLeds);
-			ledState.leds.resize(totalLeds);
 		}
 		ROS_DEBUG_NAMED(("LedController_" + robotNamespace).c_str(), "Registering LED visual plugin to %s (LED id=%d)", (role == Role::Client) ? "client" : "server", ledIdx);
 		registeredLeds[ledIdx] = plugin;
-		ledState.leds[ledIdx].index = ledIdx;
-		if (role == Role::Server)
-			statePublisher.publish(ledState);
 	}
 
 	void unregisterPlugin(sim_led::LedVisualPlugin* plugin)
@@ -157,7 +140,8 @@ public:
 		{
 			auto indexStr = parentName.substr(lastDashPos + 1);
 			try {
-				myIndex = std::stoi(indexStr);
+				if (indexStr == "visual") myIndex = 0; // the first visual doesn't have index
+				else myIndex = std::stoi(indexStr);
 			} catch(const std::exception &e) {
 				gzwarn << "Failed to convert " << indexStr << " to integer: " << e.what() <<  ", assuming 0\n";
 				myIndex = 0;
@@ -195,26 +179,6 @@ public:
 };
 }
 
-// FIXME: These two functions basically do the same thing, maybe they can be merged?
-bool led_controller::LedController::setLeds(led_msgs::SetLEDs::Request &req, led_msgs::SetLEDs::Response &resp)
-{
-	std::lock_guard<std::mutex> lock(registryMutex);
-	for(const auto& led : req.leds)
-	{
-		if (led.index < registeredLeds.size()) {
-			auto color = GazeboColor(led.r / 255.0f, led.g / 255.0f, led.b / 255.0f);
-			auto ledPlugin = registeredLeds[led.index];
-			if (ledPlugin) ledPlugin->SetColor(color);
-			ledState.leds[led.index].r = led.r;
-			ledState.leds[led.index].g = led.g;
-			ledState.leds[led.index].b = led.b;
-		}
-	}
-	statePublisher.publish(ledState);
-	resp.success = true;
-	return true;
-}
-
 void led_controller::LedController::handleLedsMsg(const led_msgs::LEDStateArrayConstPtr& leds)
 {
 	std::lock_guard<std::mutex> lock(registryMutex);

clover_simulation/src/sim_leds_controller.cpp

@@ -0,0 +1,71 @@
+#include <led_msgs/SetLEDs.h>
+#include <led_msgs/LEDStateArray.h>
+
+#include <ros/ros.h>
+
+#include <gazebo/gazebo.hh>
+#include <gazebo/physics/physics.hh>
+#include <gazebo/common/common.hh>
+
+class LedControllerPlugin : public gazebo::ModelPlugin {
+private:
+	std::unique_ptr<ros::NodeHandle> nh;
+	std::string ns;
+	ros::ServiceServer setLedsSrv;
+	led_msgs::LEDStateArray ledState;
+	ros::Publisher statePublisher;
+	std::mutex handleMutex;
+
+public:
+	bool setLeds(led_msgs::SetLEDs::Request &req, led_msgs::SetLEDs::Response &resp)
+	{
+		std::lock_guard<std::mutex> lock(handleMutex);
+		for(const auto& led : req.leds)
+		{
+			if (led.index < ledState.leds.size()) {
+				ledState.leds[led.index].r = led.r;
+				ledState.leds[led.index].g = led.g;
+				ledState.leds[led.index].b = led.b;
+			}
+		}
+		statePublisher.publish(ledState);
+		resp.success = true;
+		return true;
+	}
+
+	virtual void Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) override
+	{
+		ROS_INFO("Initialize LED Controller");
+
+		// We need "libgazebo_ros_api.so" to be loaded
+		if (!ros::isInitialized())
+		{
+			ROS_FATAL_NAMED("LedController", "Tried to load ROS plugin when ROS Gazebo API is not loaded. Please use gazebo_ros node to"
+			                                 "launch Gazebo.");
+		}
+
+		ns = "";
+
+		if (sdf->HasElement("robotNamespace")) {
+			ns = sdf->Get<std::string>("robotNamespace");
+		}
+		if (!sdf->HasElement("ledCount")) {
+			gzerr << "ledCount is not set, but is required for the plugin to function correctly\n";
+			return;
+		}
+		int totalLeds = sdf->Get<int>("ledCount");
+		ledState.leds.resize(totalLeds);
+		for (int i = 0; i < totalLeds; i++) {
+			ledState.leds[i].index = i;
+		}
+
+		nh.reset(new ros::NodeHandle(ns));
+
+		setLedsSrv = nh->advertiseService("led/set_leds", &LedControllerPlugin::setLeds, this);
+		statePublisher = nh->advertise<led_msgs::LEDStateArray>("led/state", 1, true);
+
+		statePublisher.publish(ledState);
+	}
+};
+
+GZ_REGISTER_MODEL_PLUGIN(LedControllerPlugin);

docs/Dockerfile

@@ -0,0 +1,36 @@
+# Build: sudo docker build -t clover-docs:latest .
+
+# Run: docker run -it --rm -v ~/clover:/clover clover-docs:latest
+
+# Save image: sudo docker save clover-docs:latest | gzip > clover-docs.tar.gz
+
+FROM ubuntu:22.04
+
+ENV DEBIAN_FRONTEND=noninteractive
+
+RUN apt-get update && apt-get install -y curl git
+
+RUN curl https://raw.githubusercontent.com/creationix/nvm/master/install.sh | bash
+RUN . $HOME/.nvm/nvm.sh && nvm install 10.15 && nvm use 10.15 && npm --version
+
+RUN echo ttf-mscorefonts-installer msttcorefonts/accepted-mscorefonts-eula select true | debconf-set-selections \
+  && apt-get update \
+  && apt-get install -y calibre msttcorefonts ghostscript
+
+RUN . $HOME/.nvm/nvm.sh && curl https://raw.githubusercontent.com/CopterExpress/clover/master/builder/assets/install_gitbook.sh | bash
+
+# RUN . $HOME/.nvm/nvm.sh && npm install markdownlint-cli@0.28.1 -g && PUPPETEER_SKIP_DOWNLOAD=1 npm install svgexport -g
+RUN . $HOME/.nvm/nvm.sh && npm install markdownlint-cli@0.28.1 -g && npm install svgexport -g --unsafe-perm
+
+RUN . $HOME/.nvm/nvm.sh && node -v && gitbook -V && markdownlint -V
+
+RUN curl https://raw.githubusercontent.com/CopterExpress/clover/master/book.json > book.json
+
+RUN . $HOME/.nvm/nvm.sh && gitbook install
+
+CMD . $HOME/.nvm/nvm.sh && export QTWEBENGINE_DISABLE_SANDBOX=1 && cp -r node_modules /clover && cd /clover && gitbook build \
+  && gitbook pdf ./ _book/clover.pdf && \
+  gs -sDEVICE=pdfwrite -dCompatibilityLevel=1.4 -dPDFSETTINGS=/default -dNOPAUSE -dQUIET -dBATCH -dDetectDuplicateImages -dCompressFonts=true -r150 -sOutputFile=_book/clover_ru_compressed.pdf _book/clover_ru.pdf && \
+  gs -sDEVICE=pdfwrite -dCompatibilityLevel=1.4 -dPDFSETTINGS=/default -dNOPAUSE -dQUIET -dBATCH -dDetectDuplicateImages -dCompressFonts=true -r150 -sOutputFile=_book/clover_en_compressed.pdf _book/clover_en.pdf && \
+  rm _book/clover_ru.pdf && mv _book/clover_ru_compressed.pdf _book/clover_ru.pdf && \
+  rm _book/clover_en.pdf && mv _book/clover_en_compressed.pdf _book/clover_en.pdf

docs/assets/copter_cat/logo.svg

@@ -0,0 +1,25 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE svg PUBLIC "-//W3C//DTD SVG 1.1//EN" "http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd">
+<!-- Creator: CorelDRAW 2019 (64-Bit) -->
+<svg xmlns="http://www.w3.org/2000/svg" xml:space="preserve" width="255.858mm" height="78.1171mm" version="1.1" style="shape-rendering:geometricPrecision; text-rendering:geometricPrecision; image-rendering:optimizeQuality; fill-rule:evenodd; clip-rule:evenodd"
+viewBox="0 0 5794.02 1769"
+ xmlns:xlink="http://www.w3.org/1999/xlink"
+ xmlns:xodm="http://www.corel.com/coreldraw/odm/2003">
+ <defs>
+  <style type="text/css">
+   <![CDATA[
+    .fil1 {fill:#1F1B20}
+    .fil0 {fill:#1F1B20;fill-rule:nonzero}
+   ]]>
+  </style>
+ </defs>
+ <g id="Слой_x0020_1">
+  <metadata id="CorelCorpID_0Corel-Layer"/>
+  <polygon class="fil0" points="3293.37,1768.99 2509.06,1768.97 2507.86,1661.12 3292.17,1660.65 "/>
+  <polygon class="fil0" points="3133.99,231.57 2664.24,231.57 2556.99,123.72 3237.05,123.23 "/>
+  <path class="fil0" d="M3647.51 123.72l1925.38 0 0 381.38 -107.85 -8.69 0 -264.84 -1809.74 0 -7.79 -107.85zm1925.38 676.77l0 968.51 -1653.42 0 -1.5 -108.34 1547.07 0.49 0 -850.17 107.85 -10.48z"/>
+  <path class="fil0" d="M2142.36 123.72l-1925.38 0 0 381.38 107.85 -8.69 0 -264.84 1809.74 0 7.79 -107.85zm-1925.38 676.77l0 968.51 1654.08 0 1.5 -108.34 -1547.73 0.49 0 -850.17 -107.85 -10.48z"/>
+  <path class="fil1" d="M2887.45 1229.18c-100.66,0 -189.34,-87.18 -244.46,-111.14 -55.12,-23.96 -215.7,-63.51 -215.7,-63.51 0,0 -88.67,-131.81 -110.24,-196.52 -21.57,-64.71 -38.35,-225.29 -38.35,-225.29 0,0 -28.76,-59.92 -38.35,-86.28 -9.59,-26.36 -19.17,-79.09 -19.17,-79.09l67.11 -59.92 -71.9 -21.57c0,0 -9.59,-131.82 -9.59,-184.54 0,-52.73 28.76,-201.32 28.76,-201.32 0,0 143.8,76.69 242.06,148.59 98.26,71.9 201.32,220.49 201.32,220.49 0,0 97.96,-32.96 220.19,-32.96 122.23,0 212.4,32.96 212.4,32.96 0,0 103.06,-148.59 201.32,-220.49 98.26,-71.9 242.06,-148.59 242.06,-148.59 29.86,154.29 35.16,239.28 25.77,366.69 -7.33,99.38 -20.22,164.6 -68.91,266.02 0,0 -16.78,146.2 -38.35,225.29 -21.57,79.09 -110.24,196.52 -110.24,196.52 0,0 -165.37,25.17 -221.69,53.92 -56.32,28.76 -153.38,120.73 -254.04,120.73zm-260.11 -471.35c0,0 -32.95,31.4 -32.95,71.54 0,40.15 29.36,76.45 29.36,76.45 0,0 30.97,-38.24 30.97,-73.59 0,-35.35 -27.38,-74.4 -27.38,-74.4zm-164.17 -37.15c0,0 84.84,-18.88 192.09,33.85 107.25,52.73 163.22,135.12 163.22,135.12 0,0 -122.28,71.58 -245.11,36.23 -122.83,-35.35 -110.2,-205.2 -110.2,-205.2zm687.07 37.15c0,0 32.95,31.4 32.95,71.54 0,40.15 -29.36,76.45 -29.36,76.45 0,0 -30.97,-38.24 -30.97,-73.59 0,-35.35 27.38,-74.4 27.38,-74.4zm164.17 -37.15c0,0 -84.84,-18.88 -192.09,33.85 -107.25,52.73 -163.22,135.12 -163.22,135.12 0,0 122.28,71.58 245.11,36.23 122.83,-35.35 110.2,-205.2 110.2,-205.2z"/>
+  <path class="fil1" d="M2895.05 1398.42c0,0 -216.07,-1.19 -337.7,-47.93 -121.63,-46.74 -147.39,-68.91 -147.39,-68.91l-149.64 380.47 134.66 0 1.2 105.45 -411.02 0 0 -105.45 154.43 0 105.6 -464.95c0,0 -226.48,-22.77 -424.21,-81.49 -197.72,-58.72 -354.7,-127.02 -492.51,-133.01 -137.81,-5.99 -114.44,-2.99 -198.32,-7.79 -83.88,-4.79 -142.6,-47.92 -142.6,-82.68 0,-34.75 1.2,-59.32 1.2,-97.66 0,-38.35 32.36,-46.74 32.36,-46.74l-1.2 -29.96 -22.77 0c0,-8.09 12.58,-46.74 -53.93,-46.74 -66.51,0 -175.67,58.19 -233.67,77.89 -58,19.71 -552.43,-15.58 -644.7,-41.94 -92.27,-26.36 -64.71,-77.89 -45.54,-94.67 19.17,-16.78 539.25,-63.51 645.9,-56.32 106.65,7.19 329.54,44.34 329.54,44.34l0 -26.36 25.76 0 0 -25.76 12.58 0c0,0 2.39,-94.67 2.39,-134.21 0,-39.54 37.23,-60.33 65.31,-59.9 28.08,0.43 63.2,25.75 63.2,61.1 0,35.35 2.7,128.82 2.7,128.82l7.49 -0.15 0 25.02 32.95 0 0 27.56c0,0 253.15,-39.85 361,-45.84 107.85,-5.99 594.37,37.15 617.14,58.72 22.77,21.57 35.95,77.89 -39.55,100.66 -75.5,22.77 -581.19,57.52 -639.91,46.74 -58.72,-10.79 -173.73,-77.7 -243.26,-76.69 -69.53,1.01 -56.62,35.95 -56.62,45.54l-22.78 -0.9 -0.16 18.87c0,0 83.07,-1.13 121.42,16.84 38.35,17.97 115.72,50.26 168.44,62.24 52.72,11.98 568.01,-14.38 636.31,-28.76 68.3,-14.38 115.48,-40.16 115.48,-40.16 0,0 16.34,115.66 47.5,189.95 31.16,74.3 86.28,147.39 109.05,164.17 22.77,16.78 156.98,45.54 196.52,59.92 39.55,14.38 110.24,58.72 149.79,86.28 39.55,27.56 112.97,44.34 172.89,44.34 59.92,0 126.57,-16.78 166.12,-44.34 39.55,-27.56 110.24,-71.9 149.79,-86.28 39.55,-14.38 173.76,-43.14 196.52,-59.92 22.77,-16.78 77.89,-89.88 109.05,-164.17 31.16,-74.3 47.5,-189.95 47.5,-189.95 0,0 47.17,25.78 115.48,40.16 68.3,14.38 583.59,40.74 636.31,28.76 52.72,-11.98 130.1,-44.27 168.44,-62.24 38.35,-17.97 121.42,-16.84 121.42,-16.84l-0.16 -18.87 -22.78 0.9c0,-9.59 12.91,-44.53 -56.62,-45.54 -69.53,-1.01 -184.54,65.91 -243.26,76.69 -58.72,10.79 -564.41,-23.97 -639.91,-46.74 -75.5,-22.77 -62.31,-79.09 -39.55,-100.66 22.77,-21.57 509.29,-64.71 617.14,-58.72 107.85,5.99 361,45.84 361,45.84l0 -27.56 32.95 0 0 -25.02 7.49 0.15c0,0 2.7,-93.47 2.7,-128.82 0,-35.35 35.12,-60.67 63.2,-61.1 28.08,-0.43 65.31,20.36 65.31,59.9 0,39.54 2.39,134.21 2.39,134.21l12.58 0 0 25.76 25.76 0 0 26.36c0,0 222.89,-37.15 329.54,-44.34 106.65,-7.19 626.73,39.54 645.9,56.32 19.17,16.78 46.74,68.3 -45.54,94.67 -92.27,26.36 -586.7,61.65 -644.7,41.94 -58,-19.71 -167.17,-77.89 -233.67,-77.89 -66.51,0 -53.93,38.65 -53.93,46.74l-22.77 0 -1.2 29.96c0,0 32.36,8.4 32.36,46.74 0,38.35 1.2,62.91 1.2,97.66 0,34.75 -58.72,77.88 -142.6,82.68 -83.88,4.79 -60.52,1.79 -198.32,7.79 -137.81,5.99 -294.79,74.3 -492.51,133.01 -197.72,58.72 -424.21,81.49 -424.21,81.49l105.6 464.95 154.43 0 0 105.45 -411.02 0 1.2 -105.45 134.66 0 -149.64 -380.47c0,0 -25.76,22.17 -147.39,68.91 -121.63,46.74 -341.62,47.93 -341.62,47.93z"/>
+ </g>
+</svg>

docs/assets/copterhack2023.svg

@@ -0,0 +1,61 @@
+<?xml version="1.0" encoding="utf-8"?>
+<!-- Generator: Adobe Illustrator 23.0.1, SVG Export Plug-In . SVG Version: 6.00 Build 0)  -->
+<svg version="1.1" id="Слой_1" xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" x="0px" y="0px"
+	 viewBox="0 0 850.39 283.46" style="enable-background:new 0 0 850.39 283.46;" xml:space="preserve">
+<style type="text/css">
+	.st0{fill:#333333;}
+	.st1{fill:#FFFFFF;}
+	.st2{fill:#CCCCCC;}
+	.st3{fill:#1E1183;}
+	.st4{fill:#F71523;}
+	.st5{fill:url(#SVGID_1_);}
+	.st6{fill:url(#SVGID_2_);}
+	.st7{fill:url(#SVGID_3_);}
+	.st8{opacity:0.05;fill:#FFFFFF;}
+	.st9{opacity:0.05;fill:#CCCCCC;}
+	.st10{opacity:0.05;fill:url(#SVGID_4_);}
+	.st11{opacity:0.05;fill:url(#SVGID_5_);}
+	.st12{opacity:0.05;fill:url(#SVGID_6_);}
+	.st13{opacity:0.05;fill:url(#SVGID_7_);}
+	.st14{opacity:0.05;fill:url(#SVGID_8_);}
+	.st15{opacity:0.05;fill:url(#SVGID_9_);}
+	.st16{opacity:0.05;fill:url(#SVGID_10_);}
+	.st17{opacity:0.05;fill:url(#SVGID_11_);}
+	.st18{opacity:0.05;fill:url(#SVGID_12_);}
+</style>
+<g>
+	<polygon class="st3" points="559.36,2.05 433.46,2.05 433.46,36.37 479.25,36.37 479.25,128.02 513.57,128.02 513.57,36.37 
+		559.36,36.37 	"/>
+	<polygon class="st3" points="702.71,36.37 702.71,2.05 576.81,2.05 576.81,2.12 576.75,2.12 576.75,128.02 576.81,128.02 
+		611.07,128.02 702.71,128.02 702.71,93.7 611.07,93.7 611.07,82.23 668.45,82.23 668.45,47.91 611.07,47.91 611.07,36.37 	"/>
+	<polygon class="st3" points="559.45,179.72 559.38,155.4 535.19,155.45 489.34,201.3 467.68,201.3 467.68,155.45 433.37,155.45 
+		433.37,281.35 467.68,281.35 467.68,235.62 489.46,235.62 535.19,281.35 559.38,281.41 559.45,257.09 520.77,218.4 	"/>
+	<path class="st3" d="M67.26,36.87h63.01V2.05H67.26c-34.8,0-63.01,28.21-63.01,63.01s28.21,63.01,63.01,63.01h63.01V93.25H67.26
+		c-15.57,0-28.19-12.62-28.19-28.19C39.07,49.49,51.69,36.87,67.26,36.87z"/>
+	<path class="st3" d="M238.36,218.4v63.01h34.82V218.4c0-34.8-28.21-63.01-63.01-63.01s-63.01,28.21-63.01,63.01v63.01h34.82V218.4
+		c0-15.57,12.62-28.19,28.19-28.19C225.74,190.21,238.36,202.83,238.36,218.4z"/>
+	<path class="st3" d="M353.08,190.21h63.01V155.4h-63.01c-34.8,0-63.01,28.21-63.01,63.01s28.21,63.01,63.01,63.01h63.01v-34.82
+		h-63.01c-15.57,0-28.19-12.62-28.19-28.19C324.88,202.83,337.51,190.21,353.08,190.21z"/>
+	<polygon class="st3" points="95.79,155.4 95.79,200.82 39.41,200.82 39.41,155.4 4.25,155.4 4.25,281.41 39.41,281.41 
+		39.41,235.98 95.79,235.98 95.79,281.41 130.27,281.41 130.27,155.4 	"/>
+	<path class="st4" d="M737.06,200.43c-0.42-25.12-21.44-45.04-46.57-45.04h-45.07v34.32l45.49,0c6.19,0,11.52,4.76,11.81,10.95
+		c0.31,6.61-4.95,12.06-11.49,12.06h-11.5c-18.95,0-34.32,15.36-34.32,34.32v0v34.26h91.64v-34.26h-45.82
+		C716.81,247.03,737.49,226.1,737.06,200.43z"/>
+	<path class="st3" d="M272.4,55.19c-5.46-34.37-37.74-57.8-72.11-52.35c-34.37,5.46-57.8,37.74-52.35,72.11
+		c5.46,34.37,37.74,57.8,72.11,52.35C254.42,121.84,277.85,89.56,272.4,55.19z M210.17,93.26c-15.57,0-28.19-12.62-28.19-28.19
+		c0-15.57,12.62-28.19,28.19-28.19c15.57,0,28.19,12.62,28.19,28.19C238.36,80.64,225.74,93.26,210.17,93.26z"/>
+	
+		<rect x="593.74" y="155.4" transform="matrix(-1 -1.224647e-16 1.224647e-16 -1 1221.7888 356.6999)" class="st4" width="34.32" height="45.91"/>
+	<path class="st3" d="M370.51,2.12V2.05h-60.96v0.07h-19.48v125.9h34.32V93.26h46.13c25.17,0,45.57-20.4,45.57-45.57
+		C416.08,22.52,395.68,2.12,370.51,2.12z M364.98,64.71h-40.6V31.12h40.6v0.02c9.27,0,16.78,7.51,16.78,16.78
+		C381.77,57.19,374.25,64.71,364.98,64.71z"/>
+	<path class="st3" d="M846.12,47.69c0-25.17-20.4-45.57-45.57-45.57V2.05h-60.96v0.01h-19.48v126.02h34.32V93.26h32.67l34.76,34.76
+		l24.19,0.06l0.07-24.32l-19.03-19.03C838.61,76.45,846.12,62.95,846.12,47.69z M795.02,64.71
+		C795.02,64.71,795.02,64.71,795.02,64.71h-40.6V31.12h40.6v0.03c0,0,0,0,0,0c9.27,0,16.78,7.51,16.78,16.78
+		C811.8,57.19,804.29,64.71,795.02,64.71z"/>
+	<path class="st4" d="M846.1,195.54c0.31-22.05-18.25-40.09-40.3-40.09h-51.34v34.32h51.59v0.01c2.71,0,5.17,1.93,5.66,4.78
+		c0.33,1.96-0.48,3.98-1.98,5.29c-1.19,1.04-2.46,1.39-3.68,1.39v0.01h-27.06v34.32h27.06v0.01c2.71,0,5.17,1.93,5.66,4.78
+		c0.33,1.96-0.48,3.98-1.98,5.29c-1.19,1.04-2.46,1.39-3.68,1.39v0.01h-51.59v34.32h51.34c22.05,0,40.61-18.04,40.3-40.09
+		c-0.12-8.55-2.96-16.44-7.68-22.86C843.14,211.99,845.98,204.1,846.1,195.54z"/>
+</g>
+</svg>