Enable strict warnings for aruco_pose

.gitattributes

@@ -3,7 +3,6 @@ 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/build.yml

@@ -16,36 +16,8 @@ jobs:
   #         docker run --rm -v $(pwd):/root/catkin_ws/src/clover ros:melodic-ros-base /root/catkin_ws/src/clover/builder/standalone-install.sh
   noetic:
     runs-on: ubuntu-latest
-    container: ros:noetic-ros-base
-    defaults:
-      run:
-        working-directory: catkin_ws
-        shell: bash
     steps:
-    - uses: actions/checkout@v2
-      with:
-        path: catkin_ws/src/clover
-    - name: Install requirements
-      run: apt-get update && apt-get -y install python3-pip fakeroot python3-bloom debhelper dpkg-dev
-    - name: Install dependencies
-      run: rosdep update && rosdep install --from-paths src --ignore-src -y
-    - name: Install GeographicLib datasets
-      run: wget -qO- https://raw.githubusercontent.com/mavlink/mavros/master/mavros/scripts/install_geographiclib_datasets.sh | bash
-    - name: catkin_make
-      run: source /opt/ros/$ROS_DISTRO/setup.bash && catkin_make
-    - name: Run tests
-      run: source devel/setup.bash && catkin_make run_tests && catkin_test_results
-    - name: Build Debian packages
-      run: |
-        source devel/setup.bash
-        for file in `find . -name "package.xml"`; do
-          cd $(dirname ${file})
-          bloom-generate rosdebian --os-name ubuntu --os-version $(lsb_release -cs) --ros-distro $ROS_DISTRO  
-          fakeroot debian/rules binary
-          cd -
-        done
-    - uses: actions/upload-artifact@v3
-      with:
-        name: debian-packages
-        path: catkin_ws/src/clover/*.deb
-        retention-days: 1
+      - uses: actions/checkout@v2
+      - name: Native Noetic build
+        run: |
+          docker run --rm -v $(pwd):/root/catkin_ws/src/clover ros:noetic-ros-base /root/catkin_ws/src/clover/builder/standalone-install.sh

.github/workflows/docs.yml

@@ -4,21 +4,16 @@ on:
   push:
     branches: [ '*' ]
   pull_request:
-    branches: [ '*' ]
-
-permissions:
-  contents: read
-  pages: write
-  id-token: write
-
-defaults:
-  run:
-    shell: bash
+    branches: [ master ]
 
 jobs:
   docs:
-    runs-on: ubuntu-22.04
+    runs-on: ubuntu-18.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
@@ -63,26 +58,11 @@ jobs:
           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
+      - name: Deploy
+        uses: JamesIves/github-pages-deploy-action@4.1.3
+        if: ${{ github.event_name == 'push' && github.ref == 'refs/heads/master' }}
         with:
-          path: _book
-
-  deploy-docs:
-    if: ${{ github.event_name == 'push' && github.ref == 'refs/heads/master' }}
-    concurrency:
-        group: "pages"
-        cancel-in-progress: true
-    environment:
-      name: github-pages
-      url: ${{ steps.deployment.outputs.page_url }}
-    runs-on: ubuntu-latest
-    needs: docs
-    steps:
-      - name: Deploy to GitHub Pages
-        env:
-          FREEZE_DOCS: ${{ secrets.FREEZE_DOCS }}
-        if: ${{ !env.FREEZE_DOCS }}
-        id: deployment
-        uses: actions/deploy-pages@v1
+          branch: gh-pages
+          folder: _book
+          clean: true
+          single-commit: true # to avoid multiple copies of large pdf files

README.md

@@ -20,7 +20,7 @@ Clover drone is used on a wide range of educational events, including [Copter Ha
 
 Preconfigured image for Raspberry Pi with installed and configured software, ready to fly, is available [in the Releases section](https://github.com/CopterExpress/clover/releases).
 
-![GitHub Workflow Status](https://img.shields.io/github/actions/workflow/status/CopterExpress/clover/build-image.yaml?branch=master)
+![GitHub Workflow Status](https://img.shields.io/github/workflow/status/CopterExpress/clover/CI)
 ![GitHub all releases](https://img.shields.io/github/downloads/CopterExpress/clover/total)
 
 Image features:

aruco_pose/CMakeLists.txt

@@ -4,6 +4,8 @@ project(aruco_pose)
 ## Compile as C++11, supported in ROS Kinetic and newer
 add_compile_options(-std=c++11)
 
+add_compile_options(-Wall -Wextra -Werror)
+
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
@@ -119,7 +121,6 @@ generate_messages(
 ## Generate dynamic reconfigure parameters in the 'cfg' folder
 generate_dynamic_reconfigure_options(
   cfg/Detector.cfg
-  cfg/Map.cfg
 )
 
 ###################################
@@ -251,5 +252,4 @@ if (CATKIN_ENABLE_TESTING)
   add_rostest(test/test_node_failure.test)
   add_rostest(test/largemap.test)
   add_rostest(test/crash_opencv.test)
-  add_rostest(test/duplicate.test)
 endif()

aruco_pose/README.md

@@ -43,8 +43,7 @@ It's recommended to run it within the same nodelet manager with the camera nodel
 * `~frame_id_prefix` (*string*) – prefix for TF transforms names, marker's ID is appended (default: `aruco_`)
 * `~length` (*double*) – markers' sides length
 * `~length_override` (*map*) – lengths of markers with specified ids
-* `~known_vertical` (*string*) – known vertical (Z axis) of all the markers as a frame
-* `~flip_vertical` – flip vertical vector
+* `~known_tilt` (*string*) – known tilt (pitch and roll) of all the markers as a frame
 
 ### Topics
 
@@ -72,12 +71,10 @@ It's recommended to run it within the same nodelet manager with the camera nodel
 
 * `~map` – path to text file with markers list
 * `~frame_id` – published frame id (default: `aruco_map`)
-* `~known_vertical` – known vertical (Z axis) of markers map as a frame
-* `~flip_vertical` – flip vertical vector
+* `~known_tilt` – known tilt (pitch and roll) of markers map as a frame
 * `~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:

aruco_pose/cfg/Map.cfg

@@ -1,14 +0,0 @@
-#!/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/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
-<package format="3">
+<package format="2">
   <name>aruco_pose</name>
-  <version>0.24.0</version>
+  <version>0.23.0</version>
   <description>Positioning with ArUco markers</description>
 
   <maintainer email="okalachev@gmail.com">Oleg Kalachev</maintainer>
@@ -28,8 +28,6 @@
   <depend>sensor_msgs</depend>
   <depend>rostest</depend>
   <depend>dynamic_reconfigure</depend>
-  <depend condition="$ROS_PYTHON_VERSION == 2">python-docopt</depend>
-  <depend condition="$ROS_PYTHON_VERSION == 3">python3-docopt</depend>
 
   <test_depend>image_publisher</test_depend>
   <test_depend>ros_pytest</test_depend>

aruco_pose/src/aruco_detect.cpp

@@ -50,7 +50,6 @@
 #include <aruco_pose/DetectorConfig.h>
 #include <aruco_pose/SetMarkers.h>
 
-#include "draw.h"
 #include "utils.h"
 #include <memory>
 #include <functional>
@@ -72,12 +71,11 @@ private:
 	ros::Publisher markers_pub_, vis_markers_pub_;
 	ros::Subscriber map_markers_sub_;
 	ros::ServiceServer set_markers_srv_;
-	bool estimate_poses_, send_tf_, flip_vertical_, auto_flip_, use_map_markers_;
-	bool waiting_for_map_;
+	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_vertical_;
+	std::string frame_id_prefix_, known_tilt_;
 	Mat camera_matrix_, dist_coeffs_;
 	aruco_pose::MarkerArray array_;
 	std::unordered_set<int> map_markers_ids_;
@@ -97,8 +95,6 @@ public:
 		dictionary = nh_priv_.param("dictionary", 2);
 		estimate_poses_ = nh_priv_.param("estimate_poses", true);
 		send_tf_ = nh_priv_.param("send_tf", true);
-		use_map_markers_ = nh_priv_.param("use_map_markers", false);
-		waiting_for_map_ = use_map_markers_;
 		if (estimate_poses_ && !nh_priv_.getParam("length", length_)) {
 			NODELET_FATAL("can't estimate marker's poses as ~length parameter is not defined");
 			ros::shutdown();
@@ -106,8 +102,7 @@ public:
 		readLengthOverride(nh_priv_);
 		transform_timeout_ = ros::Duration(nh_priv_.param("transform_timeout", 0.02));
 
-		known_vertical_ = nh_priv_.param("known_vertical", nh_priv_.param("known_tilt", std::string(""))); // known_tilt is an old name
-		flip_vertical_ = nh_priv_.param<bool>("flip_vertical", false);
+		known_tilt_ = nh_priv_.param<std::string>("known_tilt", "");
 		auto_flip_ = nh_priv_.param("auto_flip", false);
 
 		frame_id_prefix_ = nh_priv_.param<std::string>("frame_id_prefix", "aruco_");
@@ -138,15 +133,14 @@ private:
 	void imageCallback(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr &cinfo)
 	{
 		if (!enabled_) return;
-		if (waiting_for_map_) return;
 
-		Mat image = cv_bridge::toCvShare(msg)->image;
+		Mat image = cv_bridge::toCvShare(msg, "bgr8")->image;
 
 		vector<int> ids;
 		vector<vector<cv::Point2f>> corners, rejected;
 		vector<cv::Vec3d> rvecs, tvecs;
 		vector<cv::Point3f> obj_points;
-		geometry_msgs::TransformStamped vertical;
+		geometry_msgs::TransformStamped snap_to;
 
 		// Detect markers
 		cv::aruco::detectMarkers(image, dictionary_, corners, ids, parameters_, rejected);
@@ -181,20 +175,18 @@ private:
 					}
 				}
 
-				if (!known_vertical_.empty()) {
+				if (!known_tilt_.empty()) {
 					try {
-						vertical = tf_buffer_->lookupTransform(msg->header.frame_id, known_vertical_,
-						                                       msg->header.stamp, transform_timeout_);
+						snap_to = tf_buffer_->lookupTransform(msg->header.frame_id, known_tilt_,
+						                                      msg->header.stamp, transform_timeout_);
 					} catch (const tf2::TransformException& e) {
-						NODELET_WARN_THROTTLE(5, "can't retrieve known vertical: %s", e.what());
+						NODELET_WARN_THROTTLE(5, "can't snap: %s", e.what());
 					}
 				}
 			}
 
 			array_.markers.reserve(ids.size());
 			aruco_pose::Marker marker;
-			vector<geometry_msgs::TransformStamped> transforms;
-			transforms.reserve(ids.size());
 			geometry_msgs::TransformStamped transform;
 			transform.header.stamp = msg->header.stamp;
 			transform.header.frame_id = msg->header.frame_id;
@@ -207,38 +199,25 @@ private:
 				if (estimate_poses_) {
 					fillPose(marker.pose, rvecs[i], tvecs[i]);
 
-					// apply known vertical (if enabled and vertical frame available)
-					if (!known_vertical_.empty() && !vertical.header.frame_id.empty()) {
-						applyVertical(marker.pose.orientation, vertical.transform.rotation, false, auto_flip_);
+					// snap orientation (if enabled and snap frame available)
+					if (!known_tilt_.empty() && !snap_to.header.frame_id.empty()) {
+						snapOrientation(marker.pose.orientation, snap_to.transform.rotation, auto_flip_);
 					}
 
+					// TODO: check IDs are unique
 					if (send_tf_) {
 						transform.child_frame_id = getChildFrameId(ids[i]);
 
 						// check if such static transform is in the map
 						if (map_markers_ids_.find(ids[i]) == map_markers_ids_.end()) {
-							// check if a markers with that id is already added
-							bool send = true;
-							for (auto &t : transforms) {
-								if (t.child_frame_id == transform.child_frame_id) {
-									send = false;
-									break;
-								}
-							}
-							if (send) {
-								transform.transform.rotation = marker.pose.orientation;
-								fillTranslation(transform.transform.translation, tvecs[i]);
-								transforms.push_back(transform);
-							}
+							transform.transform.rotation = marker.pose.orientation;
+							fillTranslation(transform.transform.translation, tvecs[i]);
+							br_->sendTransform(transform);
 						}
 					}
 				}
 				array_.markers.push_back(marker);
 			}
-
-			if (send_tf_) {
-				br_->sendTransform(transforms);
-			}
 		}
 
 		markers_pub_.publish(array_);
@@ -265,7 +244,8 @@ private:
 			cv::aruco::drawDetectedMarkers(debug, corners, ids); // draw markers
 			if (estimate_poses_)
 				for (unsigned int i = 0; i < ids.size(); i++)
-					_drawAxis(debug, camera_matrix_, dist_coeffs_, rvecs[i], tvecs[i], getMarkerLength(ids[i]));
+					cv::aruco::drawAxis(debug, camera_matrix_, dist_coeffs_,
+					                    rvecs[i], tvecs[i], getMarkerLength(ids[i]));
 
 			cv_bridge::CvImage out_msg;
 			out_msg.header.frame_id = msg->header.frame_id;
@@ -400,13 +380,7 @@ private:
 		map_markers_ids_.clear();
 		for (auto const& marker : msg.markers) {
 			map_markers_ids_.insert(marker.id);
-			if (use_map_markers_) {
-				if (length_override_.find(marker.id) == length_override_.end()) {
-					length_override_[marker.id] = marker.length;
-				}
-			}
 		}
-		waiting_for_map_ = false;
 	}
 
 	void paramCallback(aruco_pose::DetectorConfig &config, uint32_t level)

aruco_pose/src/aruco_map.cpp

@@ -19,13 +19,11 @@
 #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>
@@ -43,7 +41,6 @@
 
 #include <aruco_pose/MarkerArray.h>
 #include <aruco_pose/Marker.h>
-#include <aruco_pose/MapConfig.h>
 
 #include <opencv2/opencv.hpp>
 #include <opencv2/aruco.hpp>
@@ -77,13 +74,10 @@ 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_vertical_, map_, markers_frame_, markers_parent_frame_;
+	std::string known_tilt_, map_, markers_frame_, markers_parent_frame_;
 	int image_width_, image_height_, image_margin_;
-	bool flip_vertical_, auto_flip_, image_axis_, put_markers_count_to_covariance_;
+	bool auto_flip_, image_axis_;
 
 public:
 	virtual void onInit()
@@ -102,28 +96,27 @@ public:
 			                 static_cast<cv::aruco::PREDEFINED_DICTIONARY_NAME>(nh_priv_.param("dictionary", 2)));
 		camera_matrix_ = cv::Mat::zeros(3, 3, CV_64F);
 
-		type_ = nh_priv_.param<std::string>("type", "map");
+		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_vertical_ = nh_priv_.param("known_vertical", nh_priv_.param("known_tilt", std::string(""))); // known_tilt is an old name
-		flip_vertical_ = nh_priv_.param<bool>("flip_vertical", false);
+		known_tilt_ = nh_priv_.param<std::string>("known_tilt", "");
 		auto_flip_ = nh_priv_.param("auto_flip", false);
 		image_width_ = nh_priv_.param("image_width" , 2000);
 		image_height_ = nh_priv_.param("image_height", 2000);
 		image_margin_ = nh_priv_.param("image_margin", 200);
 		image_axis_ = nh_priv_.param("image_axis", true);
-		put_markers_count_to_covariance_ = nh_priv_.param("put_markers_count_to_covariance", false);
 		markers_parent_frame_ = nh_priv_.param<std::string>("markers/frame_id", transform_.child_frame_id);
 		markers_frame_ = nh_priv_.param<std::string>("markers/child_frame_id_prefix", "");
 
 		// createStripLine();
 
-		if (type_ == "map") {
-			map_ = nh_priv_.param<std::string>("map" , "");
-			loadMap(map_);
-		} else if (type_ == "gridboard") {
+		if (type == "map") {
+			param(nh_priv_, "map", map);
+			loadMap(map);
+		} 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();
 		}
 
@@ -131,7 +124,10 @@ public:
 		vis_markers_pub_ = nh_priv_.advertise<visualization_msgs::MarkerArray>("visualization", 1, true);
 		debug_pub_ = it_priv.advertise("debug", 1);
 
-		publishMap();
+		publishMarkersFrames();
+		publishMarkers();
+		publishMapImage();
+		vis_markers_pub_.publish(vis_array_);
 
 		image_sub_.subscribe(nh_, "image_raw", 1);
 		info_sub_.subscribe(nh_, "camera_info", 1);
@@ -140,12 +136,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));
 
-		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");
 	}
 
@@ -153,9 +143,6 @@ 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;
@@ -179,21 +166,7 @@ public:
 			corners.push_back(marker_corners);
 		}
 
-		if (put_markers_count_to_covariance_) {
-			// HACK: pass markers count using covariance field
-			int valid_markers = 0;
-			for (auto const &marker : markers->markers) {
-				for (auto const &board_marker : board_->ids) {
-					if (board_marker == marker.id) {
-						valid_markers++;
-						break;
-					}
-				}
-			}
-			pose_.pose.covariance[0] = valid_markers;
-		}
-
-		if (known_vertical_.empty()) {
+		if (known_tilt_.empty()) {
 			// simple estimation
 			valid = cv::aruco::estimatePoseBoard(corners, ids, board_, camera_matrix_, dist_coeffs_,
 			                                     rvec, tvec, false);
@@ -207,7 +180,7 @@ public:
 
 		} else {
 			Mat obj_points, img_points;
-			// estimation with known vertical
+			// estimation with "snapping"
 			cv::aruco::getBoardObjectAndImagePoints(board_, corners, ids, obj_points, img_points);
 			if (obj_points.empty()) goto publish_debug;
 
@@ -219,11 +192,11 @@ public:
 
 			fillTransform(transform_.transform, rvec, tvec);
 			try {
-				geometry_msgs::TransformStamped vertical = tf_buffer_.lookupTransform(markers->header.frame_id,
-				                                           known_vertical_, markers->header.stamp, ros::Duration(0.02));
-				applyVertical(transform_.transform.rotation, vertical.transform.rotation, flip_vertical_, auto_flip_);
+				geometry_msgs::TransformStamped snap_to = tf_buffer_.lookupTransform(markers->header.frame_id,
+				                                          known_tilt_, markers->header.stamp, ros::Duration(0.02));
+				snapOrientation(transform_.transform.rotation, snap_to.transform.rotation, auto_flip_);
 			} catch (const tf2::TransformException& e) {
-				NODELET_WARN_THROTTLE(1, "can't retrieve known vertical: %s", e.what());
+				NODELET_WARN_THROTTLE(1, "can't snap: %s", e.what());
 			}
 
 			geometry_msgs::TransformStamped shift;
@@ -295,17 +268,9 @@ publish_debug:
 		std::ifstream f(filename);
 		std::string line;
 
-		clearMarkers();
-
-		if (map_ == "") {
-			NODELET_INFO("No map loaded");
-			return;
-		}
-
 		if (!f.good()) {
-			NODELET_ERROR("%s - %s", strerror(errno), filename.c_str());
-			map_ = "";
-			return;
+			NODELET_FATAL("%s - %s", strerror(errno), filename.c_str());
+			ros::shutdown();
 		}
 
 		while (std::getline(f, line)) {
@@ -331,10 +296,9 @@ publish_debug:
 				s.putback(first);
 			} else {
 				// Probably garbage data; inform user and throw an exception, possibly killing nodelet
-				NODELET_ERROR("Malformed input: %s", line.c_str());
-				map_ = "";
-				clearMarkers();
-				return;
+				NODELET_FATAL("Malformed input: %s", line.c_str());
+				ros::shutdown();
+				throw std::runtime_error("Malformed input");
 			}
 
 			if (!(s >> id >> length >> x >> y)) {
@@ -365,14 +329,6 @@ 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");
@@ -414,15 +370,6 @@ 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;
@@ -519,7 +466,7 @@ publish_debug:
 		vis_marker.pose.position.x = x;
 		vis_marker.pose.position.y = y;
 		vis_marker.pose.position.z = z;
-		tf::quaternionTFToMsg(q, vis_marker.pose.orientation);
+		tf::quaternionTFToMsg(q, marker.pose.orientation);
 		vis_marker.frame_locked = true;
 		vis_array_.markers.push_back(vis_marker);
 
@@ -562,22 +509,6 @@ 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/src/utils.h

@@ -106,25 +106,26 @@ inline bool isFlipped(tf::Quaternion& q)
 	return (abs(pitch) > M_PI / 2) || (abs(roll) > M_PI / 2);
 }
 
-/* Apply a vertical to an orientation */
-inline void applyVertical(geometry_msgs::Quaternion& orientation, const geometry_msgs::Quaternion& vertical,
-                          bool flip_vertical = false, bool auto_flip = false) // editorconfig-checker-disable-line
+/* Set roll and pitch from "from" to "to", keeping yaw */
+inline void snapOrientation(geometry_msgs::Quaternion& to, const geometry_msgs::Quaternion& from, bool auto_flip = false)
 {
-	tf::Quaternion _vertical, _orientation;
-	tf::quaternionMsgToTF(vertical, _vertical);
-	tf::quaternionMsgToTF(orientation, _orientation);
+	tf::Quaternion _from, _to;
+	tf::quaternionMsgToTF(from, _from);
+	tf::quaternionMsgToTF(to, _to);
 
-	if (flip_vertical || (auto_flip && !isFlipped(_orientation))) {
-		static const tf::Quaternion flip = tf::createQuaternionFromRPY(M_PI, 0, 0);
-		_vertical *= flip; // flip vertical
+	if (auto_flip) {
+		if (!isFlipped(_from)) {
+			static const tf::Quaternion flip = tf::createQuaternionFromRPY(M_PI, 0, 0);
+			_from *= flip; // flip "from"
+		}
 	}
 
-	auto diff = tf::Matrix3x3(_orientation).transposeTimes(tf::Matrix3x3(_vertical));
+	auto diff = tf::Matrix3x3(_to).transposeTimes(tf::Matrix3x3(_from));
 	double _, yaw;
 	diff.getRPY(_, _, yaw);
 	auto q = tf::createQuaternionFromRPY(0, 0, -yaw);
-	_vertical = _vertical * q; // set yaw from orientation to vertical
-	tf::quaternionTFToMsg(_vertical, orientation); // set vertical to orientation
+	_from = _from * q; // set yaw from "to" to "from"
+	tf::quaternionTFToMsg(_from, to); // set "from" to "to"
 }
 
 inline void transformToPose(const geometry_msgs::Transform& transform, geometry_msgs::Pose& pose)

aruco_pose/test/basic.py

@@ -6,7 +6,7 @@ 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, Marker as VisMarker
+from visualization_msgs.msg import MarkerArray as VisMarkerArray
 
 
 @pytest.fixture
@@ -199,36 +199,6 @@ def test_map_markers(node):
 
 def test_map_visualization(node):
     vis = rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5)
-    assert len(vis.markers) == 7
-    assert vis.markers[0].header.frame_id == 'aruco_map'
-    assert vis.markers[0].type == VisMarker.CUBE
-    assert vis.markers[0].action == VisMarker.ADD
-    assert vis.markers[0].pose.position.x == 0
-    assert vis.markers[0].pose.position.y == 0
-    assert vis.markers[0].pose.position.z == 0
-    assert vis.markers[0].pose.orientation.x == 0
-    assert vis.markers[0].pose.orientation.y == 0
-    assert vis.markers[0].pose.orientation.z == 0
-    assert vis.markers[0].pose.orientation.w == 1
-    assert vis.markers[0].scale.x == approx(0.33)
-    assert vis.markers[0].scale.y == approx(0.33)
-    assert vis.markers[0].scale.z == approx(0.001)
-    assert vis.markers[1].pose.position.x == 1
-    assert vis.markers[1].pose.position.y == 0
-    assert vis.markers[1].pose.position.z == 0
-    assert vis.markers[1].pose.orientation.x == 0
-    assert vis.markers[1].pose.orientation.y == 0
-    assert vis.markers[1].pose.orientation.z == 0
-    assert vis.markers[1].pose.orientation.w == 1
-    # non-zero yaw marker:
-    assert vis.markers[4].scale.x == approx(0.5)
-    assert vis.markers[4].pose.position.x == approx(0.5)
-    assert vis.markers[4].pose.position.y == 2
-    assert vis.markers[4].pose.position.z == 0
-    assert vis.markers[4].pose.orientation.x == 0
-    assert vis.markers[4].pose.orientation.y == 0
-    assert vis.markers[4].pose.orientation.z == approx(0.5646424733950354)
-    assert vis.markers[4].pose.orientation.w == approx(0.8253356149096783)
 
 def test_map_debug(node):
     img = rospy.wait_for_message('aruco_map/debug', Image, timeout=5)

aruco_pose/test/duplicate.py

@@ -1,8 +0,0 @@
-import pytest
-import subprocess
-
-def test_no_tf_repeated_data():
-    # `/rosout` acts weirdly inside rostest, so using a subprocess
-    cmd = """python -c 'import rospy, tf; rospy.init_node("foo"); listener = tf.TransformListener(); rospy.sleep(2)'"""
-    output = str(subprocess.check_output(cmd, shell=True, stderr=subprocess.STDOUT))
-    assert 'TF_REPEATED_DATA' not in output, 'TF_REPEATED_DATA was logged on duplicate markers'

aruco_pose/test/duplicate.test

@@ -1,21 +0,0 @@
-<launch>
-    <node pkg="image_publisher" type="image_publisher" name="main_camera" args="$(find aruco_pose)/test/duplicate.png">
-        <param name="frame_id" value="main_camera_optical"/>
-        <param name="publish_rate" value="10"/>
-        <param name="camera_info_url" value="file://$(find aruco_pose)/test/camera_info.yaml" />
-    </node>
-
-    <node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager" required="true"/>
-
-    <node pkg="nodelet" clear_params="true" type="nodelet" name="aruco_detect" args="load aruco_pose/aruco_detect nodelet_manager" required="true">
-        <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="estimate_poses" value="true"/>
-        <param name="send_tf" value="true"/>
-        <param name="cornerRefinementMethod" value="1"/>
-    </node>
-
-	<param name="test_module" value="$(find aruco_pose)/test/duplicate.py"/>
-    <test test-name="aruco_pose_test" pkg="ros_pytest" type="ros_pytest_runner"/>
-</launch>

aruco_pose/test/test_node_failure.py

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

builder/assets/noetic-rosdep-clover.yaml

@@ -16,723 +16,3 @@ web_video_server:
 ws281x:
   debian:
     buster: [ros-noetic-ws281x]
-catkin:
-  debian:
-    buster: [ros-noetic-catkin]
-genmsg:
-  debian:
-    buster: [ros-noetic-genmsg]
-gencpp:
-  debian:
-    buster: [ros-noetic-gencpp]
-geneus:
-  debian:
-    buster: [ros-noetic-geneus]
-genlisp:
-  debian:
-    buster: [ros-noetic-genlisp]
-gennodejs:
-  debian:
-    buster: [ros-noetic-gennodejs]
-genpy:
-  debian:
-    buster: [ros-noetic-genpy]
-bond_core:
-  debian:
-    buster: [ros-noetic-bond-core]
-cmake_modules:
-  debian:
-    buster: [ros-noetic-cmake-modules]
-class_loader:
-  debian:
-    buster: [ros-noetic-class-loader]
-common_msgs:
-  debian:
-    buster: [ros-noetic-common-msgs]
-common_tutorials:
-  debian:
-    buster: [ros-noetic-common-tutorials]
-cpp_common:
-  debian:
-    buster: [ros-noetic-cpp-common]
-desktop:
-  debian:
-    buster: [ros-noetic-desktop]
-diagnostics:
-  debian:
-    buster: [ros-noetic-diagnostics]
-executive_smach:
-  debian:
-    buster: [ros-noetic-executive-smach]
-geometry:
-  debian:
-    buster: [ros-noetic-geometry]
-geometry_tutorials:
-  debian:
-    buster: [ros-noetic-geometry-tutorials]
-gl_dependency:
-  debian:
-    buster: [ros-noetic-gl-dependency]
-image_common:
-  debian:
-    buster: [ros-noetic-image-common]
-image_pipeline:
-  debian:
-    buster: [ros-noetic-image-pipeline]
-image_transport_plugins:
-  debian:
-    buster: [ros-noetic-image-transport-plugins]
-laser_pipeline:
-  debian:
-    buster: [ros-noetic-laser-pipeline]
-mavlink:
-  debian:
-    buster: [ros-noetic-mavlink]
-media_export:
-  debian:
-    buster: [ros-noetic-media-export]
-message_generation:
-  debian:
-    buster: [ros-noetic-message-generation]
-message_runtime:
-  debian:
-    buster: [ros-noetic-message-runtime]
-mk:
-  debian:
-    buster: [ros-noetic-mk]
-nodelet_core:
-  debian:
-    buster: [ros-noetic-nodelet-core]
-orocos_kdl:
-  debian:
-    buster: [ros-noetic-orocos-kdl]
-perception:
-  debian:
-    buster: [ros-noetic-perception]
-perception_pcl:
-  debian:
-    buster: [ros-noetic-perception-pcl]
-python_orocos_kdl:
-  debian:
-    buster: [ros-noetic-python-orocos-kdl]
-qt_dotgraph:
-  debian:
-    buster: [ros-noetic-qt-dotgraph]
-qt_gui:
-  debian:
-    buster: [ros-noetic-qt-gui]
-qt_gui_py_common:
-  debian:
-    buster: [ros-noetic-qt-gui-py-common]
-qwt_dependency:
-  debian:
-    buster: [ros-noetic-qwt-dependency]
-robot:
-  debian:
-    buster: [ros-noetic-robot]
-ros:
-  debian:
-    buster: [ros-noetic-ros]
-ros_base:
-  debian:
-    buster: [ros-noetic-ros-base]
-ros_comm:
-  debian:
-    buster: [ros-noetic-ros-comm]
-ros_core:
-  debian:
-    buster: [ros-noetic-ros-core]
-ros_environment:
-  debian:
-    buster: [ros-noetic-ros-environment]
-ros_tutorials:
-  debian:
-    buster: [ros-noetic-ros-tutorials]
-rosapi:
-  debian:
-    buster: [ros-noetic-rosapi]
-rosbag_migration_rule:
-  debian:
-    buster: [ros-noetic-rosbag-migration-rule]
-rosbash:
-  debian:
-    buster: [ros-noetic-rosbash]
-rosboost_cfg:
-  debian:
-    buster: [ros-noetic-rosboost-cfg]
-rosbridge_server:
-  debian:
-    buster: [ros-noetic-rosbridge-server]
-rosbridge_suite:
-  debian:
-    buster: [ros-noetic-rosbridge-suite]
-rosbuild:
-  debian:
-    buster: [ros-noetic-rosbuild]
-rosclean:
-  debian:
-    buster: [ros-noetic-rosclean]
-roscpp_core:
-  debian:
-    buster: [ros-noetic-roscpp-core]
-roscpp_traits:
-  debian:
-    buster: [ros-noetic-roscpp-traits]
-roscreate:
-  debian:
-    buster: [ros-noetic-roscreate]
-rosgraph:
-  debian:
-    buster: [ros-noetic-rosgraph]
-roslang:
-  debian:
-    buster: [ros-noetic-roslang]
-roslint:
-  debian:
-    buster: [ros-noetic-roslint]
-roslisp:
-  debian:
-    buster: [ros-noetic-roslisp]
-rosmake:
-  debian:
-    buster: [ros-noetic-rosmake]
-rosmaster:
-  debian:
-    buster: [ros-noetic-rosmaster]
-rospack:
-  debian:
-    buster: [ros-noetic-rospack]
-roslib:
-  debian:
-    buster: [ros-noetic-roslib]
-rosparam:
-  debian:
-    buster: [ros-noetic-rosparam]
-rospy:
-  debian:
-    buster: [ros-noetic-rospy]
-rosserial:
-  debian:
-    buster: [ros-noetic-rosserial]
-rosserial_msgs:
-  debian:
-    buster: [ros-noetic-rosserial-msgs]
-rosserial_python:
-  debian:
-    buster: [ros-noetic-rosserial-python]
-rosservice:
-  debian:
-    buster: [ros-noetic-rosservice]
-rostime:
-  debian:
-    buster: [ros-noetic-rostime]
-roscpp_serialization:
-  debian:
-    buster: [ros-noetic-roscpp-serialization]
-python_qt_binding:
-  debian:
-    buster: [ros-noetic-python-qt-binding]
-roslaunch:
-  debian:
-    buster: [ros-noetic-roslaunch]
-rosunit:
-  debian:
-    buster: [ros-noetic-rosunit]
-angles:
-  debian:
-    buster: [ros-noetic-angles]
-libmavconn:
-  debian:
-    buster: [ros-noetic-libmavconn]
-rosconsole:
-  debian:
-    buster: [ros-noetic-rosconsole]
-pluginlib:
-  debian:
-    buster: [ros-noetic-pluginlib]
-qt_gui_cpp:
-  debian:
-    buster: [ros-noetic-qt-gui-cpp]
-resource_retriever:
-  debian:
-    buster: [ros-noetic-resource-retriever]
-rosconsole_bridge:
-  debian:
-    buster: [ros-noetic-rosconsole-bridge]
-roslz4:
-  debian:
-    buster: [ros-noetic-roslz4]
-rosserial_client:
-  debian:
-    buster: [ros-noetic-rosserial-client]
-rostest:
-  debian:
-    buster: [ros-noetic-rostest]
-rqt_action:
-  debian:
-    buster: [ros-noetic-rqt-action]
-rqt_bag:
-  debian:
-    buster: [ros-noetic-rqt-bag]
-rqt_bag_plugins:
-  debian:
-    buster: [ros-noetic-rqt-bag-plugins]
-rqt_common_plugins:
-  debian:
-    buster: [ros-noetic-rqt-common-plugins]
-rqt_console:
-  debian:
-    buster: [ros-noetic-rqt-console]
-rqt_dep:
-  debian:
-    buster: [ros-noetic-rqt-dep]
-rqt_graph:
-  debian:
-    buster: [ros-noetic-rqt-graph]
-rqt_gui:
-  debian:
-    buster: [ros-noetic-rqt-gui]
-rqt_logger_level:
-  debian:
-    buster: [ros-noetic-rqt-logger-level]
-rqt_moveit:
-  debian:
-    buster: [ros-noetic-rqt-moveit]
-rqt_msg:
-  debian:
-    buster: [ros-noetic-rqt-msg]
-rqt_nav_view:
-  debian:
-    buster: [ros-noetic-rqt-nav-view]
-rqt_plot:
-  debian:
-    buster: [ros-noetic-rqt-plot]
-rqt_pose_view:
-  debian:
-    buster: [ros-noetic-rqt-pose-view]
-rqt_publisher:
-  debian:
-    buster: [ros-noetic-rqt-publisher]
-rqt_py_console:
-  debian:
-    buster: [ros-noetic-rqt-py-console]
-rqt_reconfigure:
-  debian:
-    buster: [ros-noetic-rqt-reconfigure]
-rqt_robot_dashboard:
-  debian:
-    buster: [ros-noetic-rqt-robot-dashboard]
-rqt_robot_monitor:
-  debian:
-    buster: [ros-noetic-rqt-robot-monitor]
-rqt_robot_plugins:
-  debian:
-    buster: [ros-noetic-rqt-robot-plugins]
-rqt_robot_steering:
-  debian:
-    buster: [ros-noetic-rqt-robot-steering]
-rqt_runtime_monitor:
-  debian:
-    buster: [ros-noetic-rqt-runtime-monitor]
-rqt_service_caller:
-  debian:
-    buster: [ros-noetic-rqt-service-caller]
-rqt_shell:
-  debian:
-    buster: [ros-noetic-rqt-shell]
-rqt_srv:
-  debian:
-    buster: [ros-noetic-rqt-srv]
-rqt_tf_tree:
-  debian:
-    buster: [ros-noetic-rqt-tf-tree]
-rqt_top:
-  debian:
-    buster: [ros-noetic-rqt-top]
-rqt_topic:
-  debian:
-    buster: [ros-noetic-rqt-topic]
-rqt_web:
-  debian:
-    buster: [ros-noetic-rqt-web]
-smach:
-  debian:
-    buster: [ros-noetic-smach]
-smclib:
-  debian:
-    buster: [ros-noetic-smclib]
-std_msgs:
-  debian:
-    buster: [ros-noetic-std-msgs]
-actionlib_msgs:
-  debian:
-    buster: [ros-noetic-actionlib-msgs]
-bond:
-  debian:
-    buster: [ros-noetic-bond]
-diagnostic_msgs:
-  debian:
-    buster: [ros-noetic-diagnostic-msgs]
-geometry_msgs:
-  debian:
-    buster: [ros-noetic-geometry-msgs]
-eigen_conversions:
-  debian:
-    buster: [ros-noetic-eigen-conversions]
-kdl_conversions:
-  debian:
-    buster: [ros-noetic-kdl-conversions]
-nav_msgs:
-  debian:
-    buster: [ros-noetic-nav-msgs]
-rosbridge_msgs:
-  debian:
-    buster: [ros-noetic-rosbridge-msgs]
-rosgraph_msgs:
-  debian:
-    buster: [ros-noetic-rosgraph-msgs]
-rosmsg:
-  debian:
-    buster: [ros-noetic-rosmsg]
-rqt_py_common:
-  debian:
-    buster: [ros-noetic-rqt-py-common]
-shape_msgs:
-  debian:
-    buster: [ros-noetic-shape-msgs]
-smach_msgs:
-  debian:
-    buster: [ros-noetic-smach-msgs]
-std_srvs:
-  debian:
-    buster: [ros-noetic-std-srvs]
-tf2_msgs:
-  debian:
-    buster: [ros-noetic-tf2-msgs]
-tf2:
-  debian:
-    buster: [ros-noetic-tf2]
-tf2_eigen:
-  debian:
-    buster: [ros-noetic-tf2-eigen]
-trajectory_msgs:
-  debian:
-    buster: [ros-noetic-trajectory-msgs]
-control_msgs:
-  debian:
-    buster: [ros-noetic-control-msgs]
-urdf_parser_plugin:
-  debian:
-    buster: [ros-noetic-urdf-parser-plugin]
-urdfdom_py:
-  debian:
-    buster: [ros-noetic-urdfdom-py]
-uuid_msgs:
-  debian:
-    buster: [ros-noetic-uuid-msgs]
-geographic_msgs:
-  debian:
-    buster: [ros-noetic-geographic-msgs]
-vision_opencv:
-  debian:
-    buster: [ros-noetic-vision-opencv]
-visualization_msgs:
-  debian:
-    buster: [ros-noetic-visualization-msgs]
-visualization_tutorials:
-  debian:
-    buster: [ros-noetic-visualization-tutorials]
-viz:
-  debian:
-    buster: [ros-noetic-viz]
-webkit_dependency:
-  debian:
-    buster: [ros-noetic-webkit-dependency]
-xmlrpcpp:
-  debian:
-    buster: [ros-noetic-xmlrpcpp]
-roscpp:
-  debian:
-    buster: [ros-noetic-roscpp]
-bondcpp:
-  debian:
-    buster: [ros-noetic-bondcpp]
-bondpy:
-  debian:
-    buster: [ros-noetic-bondpy]
-nodelet:
-  debian:
-    buster: [ros-noetic-nodelet]
-nodelet_tutorial_math:
-  debian:
-    buster: [ros-noetic-nodelet-tutorial-math]
-pluginlib_tutorials:
-  debian:
-    buster: [ros-noetic-pluginlib-tutorials]
-roscpp_tutorials:
-  debian:
-    buster: [ros-noetic-roscpp-tutorials]
-rosout:
-  debian:
-    buster: [ros-noetic-rosout]
-camera_calibration:
-  debian:
-    buster: [ros-noetic-camera-calibration]
-diagnostic_aggregator:
-  debian:
-    buster: [ros-noetic-diagnostic-aggregator]
-diagnostic_updater:
-  debian:
-    buster: [ros-noetic-diagnostic-updater]
-diagnostic_common_diagnostics:
-  debian:
-    buster: [ros-noetic-diagnostic-common-diagnostics]
-dynamic_reconfigure:
-  debian:
-    buster: [ros-noetic-dynamic-reconfigure]
-filters:
-  debian:
-    buster: [ros-noetic-filters]
-joint_state_publisher:
-  debian:
-    buster: [ros-noetic-joint-state-publisher]
-message_filters:
-  debian:
-    buster: [ros-noetic-message-filters]
-rosauth:
-  debian:
-    buster: [ros-noetic-rosauth]
-rosbag_storage:
-  debian:
-    buster: [ros-noetic-rosbag-storage]
-rosnode:
-  debian:
-    buster: [ros-noetic-rosnode]
-rospy_tutorials:
-  debian:
-    buster: [ros-noetic-rospy-tutorials]
-rosshow:
-  debian:
-    buster: [ros-noetic-rosshow]
-rostopic:
-  debian:
-    buster: [ros-noetic-rostopic]
-rqt_gui_cpp:
-  debian:
-    buster: [ros-noetic-rqt-gui-cpp]
-rqt_gui_py:
-  debian:
-    buster: [ros-noetic-rqt-gui-py]
-self_test:
-  debian:
-    buster: [ros-noetic-self-test]
-smach_ros:
-  debian:
-    buster: [ros-noetic-smach-ros]
-tf2_py:
-  debian:
-    buster: [ros-noetic-tf2-py]
-topic_tools:
-  debian:
-    buster: [ros-noetic-topic-tools]
-rosbag:
-  debian:
-    buster: [ros-noetic-rosbag]
-actionlib:
-  debian:
-    buster: [ros-noetic-actionlib]
-actionlib_tutorials:
-  debian:
-    buster: [ros-noetic-actionlib-tutorials]
-diagnostic_analysis:
-  debian:
-    buster: [ros-noetic-diagnostic-analysis]
-nodelet_topic_tools:
-  debian:
-    buster: [ros-noetic-nodelet-topic-tools]
-roswtf:
-  debian:
-    buster: [ros-noetic-roswtf]
-rqt_launch:
-  debian:
-    buster: [ros-noetic-rqt-launch]
-sensor_msgs:
-  debian:
-    buster: [ros-noetic-sensor-msgs]
-camera_calibration_parsers:
-  debian:
-    buster: [ros-noetic-camera-calibration-parsers]
-cv_bridge:
-  debian:
-    buster: [ros-noetic-cv-bridge]
-image_geometry:
-  debian:
-    buster: [ros-noetic-image-geometry]
-image_transport:
-  debian:
-    buster: [ros-noetic-image-transport]
-camera_info_manager:
-  debian:
-    buster: [ros-noetic-camera-info-manager]
-compressed_depth_image_transport:
-  debian:
-    buster: [ros-noetic-compressed-depth-image-transport]
-compressed_image_transport:
-  debian:
-    buster: [ros-noetic-compressed-image-transport]
-cv_camera:
-  debian:
-    buster: [ros-noetic-cv-camera]
-image_proc:
-  debian:
-    buster: [ros-noetic-image-proc]
-image_publisher:
-  debian:
-    buster: [ros-noetic-image-publisher]
-map_msgs:
-  debian:
-    buster: [ros-noetic-map-msgs]
-mavros_msgs:
-  debian:
-    buster: [ros-noetic-mavros-msgs]
-pcl_msgs:
-  debian:
-    buster: [ros-noetic-pcl-msgs]
-pcl_conversions:
-  debian:
-    buster: [ros-noetic-pcl-conversions]
-polled_camera:
-  debian:
-    buster: [ros-noetic-polled-camera]
-rqt_image_view:
-  debian:
-    buster: [ros-noetic-rqt-image-view]
-stereo_msgs:
-  debian:
-    buster: [ros-noetic-stereo-msgs]
-image_view:
-  debian:
-    buster: [ros-noetic-image-view]
-rosbridge_library:
-  debian:
-    buster: [ros-noetic-rosbridge-library]
-stereo_image_proc:
-  debian:
-    buster: [ros-noetic-stereo-image-proc]
-tf2_ros:
-  debian:
-    buster: [ros-noetic-tf2-ros]
-depth_image_proc:
-  debian:
-    buster: [ros-noetic-depth-image-proc]
-mavros:
-  debian:
-    buster: [ros-noetic-mavros]
-tf:
-  debian:
-    buster: [ros-noetic-tf]
-interactive_markers:
-  debian:
-    buster: [ros-noetic-interactive-markers]
-interactive_marker_tutorials:
-  debian:
-    buster: [ros-noetic-interactive-marker-tutorials]
-laser_geometry:
-  debian:
-    buster: [ros-noetic-laser-geometry]
-laser_assembler:
-  debian:
-    buster: [ros-noetic-laser-assembler]
-laser_filters:
-  debian:
-    buster: [ros-noetic-laser-filters]
-pcl_ros:
-  debian:
-    buster: [ros-noetic-pcl-ros]
-tf2_geometry_msgs:
-  debian:
-    buster: [ros-noetic-tf2-geometry-msgs]
-image_rotate:
-  debian:
-    buster: [ros-noetic-image-rotate]
-tf2_kdl:
-  debian:
-    buster: [ros-noetic-tf2-kdl]
-tf2_web_republisher:
-  debian:
-    buster: [ros-noetic-tf2-web-republisher]
-tf_conversions:
-  debian:
-    buster: [ros-noetic-tf-conversions]
-theora_image_transport:
-  debian:
-    buster: [ros-noetic-theora-image-transport]
-turtlesim:
-  debian:
-    buster: [ros-noetic-turtlesim]
-turtle_actionlib:
-  debian:
-    buster: [ros-noetic-turtle-actionlib]
-turtle_tf:
-  debian:
-    buster: [ros-noetic-turtle-tf]
-turtle_tf2:
-  debian:
-    buster: [ros-noetic-turtle-tf2]
-urdf:
-  debian:
-    buster: [ros-noetic-urdf]
-kdl_parser:
-  debian:
-    buster: [ros-noetic-kdl-parser]
-kdl_parser_py:
-  debian:
-    buster: [ros-noetic-kdl-parser-py]
-mavros_extras:
-  debian:
-    buster: [ros-noetic-mavros-extras]
-robot_state_publisher:
-  debian:
-    buster: [ros-noetic-robot-state-publisher]
-rviz:
-  debian:
-    buster: [ros-noetic-rviz]
-librviz_tutorial:
-  debian:
-    buster: [ros-noetic-librviz-tutorial]
-rqt_rviz:
-  debian:
-    buster: [ros-noetic-rqt-rviz]
-rviz_plugin_tutorials:
-  debian:
-    buster: [ros-noetic-rviz-plugin-tutorials]
-rviz_python_tutorial:
-  debian:
-    buster: [ros-noetic-rviz-python-tutorial]
-urdf_tutorial:
-  debian:
-    buster: [ros-noetic-urdf-tutorial]
-usb_cam:
-  debian:
-    buster: [ros-noetic-usb-cam]
-visualization_marker_tutorials:
-  debian:
-    buster: [ros-noetic-visualization-marker-tutorials]
-vl53l1x:
-  debian:
-    buster: [ros-noetic-vl53l1x]
-xacro:
-  debian:
-    buster: [ros-noetic-xacro]
-ddynamic_reconfigure:
-  debian:
-    buster: [ros-noetic-ddynamic-reconfigure]
-librealsense2:
-  debian:
-    buster: [ros-noetic-librealsense2]
-realsense2_camera:
-  debian:
-    buster: [ros-noetic-realsense2-camera]
-realsense2_description:
-  debian:
-    buster: [ros-noetic-realsense2-description]

builder/image-network.sh

@@ -60,24 +60,4 @@ domain-needed
 quiet-dhcp6
 EOF
 
-echo_stamp "#4 Build the RTL8814AU Wi-Fi adapter driver"
-cd /home/pi
-git clone https://github.com/aircrack-ng/rtl8812au.git --depth=1
-cd rtl8812au
-echo kernel version: $(uname -r)
-echo kernel version from procfs: $(cat /proc/version)
-echo version: $(git describe --tags --always)
-sed -i 's/CONFIG_PLATFORM_I386_PC = y/CONFIG_PLATFORM_I386_PC = n/g' Makefile # https://github.com/aircrack-ng/rtl8812au#for-raspberry-rpi
-sed -i 's/CONFIG_PLATFORM_ARM_RPI = n/CONFIG_PLATFORM_ARM_RPI = y/g' Makefile
-# sed -i 's/CONFIG_PLATFORM_ARM64_RPI = n/CONFIG_PLATFORM_ARM64_RPI = y/g' Makefile
-apt-cache policy raspberrypi-kernel-headers
-apt-get install -y raspberrypi-kernel-headers dkms
-ls /lib/modules
-KERNEL_VERSION=$(cd /lib/modules && echo *-v7l+) # can't use `uname` as it gives incorrect value in emulated environment
-echo make
-make KERNEL_VER=$KERNEL_VERSION KVER=$KERNEL_VERSION
-echo make install
-make install KERNEL_VER=$KERNEL_VERSION KVER=$KERNEL_VERSION
-# TODO: rm -rf /home/pi/rtl8812au
-
 echo_stamp "#4 End of network installation"

builder/image-ros.sh

@@ -49,7 +49,7 @@ echo_stamp() {
 my_travis_retry() {
   local result=0
   local count=1
-  local max_count=5
+  local max_count=50
   while [ $count -le $max_count ]; do
     [ $result -ne 0 ] && {
       echo -e "\nThe command \"$@\" failed. Retrying, $count of $max_count.\n" >&2
@@ -137,8 +137,7 @@ my_travis_retry apt-get install -y --no-install-recommends \
     ros-${ROS_DISTRO}-ws281x \
     ros-${ROS_DISTRO}-rosshow \
     ros-${ROS_DISTRO}-cmake-modules \
-    ros-${ROS_DISTRO}-image-view \
-    ros-${ROS_DISTRO}-image-geometry
+    ros-${ROS_DISTRO}-image-view
 
 # TODO move GeographicLib datasets to Mavros debian package
 echo_stamp "Install GeographicLib datasets (needed for mavros)" \
@@ -152,9 +151,6 @@ catkin_make run_tests #&& catkin_test_results
 echo_stamp "Change permissions for catkin_ws"
 chown -Rf pi:pi /home/pi/catkin_ws
 
-echo_stamp "Update www"
-sudo -u pi sh -c ". devel/setup.sh && rosrun clover www"
-
 echo_stamp "Make \$HOME/examples symlink"
 ln -s "$(catkin_find clover examples --first-only)" /home/pi
 chown -Rf pi:pi /home/pi/examples

builder/image-validate.sh

@@ -37,7 +37,3 @@ apt-cache show openvpn
 
 echo "Move /etc/ld.so.preload back to its original position"
 mv /etc/ld.so.preload.disabled-for-build /etc/ld.so.preload
-
-echo "Largest packages installed"
-sudo -E sh -c 'apt-get install -y debian-goodies'
-dpigs -H -n 100

builder/test/tests.py

@@ -2,7 +2,6 @@
 
 # validate all required modules installed
 
-import os
 import rospy
 from geometry_msgs.msg import PoseStamped
 from sensor_msgs.msg import Range, BatteryState
@@ -23,7 +22,6 @@ from clover.srv import GetTelemetry, Navigate, NavigateGlobal, SetPosition, SetV
 from led_msgs.srv import SetLEDs
 from led_msgs.msg import LEDStateArray, LEDState
 from aruco_pose.msg import Marker, MarkerArray, Point2D
-from clover import long_callback
 
 import dynamic_reconfigure.client
 
@@ -33,15 +31,9 @@ import tf2_geometry_msgs
 import VL53L1X
 import pymavlink
 from pymavlink import mavutil
-from image_geometry import PinholeCameraModel, StereoCameraModel
+import rpi_ws281x
+import pigpio
 # from espeak import espeak
 from pyzbar import pyzbar
-import docopt
-import geopy
-import flask
 
 print(cv2.getBuildInformation())
-
-if not os.environ.get('VM'):
-    import rpi_ws281x
-    import pigpio

builder/test/tests.sh

@@ -6,10 +6,16 @@ set -ex
 
 # validate required software is installed
 
+python --version
 python2 --version
 python3 --version
+ipython --version
 ipython3 --version
 
+# ptvsd does not have a stand-alone binary
+python -m ptvsd --version
+python3 -m ptvsd --version
+
 node -v
 npm -v
 
@@ -19,77 +25,42 @@ lsof -v
 git --version
 vim --version
 pip --version
+pip2 --version
 pip3 --version
 tcpdump --version
 monkey --version
+pigpiod -v
+i2cdetect -V
+butterfly -h
 # espeak --version
+mjpg_streamer --version
 systemctl --version
 
-if [ -z $VM ]; then
-	# rpi only software
-	python --version
-	ipython --version
-	pip2 --version
-	# `python` is python2 for now
-	[[ $(python -c 'import sys;print(sys.version_info.major)') == "2" ]]
-
-	# ptvsd does not have a stand-alone binary
-	python -m ptvsd --version
-	python3 -m ptvsd --version
-
-	pigpiod -v
-	i2cdetect -V
-	butterfly -h
-	mjpg_streamer --version
-fi
-
 # ros stuff
 
 roscore -h
 rosversion clover
 rosversion aruco_pose
+rosversion vl53l1x
 rosversion mavros
 rosversion mavros_extras
 rosversion ws281x
 rosversion led_msgs
 rosversion dynamic_reconfigure
 rosversion tf2_web_republisher
-rosversion rosbridge_server
+rosversion compressed_image_transport
+rosversion rosbridge_suite
+rosversion rosserial
 rosversion usb_cam
 rosversion cv_camera
 rosversion web_video_server
+rosversion rosshow
 rosversion nodelet
 rosversion image_view
 
-[[ $(rosversion ws281x) == "0.0.13" ]]
-
-if [ -z $VM ]; then
-	rosversion compressed_image_transport
-	rosversion rosshow
-	rosversion vl53l1x
-	rosversion rosserial
-	[[ $(rosversion cv_camera) == "0.5.1" ]] # patched version with init fix
-fi
-
-# determine user home directory
-[ $VM ] && H="/home/clover" || H="/home/pi"
-
-# test basic ros tool work
-source $H/catkin_ws/devel/setup.bash
-roscd
-rosrun
-rosmsg
-rossrv
-rosnode || [ $? -eq 64 ] # usage output code is 64
-rostopic || [ $? -eq 64 ]
-rosservice || [ $? -eq 64 ]
-rosparam
-roslaunch -h
+# validate some versions
+[[ $(rosversion cv_camera) == "0.5.1" ]] # patched version with init fix
+[[ $(rosversion ws281x) == "0.0.12" ]]
 
 # validate examples are present
-[[ $(ls $H/examples/*) ]]
-
-# validate web tools present
-[ -d $H/.ros/www ]
-[ "$(readlink $H/.ros/www/clover)" = "$H/catkin_ws/src/clover/clover/www" ]
-[ "$(readlink $H/.ros/www/clover_blocks)" = "$H/catkin_ws/src/clover/clover_blocks/www" ]
+[[ $(ls /home/pi/examples/*) ]]

clover/CMakeLists.txt

@@ -4,6 +4,8 @@ project(clover)
 ## Compile as C++11, supported in ROS Kinetic and newer
 add_compile_options(-std=c++11)
 
+add_compile_options(-Wall -Wextra -Werror)
+
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
@@ -17,7 +19,6 @@ find_package(catkin REQUIRED COMPONENTS
   message_generation
   geometry_msgs
   sensor_msgs
-  led_msgs
   geographic_msgs
   tf
   tf2
@@ -53,7 +54,7 @@ find_package(OpenCV ${_opencv_version} REQUIRED
 ## Uncomment this if the package has a setup.py. This macro ensures
 ## modules and global scripts declared therein get installed
 ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
-catkin_python_setup()
+# catkin_python_setup()
 
 ################################################
 ## Declare ROS messages, services and actions ##
@@ -80,10 +81,11 @@ catkin_python_setup()
 ##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
 
 ## Generate messages in the 'msg' folder
-add_message_files(
-  FILES
-  State.msg
-)
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
 
 ## Generate services in the 'srv' folder
 add_service_files(
@@ -91,9 +93,6 @@ add_service_files(
   GetTelemetry.srv
   Navigate.srv
   NavigateGlobal.srv
-  SetAltitude.srv
-  SetYaw.srv
-  SetYawRate.srv
   SetPosition.srv
   SetVelocity.srv
   SetAttitude.srv
@@ -308,5 +307,4 @@ endif()
 if (CATKIN_ENABLE_TESTING)
   find_package(rostest REQUIRED)
   add_rostest(test/basic.test)
-  add_rostest(test/offboard.test)
 endif()

clover/examples/camera.py

@@ -1,64 +0,0 @@
-# Information: https://clover.coex.tech/camera
-
-# Example on basic working with the camera and image processing:
-
-# - cuts out a central square from the camera image;
-# - publishes this cropped image to the topic `/cv/center`;
-# - computes the average color of it;
-# - prints its name to the console.
-
-import rospy
-import cv2
-from sensor_msgs.msg import Image
-from cv_bridge import CvBridge
-from clover import long_callback
-
-rospy.init_node('cv')
-bridge = CvBridge()
-
-printed_color = None
-center_pub = rospy.Publisher('~center', Image, queue_size=1)
-
-def get_color_name(h):
-    if h < 15: return 'red'
-    elif h < 30: return 'orange'
-    elif h < 60: return 'yellow'
-    elif h < 90: return 'green'
-    elif h < 120: return 'cyan'
-    elif h < 150: return 'blue'
-    elif h < 170: return 'magenta'
-    else: return 'red'
-
-
-@long_callback
-def image_callback(msg):
-    img = bridge.imgmsg_to_cv2(msg, 'bgr8')
-
-    # convert to HSV to work with color hue
-    img_hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
-
-    # cut out a central square
-    w = img.shape[1]
-    h = img.shape[0]
-    r = 20
-    center = img_hsv[h // 2 - r:h // 2 + r, w // 2 - r:w // 2 + r]
-
-    # compute and print the average hue
-    mean_hue = center[:, :, 0].mean()
-    color = get_color_name(mean_hue)
-    global printed_color
-    if color != printed_color:
-        print(color)
-        printed_color = color
-
-    # publish the cropped image
-    center = cv2.cvtColor(center, cv2.COLOR_HSV2BGR)
-    center_pub.publish(bridge.cv2_to_imgmsg(center, 'bgr8'))
-
-# process every frame:
-image_sub = rospy.Subscriber('main_camera/image_raw', Image, image_callback, queue_size=1)
-
-# process 5 frames per second:
-# image_sub = rospy.Subscriber('main_camera/image_raw_throttled', Image, image_callback, queue_size=1)
-
-rospy.spin()

clover/examples/navigate_wait.py

@@ -16,8 +16,11 @@ set_attitude = rospy.ServiceProxy('set_attitude', srv.SetAttitude)
 set_rates = rospy.ServiceProxy('set_rates', srv.SetRates)
 land = rospy.ServiceProxy('land', Trigger)
 
-def navigate_wait(x=0, y=0, z=0, yaw=math.nan, speed=0.5, frame_id='body', tolerance=0.2, auto_arm=False):
-    res = navigate(x=x, y=y, z=z, yaw=yaw, speed=speed, frame_id=frame_id, auto_arm=auto_arm)
+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

clover/examples/red_circle.py

@@ -1,97 +0,0 @@
-# This example makes the drone find and follow the red circle.
-# To test in the simulator, place 'Red Circle' model on the floor.
-# More information: https://clover.coex.tech/red_circle
-
-# Input topic: main_camera/image_raw (camera image)
-# Output topics:
-#   cv/mask (red color mask)
-#   cv/red_circle (position of the center of the red circle in 3D space)
-
-import rospy
-import cv2
-import numpy as np
-from math import nan
-from sensor_msgs.msg import Image, CameraInfo
-from geometry_msgs.msg import PointStamped, Point
-from cv_bridge import CvBridge
-from clover import long_callback, srv
-import tf2_ros
-import tf2_geometry_msgs
-
-rospy.init_node('cv', disable_signals=True) # disable signals to allow interrupting with ctrl+c
-
-get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry)
-set_position = rospy.ServiceProxy('set_position', srv.SetPosition)
-
-bridge = CvBridge()
-
-tf_buffer = tf2_ros.Buffer()
-tf_listener = tf2_ros.TransformListener(tf_buffer)
-
-mask_pub = rospy.Publisher('~mask', Image, queue_size=1)
-point_pub = rospy.Publisher('~red_circle', PointStamped, queue_size=1)
-
-# read camera info
-camera_info = rospy.wait_for_message('main_camera/camera_info', CameraInfo)
-camera_matrix = np.float64(camera_info.K).reshape(3, 3)
-distortion = np.float64(camera_info.D).flatten()
-
-
-def img_xy_to_point(xy, dist):
-    xy = cv2.undistortPoints(xy, camera_matrix, distortion, P=camera_matrix)[0][0]
-
-    # Shift points to center
-    xy -= camera_info.width // 2, camera_info.height // 2
-
-    fx = camera_matrix[0, 0]
-    fy = camera_matrix[1, 1]
-
-    return Point(x=xy[0] * dist / fx, y=xy[1] * dist / fy, z=dist)
-
-def get_center_of_mass(mask):
-    M = cv2.moments(mask)
-    if M['m00'] == 0:
-        return None
-    return M['m10'] // M['m00'], M['m01'] // M['m00']
-
-follow_red_circle = False
-
-@long_callback
-def image_callback(msg):
-    img = bridge.imgmsg_to_cv2(msg, 'bgr8')
-    img_hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
-
-    # we need to use two ranges for red color
-    mask1 = cv2.inRange(img_hsv, (0, 150, 150), (15, 255, 255))
-    mask2 = cv2.inRange(img_hsv, (160, 150, 150), (180, 255, 255))
-
-    # combine two masks using bitwise OR
-    mask = cv2.bitwise_or(mask1, mask2)
-
-    # publish the mask
-    if mask_pub.get_num_connections() > 0:
-        mask_pub.publish(bridge.cv2_to_imgmsg(mask, 'mono8'))
-
-    # calculate x and y of the circle
-    xy = get_center_of_mass(mask)
-    if xy is None:
-        return
-
-    # calculate and publish the position of the circle in 3D space
-    altitude = get_telemetry('terrain').z
-    xy3d = img_xy_to_point(xy, altitude)
-    target = PointStamped(header=msg.header, point=xy3d)
-    point_pub.publish(target)
-
-    if follow_red_circle:
-        # follow the target
-        setpoint = tf_buffer.transform(target, 'map', timeout=rospy.Duration(0.2))
-        set_position(x=setpoint.point.x, y=setpoint.point.y, z=nan, yaw=nan, frame_id=setpoint.header.frame_id)
-
-# process each camera frame:
-image_sub = rospy.Subscriber('main_camera/image_raw', Image, image_callback, queue_size=1)
-
-rospy.loginfo('Hit enter to follow the red circle')
-input()
-follow_red_circle = True
-rospy.spin()

clover/launch/aruco.launch

@@ -18,9 +18,8 @@
         <remap from="map_markers" to="aruco_map/map"/>
         <param name="estimate_poses" value="true"/>
         <param name="send_tf" value="true"/>
-        <param name="use_map_markers" value="true"/>
-        <param name="known_vertical" value="map" if="$(eval placement == 'floor' or placement == 'ceiling')"/>
-        <param name="flip_vertical" value="true" if="$(eval placement == 'ceiling')"/>
+        <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 -->
@@ -36,8 +35,8 @@
         <remap from="camera_info" to="main_camera/camera_info"/>
         <remap from="markers" to="aruco_detect/markers"/>
         <param name="map" value="$(find aruco_pose)/map/$(arg map)"/>
-        <param name="known_vertical" value="map" if="$(eval placement == 'floor' or placement == 'ceiling')"/>
-        <param name="flip_vertical" value="true" if="$(eval placement == 'ceiling')"/>
+        <param name="known_tilt" value="map" if="$(eval placement == 'floor')"/>
+        <param name="known_tilt" value="map_flipped" if="$(eval placement == 'ceiling')"/>
         <param name="image_axis" value="true"/>
         <param name="frame_id" value="aruco_map_detected" if="$(arg aruco_vpe)"/>
         <param name="frame_id" value="aruco_map" unless="$(arg aruco_vpe)"/>

clover/launch/clover.launch

@@ -12,7 +12,7 @@
     <arg name="led" default="true"/>
     <arg name="blocks" default="false"/>
     <arg name="rc" default="false"/>
-    <arg name="force_init" default="true"/> <!-- force estimator to init by publishing zero pose -->
+    <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 -->
 
@@ -45,13 +45,13 @@
         <remap from="camera_info" to="main_camera/camera_info"/>
         <param name="calc_flow_gyro" value="true"/>
         <param name="roi_rad" value="0.8"/>
-        <param name="disable_on_vpe" value="true"/>
     </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"/>
+
     <!-- simplified offboard control -->
     <node name="simple_offboard" pkg="clover" type="simple_offboard" output="screen" clear_params="true">
         <param name="reference_frames/main_camera_optical" value="map"/>
-        <param name="terrain_frame_mode" value="range"/>
     </node>
 
     <!-- main camera -->
@@ -72,9 +72,6 @@
         <param name="pass_statuses" type="yaml" value="[0, 6, 7, 11]"/>
     </node>
 
-    <!-- rangefinder's frame -->
-    <node pkg="tf2_ros" type="static_transform_publisher" name="rangefinder_frame" args="0 0 -0.05 0 1.5707963268 0 base_link rangefinder" if="$(arg rangefinder_vl53l1x)"/>
-
     <!-- led strip -->
     <include file="$(find clover)/launch/led.launch" if="$(arg led)">
         <arg name="simulator" value="$(arg simulator)"/>
@@ -89,4 +86,8 @@
         <param name="use_fake_gcs" value="false"/>
     </node>
 
+    <!-- Update static directory -->
+    <node pkg="roswww_static" name="roswww_static" type="main.py" clear_params="true">
+        <param name="default_package" value="clover"/>
+    </node>
 </launch>

clover/launch/main_camera.launch

@@ -4,8 +4,6 @@
     <arg name="direction_z" default="down"/> <!-- direction the camera points: down, up -->
     <arg name="direction_y" default="backward"/> <!-- direction the camera cable points: backward, forward -->
     <arg name="device" default="/dev/video0"/> <!-- v4l2 device -->
-    <arg name="throttled_topic" default="true"/> <!-- enable throttled image topic -->
-    <arg name="throttled_topic_rate" default="5.0"/> <!-- throttled image topic rate -->
     <arg name="simulator" default="false"/>
 
     <node if="$(eval direction_z == 'down' and direction_y == 'backward')" 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"/>
@@ -45,8 +43,4 @@
     <node pkg="clover" type="camera_markers" ns="main_camera" name="main_camera_markers">
         <param name="scale" value="3.0"/>
     </node>
-
-    <!-- image topic throttled -->
-    <node pkg="topic_tools" name="main_camera_throttle" type="throttle" ns="main_camera"
-        args="messages image_raw $(arg throttled_topic_rate) image_raw_throttled" if="$(arg throttled_topic)"/>
 </launch>

clover/launch/mavros.launch

@@ -1,5 +1,5 @@
 <launch>
-    <arg name="fcu_conn" default="usb"/> <!-- options: usb, uart, tcp, udp, sitl, hitl -->
+    <arg name="fcu_conn" default="usb"/> <!-- options: usb, uart, tcp, udp, sitl -->
     <arg name="fcu_ip" default="127.0.0.1"/>
     <arg name="fcu_sys_id" default="1"/>
     <arg name="gcs_bridge" default="tcp"/>
@@ -23,9 +23,6 @@
         <!-- sitl since PX4 1.9.0 -->
         <param name="fcu_url" value="udp://@$(arg fcu_ip):14580" if="$(eval fcu_conn == 'sitl')"/>
 
-        <!-- hitl connection (to gazebo_mavlink_interface plugin) -->
-        <param name="fcu_url" value="udp://$(arg fcu_ip):14540@" if="$(eval fcu_conn == 'hitl')"/>
-
         <!-- set target_system_id -->
         <param name="target_system_id" value="$(arg fcu_sys_id)" />
 
@@ -77,6 +74,9 @@
         covariance: 1  # cm
     </rosparam>
 
+    <!-- Rangefinders frame -->
+    <node pkg="tf2_ros" type="static_transform_publisher" name="rangefinder_frame" args="0 0 -0.05 0 1.5707963268 0 base_link rangefinder"/>
+
     <!-- Copter visualization -->
     <node name="visualization" pkg="mavros_extras" type="visualization" if="$(arg viz)">
         <remap to="mavros/local_position/pose" from="local_position"/>

clover/launch/simulator.launch

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

clover/msg/State.msg

@@ -1,38 +0,0 @@
-uint8 MODE_NONE = 0
-uint8 MODE_NAVIGATE = 1
-uint8 MODE_NAVIGATE_GLOBAL = 2
-uint8 MODE_POSITION = 3
-uint8 MODE_VELOCITY = 4
-uint8 MODE_ATTITUDE = 5
-uint8 MODE_RATES = 6
-
-uint8 YAW_MODE_YAW = 0
-uint8 YAW_MODE_YAW_RATE = 1
-uint8 YAW_MODE_YAW_TOWARDS = 2
-
-# type of offboard control
-uint8 mode
-uint8 yaw_mode
-
-# targets
-float32 x
-float32 y
-float32 z
-float32 speed
-float32 lat
-float32 lon
-float32 vx
-float32 vy
-float32 vz
-float32 roll
-float32 pitch
-float32 yaw
-float32 roll_rate
-float32 pitch_rate
-float32 yaw_rate
-float32 thrust
-
-# frames of reference
-string xy_frame_id
-string z_frame_id
-string yaw_frame_id

clover/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>clover</name>
-  <version>0.24.0</version>
+  <version>0.23.0</version>
   <description>The Clover package</description>
 
   <maintainer email="okalachev@gmail.com">Oleg Kalachev</maintainer>
@@ -37,13 +37,12 @@
   <depend>rosbridge_server</depend>
   <depend>web_video_server</depend>
   <depend>tf2_web_republisher</depend>
-  <depend>libxml2</depend>
-  <depend>libxslt</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>
-  <test_depend>ros_pytest</test_depend>
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>

clover/requirements.txt

@@ -1,4 +1,5 @@
-flask
-geopy
-smbus2
-VL53L1X
+flask==1.1.1
+docopt==0.6.2
+geopy==1.11.0
+smbus2==0.3.0
+VL53L1X==0.0.5

clover/setup.py

@@ -1,11 +0,0 @@
-## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD
-
-from distutils.core import setup
-from catkin_pkg.python_setup import generate_distutils_setup
-
-# fetch values from package.xml
-setup_args = generate_distutils_setup(
-    packages=['clover'],
-    package_dir={'': 'src'})
-
-setup(**setup_args)

clover/src/autotest/autotest_aruco.py

@@ -13,12 +13,7 @@ from util import handle_response
 
 rospy.init_node('autotest_aruco', disable_signals=True) # disable signals to allow interrupting with ctrl+c
 
-try:
-    flow_client = dynamic_reconfigure.client.Client('optical_flow', timeout=2)
-except rospy.ROSException:
-    flow_client = None
-    print('Cannot configure optical flow, skip')
-
+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))
@@ -35,8 +30,11 @@ def print_current_map_position():
     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=math.nan, speed=0.5, frame_id='body', tolerance=0.2, auto_arm=False):
-    res = navigate(x=x, y=y, z=z, yaw=yaw, speed=speed, frame_id=frame_id, auto_arm=auto_arm)
+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
@@ -69,13 +67,12 @@ 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()
 
-if flow_client:
-    input('Disable optical flow and keep hovering [enter] ')
-    flow_client.update_configuration({'enabled': False})
-    rospy.sleep(5)
+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('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')

clover/src/autotest/autotest_flight.py

@@ -2,7 +2,7 @@
 
 import rospy
 import math
-from math import nan, inf
+from math import nan
 import signal
 import sys
 from clover import srv
@@ -15,8 +15,6 @@ rospy.init_node('autotest_flight', disable_signals=True) # disable signals to al
 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_yaw = handle_response(rospy.ServiceProxy('set_yaw', srv.SetYaw))
-set_yaw_rate = handle_response(rospy.ServiceProxy('set_yaw_rate', srv.SetYawRate))
 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))
@@ -30,8 +28,11 @@ def interrupt(sig, frame):
 
 signal.signal(signal.SIGINT, interrupt)
 
-def navigate_wait(x=0, y=0, z=0, yaw=nan, speed=0.5, frame_id='body', tolerance=0.2, auto_arm=False):
-    res = navigate(x=x, y=y, z=z, yaw=yaw, speed=speed, frame_id=frame_id, auto_arm=auto_arm)
+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
@@ -68,17 +69,17 @@ set_velocity(vx=1, vy=0.0, vz=0, frame_id='body')
 rospy.sleep(2)
 set_position(frame_id='body')
 
-input('Rotate right 90° using set_yaw [enter] ')
-set_yaw(yaw=-math.pi / 2, frame_id='navigate_target')
+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(roll=0, pitch=-0.3, yaw=0, thrust=0.5, frame_id='body')
+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(roll=0.3, pitch=0, yaw=0, thrust=0.5, frame_id='body')
+set_attitude(pitch=0, roll=0.3, yaw=0, thrust=0.5, frame_id='body')
 rospy.sleep(0.5)
 set_position(frame_id='body')
 
@@ -87,13 +88,13 @@ 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 set_yaw_rate [enter]')
-set_yaw_rate(yaw_rate=-1)
+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 heading forward [enter]')
-navigate_wait(x=start.x, y=start.y, z=start.z, yaw=inf, speed=1, frame_id='map')
+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/clover/__init__.py

@@ -1,35 +0,0 @@
-import rospy
-from threading import Thread, Event
-
-def long_callback(fn):
-    """
-    Decorator fixing a rospy issue for long-running topic callbacks, primarily
-    for image processing.
-
-    See: https://github.com/ros/ros_comm/issues/1901.
-
-    Usage example:
-
-    @long_callback
-    def image_callback(msg):
-        # perform image processing
-        # ...
-
-    rospy.Subscriber('main_camera/image_raw', Image, image_callback, queue_size=1)
-    """
-    e = Event()
-
-    def thread():
-        while not rospy.is_shutdown():
-            e.wait()
-            e.clear()
-            fn(thread.current_msg)
-
-    thread.current_msg = None
-    Thread(target=thread, daemon=True).start()
-
-    def wrapper(msg):
-        thread.current_msg = msg
-        e.set()
-
-    return wrapper

clover/src/led.cpp

@@ -31,6 +31,7 @@ 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;
@@ -86,8 +87,9 @@ void proceed(const ros::TimerEvent& event)
 	set_leds.request.leds.resize(led_count);
 
 	if (current_effect.effect == "blink" || current_effect.effect == "blink_fast") {
-		// enable on odd counter
-		if (counter % 2 != 0) {
+		blink_state = !blink_state;
+		// toggle all leds
+		if (blink_state) {
 			fill(current_effect.r, current_effect.g, current_effect.b);
 		} else {
 			fill(0, 0, 0);
@@ -220,7 +222,6 @@ 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;
 }
@@ -319,8 +320,8 @@ int main(int argc, char **argv)
 
 	auto set_effect = nh.advertiseService("set_effect", &setEffect);
 
-	auto mavros_state_sub = nh.subscribe("mavros/state", 1, &handleMavrosState);
-	auto battery_sub = nh.subscribe("mavros/battery", 1, &handleBattery);
+	auto mavros_state_sub = nh.subscribe("/mavros/state", 1, &handleMavrosState);
+	auto battery_sub = nh.subscribe("/mavros/battery", 1, &handleBattery);
 	auto rosout_sub = nh.subscribe("/rosout_agg", 1, &handleLog);
 
 	timer = nh.createTimer(ros::Duration(0), &proceed, false, false);

clover/src/optical_flow.cpp

@@ -25,7 +25,6 @@
 #include <dynamic_reconfigure/server.h>
 #include <mavros_msgs/OpticalFlowRad.h>
 #include <sensor_msgs/Imu.h>
-#include <geometry_msgs/PoseStamped.h>
 #include <geometry_msgs/Vector3Stamped.h>
 #include <geometry_msgs/PointStamped.h>
 #include <geometry_msgs/TwistStamped.h>
@@ -58,9 +57,6 @@ private:
 	std::unique_ptr<tf2_ros::TransformListener> tf_listener_;
 	bool calc_flow_gyro_;
 	float flow_gyro_default_;
-	bool disable_on_vpe_;
-	ros::Subscriber vpe_sub_;
-	ros::Time last_vpe_time_;
 	std::shared_ptr<dynamic_reconfigure::Server<clover::FlowConfig>> dyn_srv_;
 
 	void onInit()
@@ -91,11 +87,6 @@ private:
 
 		img_sub_ = it.subscribeCamera("image_raw", 1, &OpticalFlow::flow, this);
 
-		disable_on_vpe_ = nh_priv.param("disable_on_vpe", false);
-		if (disable_on_vpe_) {
-			vpe_sub_ = nh.subscribe("mavros/vision_pose/pose", 1, &OpticalFlow::vpeCallback, this);
-		}
-
 		dyn_srv_ = std::make_shared<dynamic_reconfigure::Server<clover::FlowConfig>>(nh_priv);
 		dynamic_reconfigure::Server<clover::FlowConfig>::CallbackType cb;
 
@@ -130,12 +121,6 @@ private:
 	{
 		if (!enabled_) return;
 
-		if (disable_on_vpe_ &&
-			!last_vpe_time_.isZero() &&
-			(msg->header.stamp - last_vpe_time_).toSec() < 0.1) {
-			return;
-		}
-
 		parseCameraInfo(cinfo);
 
 		auto img = cv_bridge::toCvShare(msg, "mono8")->image;
@@ -169,7 +154,7 @@ private:
 
 		img.convertTo(curr_, CV_32F);
 
-		if (prev_.empty() || (msg->header.stamp - prev_stamp_).toSec() > 0.1) { // outdated previous frame
+		if (prev_.empty()) {
 			prev_ = curr_.clone();
 			prev_stamp_ = msg->header.stamp;
 			cv::createHanningWindow(hann_, curr_.size(), CV_32F);
@@ -251,15 +236,6 @@ private:
 			prev_ = curr_.clone();
 			prev_stamp_ = msg->header.stamp;
 
-			// Publish estimated angular velocity
-			geometry_msgs::TwistStamped velo;
-			velo.header.stamp = msg->header.stamp;
-			velo.header.frame_id = fcu_frame_id_;
-			velo.twist.angular.x = flow_fcu.vector.x / integration_time.toSec();
-			velo.twist.angular.y = flow_fcu.vector.y / integration_time.toSec();
-			velo_pub_.publish(velo);
-
-publish_debug:
 			// Publish debug image
 			if (img_pub_.getNumSubscribers() > 0) {
 				// publish debug image
@@ -271,6 +247,14 @@ publish_debug:
 				out_msg.image = img;
 				img_pub_.publish(out_msg.toImageMsg());
 			}
+
+			// Publish estimated angular velocity
+			geometry_msgs::TwistStamped velo;
+			velo.header.stamp = msg->header.stamp;
+			velo.header.frame_id = fcu_frame_id_;
+			velo.twist.angular.x = flow_fcu.vector.x / integration_time.toSec();
+			velo.twist.angular.y = flow_fcu.vector.y / integration_time.toSec();
+			velo_pub_.publish(velo);
 		}
 	}
 
@@ -292,17 +276,13 @@ publish_debug:
 		return flow;
 	}
 
-	void paramCallback(clover::FlowConfig &config, uint32_t level)
+	void paramCallback(clover::FlowConfig &config, [[maybe_unused]] uint32_t level)
 	{
 		enabled_ = config.enabled;
 		if (!enabled_) {
 			prev_ = Mat(); // clear previous frame
 		}
 	}
-
-	void vpeCallback(const geometry_msgs::PoseStamped& vpe) {
-		last_vpe_time_ = vpe.header.stamp;
-	}
 };
 
 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 heartbeats.
+		// if there is no GCS hearbeats.
 		// TODO: use timer
 		// TODO: remove, when PX4 get this fixed.
 		ros::Publisher mavlink_pub = nh.advertise<mavros_msgs::Mavlink>("mavlink/to", 1);

clover/src/selfcheck.py

@@ -9,14 +9,13 @@
 # The above copyright notice and this permission notice shall be included in all
 # copies or substantial portions of the Software.
 
-import os, sys
+import os
 import math
 import subprocess
 import re
 from collections import OrderedDict
 import traceback
-import threading
-from threading import Event, Thread, Lock
+from threading import Event
 import numpy
 import rospy
 import tf2_ros
@@ -28,16 +27,24 @@ from mavros_msgs.msg import State, OpticalFlowRad, Mavlink
 from mavros_msgs.srv import ParamGet
 from geometry_msgs.msg import PoseStamped, TwistStamped, PoseWithCovarianceStamped, Vector3Stamped
 from visualization_msgs.msg import MarkerArray as VisualizationMarkerArray
-from diagnostic_msgs.msg import DiagnosticArray
 import tf.transformations as t
 from aruco_pose.msg import MarkerArray
 from mavros import mavlink
 import locale
 
 
+# TODO: check attitude is present
+# TODO: disk free space
+# TODO: map, base_link, body
+# TODO: rc service
+# TODO: perform commander check, ekf2 status on PX4
+# TODO: check if FCU params setter succeed
+# TODO: selfcheck ROS service (with blacklists for checks)
+
+
 rospy.init_node('selfcheck')
 
-os.environ['ROSCONSOLE_FORMAT']='${message}'
+os.environ['ROSCONSOLE_FORMAT']='[${severity}]: ${message}'
 
 # use user's locale to convert numbers, etc
 locale.setlocale(locale.LC_ALL, '')
@@ -46,68 +53,46 @@ tf_buffer = tf2_ros.Buffer()
 tf_listener = tf2_ros.TransformListener(tf_buffer)
 
 
-thread_local = threading.local()
-reports_lock = Lock()
-
-
-# formatting colors
-if sys.stdout.isatty():
-    GREY = '\033[90m'
-    GREEN = '\033[92m'
-    RED = '\033[31m'
-    END = '\033[0m'
-else:
-    GREY = GREEN = RED = END = ''
+failures = []
+infos = []
+current_check = None
 
 
 def failure(text, *args):
     msg = text % args
-    thread_local.reports += [{'failure': msg}]
+    rospy.logwarn('%s: %s', current_check, msg)
+    failures.append(msg)
 
 
 def info(text, *args):
     msg = text % args
-    thread_local.reports += [{'info': msg}]
+    rospy.loginfo('%s: %s', current_check, msg)
+    infos.append(msg)
 
 
 def check(name):
     def inner(fn):
         def wrapper(*args, **kwargs):
-            start = rospy.get_time()
-            thread_local.reports = []
+            failures[:] = []
+            infos[:] = []
+            global current_check
+            current_check = name
             try:
                 fn(*args, **kwargs)
             except Exception as e:
                 traceback.print_exc()
                 rospy.logerr('%s: exception occurred', name)
-            with reports_lock:
-                for report in thread_local.reports:
-                    if 'failure' in report:
-                        rospy.logerr('%s: %s', name, report['failure'])
-                    elif 'info' in report:
-                        rospy.loginfo(GREY + name + END + ': ' + report['info'])
-                if not thread_local.reports:
-                    rospy.loginfo(GREY + name + END + ': ' + GREEN + 'OK' + END)
-                if rospy.get_param('~time', False):
-                    rospy.loginfo('%s: %.1f sec', name, rospy.get_time() - start)
+                return
+            if not failures and not infos:
+                rospy.loginfo('%s: OK', name)
         return wrapper
     return inner
 
 
-def ff(value, precision=2):
-    # safely format float or int
-    if value is None:
-        return RED + '???' + END
-    if isinstance(value, float):
-        return ('{:.' + str(precision + 1) + '}').format(value)
-    elif isinstance(value, int):
-        return str(value)
-
-
 param_get = rospy.ServiceProxy('mavros/param/get', ParamGet)
 
 
-def get_param(name, default=None):
+def get_param(name):
     try:
         res = param_get(param_id=name)
     except rospy.ServiceException as e:
@@ -116,17 +101,12 @@ def get_param(name, default=None):
 
     if not res.success:
         failure('unable to retrieve PX4 parameter %s', name)
-        return default
     else:
         if res.value.integer != 0:
             return res.value.integer
         return res.value.real
 
 
-def get_paramf(name, precision=2):
-    return ff(get_param(name), precision)
-
-
 recv_event = Event()
 link = mavutil.mavlink.MAVLink('', 255, 1)
 mavlink_pub = rospy.Publisher('mavlink/to', Mavlink, queue_size=1)
@@ -171,24 +151,6 @@ def mavlink_exec(cmd, timeout=3.0):
     return mavlink_recv
 
 
-def read_diagnostics(name, key):
-    e = Event()
-    def cb(msg):
-        for status in msg.status:
-            if status.name.lower() == name.lower():
-                for value in status.values:
-                    if value.key.lower() == key.lower():
-                        cb.value = value.value
-                        e.set()
-                        return
-
-    cb.value = None
-    sub = rospy.Subscriber('/diagnostics', DiagnosticArray, cb)
-    e.wait(1.0)  # wait to read all the diagnostics from nodes publishing them
-    sub.unregister()
-    return cb.value
-
-
 BOARD_ROTATIONS = {
     0: 'no rotation',
     1: 'yaw 45°',
@@ -234,31 +196,29 @@ def check_fcu():
         state = rospy.wait_for_message('mavros/state', State, timeout=3)
         if not state.connected:
             failure('no connection to the FCU (check wiring)')
-            info('fcu_url = %s', rospy.get_param('mavros/fcu_url', '?'))
             return
 
-        if not is_process_running('px4', exact=True): # can't use px4 console in SITL
-            clover_tag = re.compile(r'-cl[oe]ver\.\d+$')
-            clover_fw = False
+        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')
+        # 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')
 
-            for line in version_str.split('\n'):
-                if line.startswith('FW version: '):
-                    info(line[len('FW version: '):])
-                elif line.startswith('FW git tag: '): # only Clover's firmware
-                    tag = line[len('FW git tag: '):]
-                    clover_fw = clover_tag.search(tag)
-                    info(tag)
-                elif line.startswith('HW arch: '):
-                    info(line[len('HW arch: '):])
+        for line in version_str.split('\n'):
+            if line.startswith('FW version: '):
+                info(line[len('FW version: '):])
+            elif line.startswith('FW git tag: '): # only Clover's firmware
+                tag = line[len('FW git tag: '):]
+                clover_fw = clover_tag.search(tag)
+                info(tag)
+            elif line.startswith('HW arch: '):
+                info(line[len('HW arch: '):])
 
-            if not clover_fw:
-                info('not Clover PX4 firmware, check https://clover.coex.tech/firmware')
+        if not clover_fw:
+            info('not Clover PX4 firmware, check https://clover.coex.tech/firmware')
 
         est = get_param('SYS_MC_EST_GROUP')
         if est == 1:
@@ -295,29 +255,21 @@ def check_fcu():
         if cbrk_usb_chk != 197848:
             failure('set parameter CBRK_USB_CHK to 197848 for flying with USB connected')
 
-        if not is_process_running('px4', exact=True): # skip battery check in SITL
-            try:
-                battery = rospy.wait_for_message('mavros/battery', BatteryState, timeout=3)
-                if not battery.cell_voltage:
-                    failure('cell voltage is not available, https://clover.coex.tech/power')
-                else:
-                    cell = battery.cell_voltage[0]
-                    if cell > 4.3 or cell < 3.0:
-                        failure('incorrect cell voltage: %.2f V, https://clover.coex.tech/power', cell)
-                    elif cell < 3.7:
-                        failure('critically low cell voltage: %.2f V, recharge battery', cell)
-            except rospy.ROSException:
-                failure('no battery state')
-
-        # time sync check
         try:
-            info('time sync offset: %.2f s', float(read_diagnostics('mavros: Time Sync', 'Estimated time offset (s)')))
-        except:
-            failure('cannot read time sync offset')
+            battery = rospy.wait_for_message('mavros/battery', BatteryState, timeout=3)
+            if not battery.cell_voltage:
+                failure('cell voltage is not available, https://clover.coex.tech/power')
+            else:
+                cell = battery.cell_voltage[0]
+                if cell > 4.3 or cell < 3.0:
+                    failure('incorrect cell voltage: %.2f V, https://clover.coex.tech/power', cell)
+                elif cell < 3.7:
+                    failure('critically low cell voltage: %.2f V, recharge battery', cell)
+        except rospy.ROSException:
+            failure('no battery state')
 
     except rospy.ROSException:
         failure('no MAVROS state (check wiring)')
-        info('fcu_url = %s', rospy.get_param('mavros/fcu_url', '?'))
 
 
 def describe_direction(v):
@@ -384,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 command line (including arguments) to match
+            args.append('-f')  # use full process name to match
         args.append(binary)
         subprocess.check_output(args)
         return True
@@ -394,24 +346,19 @@ def is_process_running(binary, exact=False, full=False):
 
 @check('ArUco markers')
 def check_aruco():
-    markers = None
-
     if is_process_running('aruco_detect', full=True):
         try:
-            info('aruco_detect/length = %g m', rospy.get_param('aruco_detect/length', '?'))
+            info('aruco_detect/length = %g m', rospy.get_param('aruco_detect/length'))
         except KeyError:
             failure('aruco_detect/length parameter is not set')
-        known_vertical = rospy.get_param('aruco_detect/known_vertical', '')
-        flip_vertical = rospy.get_param('aruco_detect/flip_vertical', False)
-        description = ''
-        if known_vertical == 'map' and not flip_vertical:
-            description = ' (all markers are on the floor)'
-        elif known_vertical == 'map' and flip_vertical:
-            description = ' (all markers are on the ceiling)'
-        info('aruco_detect/known_vertical = %s', known_vertical)
-        info('aruco_detect/flip_vertical = %s%s', flip_vertical, description)
+        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:
-            markers = rospy.wait_for_message('aruco_detect/markers', MarkerArray, timeout=0.8)
+            rospy.wait_for_message('aruco_detect/markers', MarkerArray, timeout=1)
         except rospy.ROSException:
             failure('no markers detection')
             return
@@ -420,61 +367,42 @@ def check_aruco():
         return
 
     if is_process_running('aruco_map', full=True):
-        known_vertical = rospy.get_param('aruco_map/known_vertical', '')
-        flip_vertical = rospy.get_param('aruco_map/flip_vertical', False)
-        description = ''
-        if known_vertical == 'map' and not flip_vertical:
-            description += ' (markers map is on the floor)'
-        elif known_vertical == 'map' and flip_vertical:
-            description += ' (markers map is on the ceiling)'
-        info('aruco_map/known_vertical = %s', known_vertical)
-        info('aruco_map/flip_vertical = %s%s', flip_vertical, description)
+        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=0.8)
+            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=0.8)
+            rospy.wait_for_message('aruco_map/pose', PoseWithCovarianceStamped, timeout=1)
         except rospy.ROSException:
-            if not markers:
-                info('no map detection as no markers detection')
-            elif not markers.markers:
-                info('no map detection as no markers detected')
-            else:
-                failure('no map detection')
+            failure('no map detection')
     else:
         info('aruco_map is not running')
 
 
-def is_on_the_floor():
-    try:
-        dist = rospy.wait_for_message('rangefinder/range', Range, timeout=1)
-        return dist.range < 0.3
-    except rospy.ROSException:
-        return False
-
-
 @check('Vision position estimate')
 def check_vpe():
     vis = None
     try:
-        vis = rospy.wait_for_message('mavros/vision_pose/pose', PoseStamped, timeout=0.8)
+        vis = rospy.wait_for_message('mavros/vision_pose/pose', PoseStamped, timeout=1)
     except rospy.ROSException:
         try:
-            vis = rospy.wait_for_message('mavros/mocap/pose', PoseStamped, timeout=0.8)
+            vis = rospy.wait_for_message('mavros/mocap/pose', PoseStamped, timeout=1)
         except rospy.ROSException:
-            if not is_process_running('vpe_publisher', full=True):
-                info('no vision position estimate, vpe_publisher is not running')
-            elif rospy.get_param('aruco_map/known_vertical', '') == 'map' \
-                   and rospy.get_param('aruco_map/flip_vertical', False):
-                failure('no vision position estimate, markers are on the ceiling')
-            elif is_on_the_floor():
-                info('no vision position estimate, the drone is on the floor')
-            else:
-                failure('no vision position estimate')
+            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
 
     # check PX4 settings
     est = get_param('SYS_MC_EST_GROUP')
@@ -486,14 +414,14 @@ 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: %s', ff(vision_yaw_w))
+            info('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')
         delay = get_param('LPE_VIS_DELAY')
         if delay != 0:
-            failure('LPE_VIS_DELAY = %s, but it should be zero', delay)
-        info('LPE_VIS_XY = %s m, LPE_VIS_Z = %s m', get_paramf('LPE_VIS_XY'), get_paramf('LPE_VIS_Z'))
+            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'))
     elif est == 2:
         fuse = get_param('EKF2_AID_MASK')
         if not fuse & (1 << 3):
@@ -502,10 +430,10 @@ def check_vpe():
             failure('vision yaw fusion is disabled, change EKF2_AID_MASK parameter')
         delay = get_param('EKF2_EV_DELAY')
         if delay != 0:
-            failure('EKF2_EV_DELAY = %.2f, but it should be zero', delay)
-        info('EKF2_EVA_NOISE = %s, EKF2_EVP_NOISE = %s',
-            get_paramf('EKF2_EVA_NOISE', 3),
-            get_paramf('EKF2_EVP_NOISE', 3))
+            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
@@ -603,19 +531,13 @@ def check_velocity():
 @check('Global position (GPS)')
 def check_global_position():
     try:
-        rospy.wait_for_message('mavros/global_position/global', NavSatFix, timeout=0.8)
+        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', 0) & (1 << 0)):
-            failure('enabled GPS fusion may suppress vision position aiding')
 
 
 @check('Optical flow')
 def check_optical_flow():
-    if not is_process_running('optical_flow', full=True):
-        info('optical_flow is not running')
-        return
-
     # TODO:check FPS!
     try:
         rospy.wait_for_message('mavros/px4flow/raw/send', OpticalFlowRad, timeout=0.5)
@@ -623,7 +545,7 @@ def check_optical_flow():
         # check PX4 settings
         rot = get_param('SENS_FLOW_ROT')
         if rot != 0:
-            failure('SENS_FLOW_ROT = %s, but it should be zero', rot)
+            failure('SENS_FLOW_ROT parameter is %s, but it should be zero', rot)
         est = get_param('SYS_MC_EST_GROUP')
         if est == 1:
             fuse = get_param('LPE_FUSION')
@@ -631,36 +553,32 @@ def check_optical_flow():
                 failure('optical flow fusion 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', 1)
+            scale = get_param('LPE_FLW_SCALE')
             if not numpy.isclose(scale, 1.0):
-                failure('LPE_FLW_SCALE = %.2f, but it should be 1.0', scale)
+                failure('LPE_FLW_SCALE parameter is %.2f, but it should be 1.0', scale)
 
-            info('LPE_FLW_QMIN = %s, LPE_FLW_R = %s, LPE_FLW_RR = %s',
-                          get_paramf('LPE_FLW_QMIN'),
-                          get_paramf('LPE_FLW_R', 4),
-                          get_paramf('LPE_FLW_RR', 4))
+            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'))
         elif est == 2:
-            fuse = get_param('EKF2_AID_MASK', 0)
+            fuse = get_param('EKF2_AID_MASK')
             if not fuse & (1 << 1):
                 failure('optical flow fusion is disabled, change EKF2_AID_MASK parameter')
-            delay = get_param('EKF2_OF_DELAY', 0)
+            delay = get_param('EKF2_OF_DELAY')
             if delay != 0:
-                failure('EKF2_OF_DELAY = %.2f, but it should be zero', delay)
-            info('EKF2_OF_QMIN = %s, EKF2_OF_N_MIN = %s, EKF2_OF_N_MAX = %s',
-                          get_paramf('EKF2_OF_QMIN'),
-                          get_paramf('EKF2_OF_N_MIN', 4),
-                          get_paramf('EKF2_OF_N_MAX', 4))
-        info('SENS_FLOW_MINHGT = %s, SENS_FLOW_MAXHGT = %s', get_paramf('SENS_FLOW_MINHGT', 3), get_paramf('SENS_FLOW_MAXHGT', 3))
+                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'))
 
     except rospy.ROSException:
-        if rospy.get_param('optical_flow/disable_on_vpe', False):
-            try:
-                rospy.wait_for_message('mavros/vision_pose/pose', PoseStamped, timeout=1)
-                info('no optical flow as disable_on_vpe is true')
-            except:
-                failure('no optical flow on RPi, disable_on_vpe is true, but no vision pose also')
-        else:
-            failure('no optical flow on RPi')
+        failure('no optical flow data (from Raspberry)')
 
 
 @check('Rangefinder')
@@ -684,7 +602,7 @@ def check_rangefinder():
 
     est = get_param('SYS_MC_EST_GROUP')
     if est == 1:
-        fuse = get_param('LPE_FUSION', 0)
+        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')
         else:
@@ -705,10 +623,6 @@ def check_rangefinder():
 
 @check('Boot duration')
 def check_boot_duration():
-    if not os.path.exists('/etc/clover_version'):
-        info('skip check')
-        return # Don't check not on Clover's image
-
     output = subprocess.check_output('systemd-analyze').decode()
     r = re.compile(r'([\d\.]+)s\s*$', flags=re.MULTILINE)
     duration = float(r.search(output).groups()[0])
@@ -718,7 +632,7 @@ def check_boot_duration():
 
 @check('CPU usage')
 def check_cpu_usage():
-    WHITELIST = 'nodelet', 'gzclient', 'gzserver', 'selfcheck.py'
+    WHITELIST = 'nodelet', 'gzclient', 'gzserver'
     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).decode()
     processes = output.split('\n')
@@ -734,9 +648,6 @@ def check_cpu_usage():
 
 @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()
@@ -787,18 +698,11 @@ def check_image():
     try:
         info('version: %s', open('/etc/clover_version').read().strip())
     except IOError:
-        try:
-            info('VM version: %s', open('/etc/clover_vm_version').read().strip())
-        except IOError:
-            info('no /etc/clover_version file, not the Clover image?')
+        info('no /etc/clover_version file, not the Clover 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')
@@ -820,10 +724,6 @@ 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:
@@ -901,47 +801,26 @@ def check_board():
         info('could not open /proc/device-tree/model, not a Raspberry Pi?')
 
 
-def parallel_for(fns):
-    threads = []
-    for fn in fns:
-        thread = Thread(target=fn)
-        thread.start()
-        threads.append(thread)
-    for thread in threads:
-        thread.join()
-
-
-def consequentially_for(fns):
-    for fn in fns:
-        fn()
-
-
 def selfcheck():
-    checks = [
-        check_image,
-        check_board,
-        check_clover_service,
-        check_network,
-        check_fcu,
-        check_imu,
-        check_local_position,
-        check_velocity,
-        check_global_position,
-        check_preflight_status,
-        check_main_camera,
-        check_aruco,
-        check_simpleoffboard,
-        check_optical_flow,
-        check_vpe,
-        check_rangefinder,
-        check_rpi_health,
-        check_cpu_usage,
-        check_boot_duration,
-    ]
-    if rospy.get_param('~parallel', False):
-        parallel_for(checks)
-    else:
-        consequentially_for(checks)
+    check_image()
+    check_board()
+    check_clover_service()
+    check_network()
+    check_fcu()
+    check_imu()
+    check_local_position()
+    check_velocity()
+    check_global_position()
+    check_preflight_status()
+    check_main_camera()
+    check_aruco()
+    check_simpleoffboard()
+    check_optical_flow()
+    check_vpe()
+    check_rangefinder()
+    check_rpi_health()
+    check_cpu_usage()
+    check_boot_duration()
 
 
 if __name__ == '__main__':

clover/src/simple_offboard.cpp

@@ -23,14 +23,12 @@
 #include <tf2_ros/static_transform_broadcaster.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 #include <std_srvs/Trigger.h>
-#include <geometry_msgs/PointStamped.h>
 #include <geometry_msgs/PoseStamped.h>
 #include <geometry_msgs/TwistStamped.h>
 #include <geometry_msgs/Vector3Stamped.h>
 #include <geometry_msgs/QuaternionStamped.h>
 #include <sensor_msgs/NavSatFix.h>
 #include <sensor_msgs/BatteryState.h>
-#include <sensor_msgs/Range.h>
 #include <mavros_msgs/CommandBool.h>
 #include <mavros_msgs/SetMode.h>
 #include <mavros_msgs/PositionTarget.h>
@@ -39,19 +37,14 @@
 #include <mavros_msgs/State.h>
 #include <mavros_msgs/StatusText.h>
 #include <mavros_msgs/ManualControl.h>
-#include <mavros_msgs/Altitude.h>
 
 #include <clover/GetTelemetry.h>
 #include <clover/Navigate.h>
 #include <clover/NavigateGlobal.h>
-#include <clover/SetAltitude.h>
-#include <clover/SetYaw.h>
-#include <clover/SetYawRate.h>
 #include <clover/SetPosition.h>
 #include <clover/SetVelocity.h>
 #include <clover/SetAttitude.h>
 #include <clover/SetRates.h>
-#include <clover/State.h>
 
 using std::string;
 using std::isnan;
@@ -61,7 +54,6 @@ using namespace clover;
 using mavros_msgs::PositionTarget;
 using mavros_msgs::AttitudeTarget;
 using mavros_msgs::Thrust;
-using mavros_msgs::Altitude;
 
 // tf2
 tf2_ros::Buffer tf_buffer;
@@ -87,43 +79,35 @@ float default_speed;
 bool auto_release;
 bool land_only_in_offboard, nav_from_sp, check_kill_switch;
 std::map<string, string> reference_frames;
-string terrain_frame_mode;
 
 // Publishers
-ros::Publisher attitude_pub, attitude_raw_pub, position_pub, position_raw_pub, rates_pub, thrust_pub, state_pub;
+ros::Publisher attitude_pub, attitude_raw_pub, position_pub, position_raw_pub, rates_pub, thrust_pub;
 
 // Service clients
 ros::ServiceClient arming, set_mode;
 
 // Containers
 ros::Timer setpoint_timer;
+tf::Quaternion tq;
 PoseStamped position_msg;
 PositionTarget position_raw_msg;
-//TwistStamped rates_msg;
+AttitudeTarget att_raw_msg;
+Thrust thrust_msg;
+TwistStamped rates_msg;
 TransformStamped target, setpoint;
 geometry_msgs::TransformStamped body;
-geometry_msgs::TransformStamped terrain;
 
 // State
 PoseStamped nav_start;
-PointStamped setpoint_position;
-PointStamped setpoint_altitude;
-Vector3Stamped setpoint_velocity;
-float setpoint_yaw, setpoint_roll, setpoint_pitch;
-Vector3 setpoint_rates;
-string yaw_frame_id;
-float setpoint_thrust;
+PoseStamped setpoint_position, setpoint_position_transformed;
+Vector3Stamped setpoint_velocity, setpoint_velocity_transformed;
+QuaternionStamped setpoint_attitude, setpoint_attitude_transformed;
+float setpoint_yaw_rate;
 float nav_speed;
-float setpoint_lat = NAN, setpoint_lon = NAN;
 bool busy = false;
 bool wait_armed = false;
 bool nav_from_sp_flag = false;
 
-// Last published
-PoseStamped setpoint_pose_local;
-Vector3Stamped setpoint_velocity_local;
-float yaw_local;
-
 enum setpoint_type_t {
 	NONE,
 	NAVIGATE,
@@ -131,10 +115,7 @@ enum setpoint_type_t {
 	POSITION,
 	VELOCITY,
 	ATTITUDE,
-	RATES,
-	_ALTITUDE,
-	_YAW,
-	_YAW_RATE,
+	RATES
 };
 
 enum setpoint_type_t setpoint_type = NONE;
@@ -169,9 +150,6 @@ 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));
@@ -189,7 +167,7 @@ void handleLocalPosition(const PoseStamped& pose)
 {
 	local_position = pose;
 	publishBodyFrame();
-	// TODO: home?
+	// TODO: terrain?, home?
 }
 
 // wait for transform without interrupting publishing setpoints
@@ -207,29 +185,6 @@ inline bool waitTransform(const string& target, const string& source,
 	return false;
 }
 
-void publishTerrain(const double distance, const ros::Time& stamp)
-{
-	if (!waitTransform(local_frame, body.child_frame_id, stamp, ros::Duration(0.1))) return;
-
-	auto t = tf_buffer.lookupTransform(local_frame, body.child_frame_id, stamp);
-	t.child_frame_id = terrain.child_frame_id;
-	t.transform.translation.z -= distance;
-	static_transform_broadcaster->sendTransform(t);
-}
-
-void handleAltitude(const Altitude& alt)
-{
-	if (!std::isfinite(alt.bottom_clearance)) return;
-	publishTerrain(alt.bottom_clearance, alt.header.stamp);
-}
-
-void handleRange(const Range& range)
-{
-	if (!std::isfinite(range.range)) return;
-	// TODO: check it's facing down
-	publishTerrain(range.range, range.header.stamp);
-}
-
 #define TIMEOUT(msg, timeout) (msg.header.stamp.isZero() || (ros::Time::now() - msg.header.stamp > timeout))
 
 bool getTelemetry(GetTelemetry::Request& req, GetTelemetry::Response& res)
@@ -249,11 +204,11 @@ bool getTelemetry(GetTelemetry::Request& req, GetTelemetry::Response& res)
 	res.vx = NAN;
 	res.vy = NAN;
 	res.vz = NAN;
-	res.roll = NAN;
 	res.pitch = NAN;
+	res.roll = NAN;
 	res.yaw = NAN;
-	res.roll_rate = NAN;
 	res.pitch_rate = NAN;
+	res.roll_rate = NAN;
 	res.yaw_rate = NAN;
 	res.voltage = NAN;
 	res.cell_voltage = NAN;
@@ -383,20 +338,20 @@ inline float getDistance(const Point& from, const Point& to)
 	return hypot(from.x - to.x, from.y - to.y, from.z - to.z);
 }
 
-void getNavigateSetpoint(const ros::Time& stamp, const float speed, Point& nav_setpoint)
+void getNavigateSetpoint(const ros::Time& stamp, float speed, Point& nav_setpoint)
 {
 	if (wait_armed) {
 		// don't start navigating if we're waiting arming
 		nav_start.header.stamp = stamp;
 	}
 
-	float distance = getDistance(nav_start.pose.position, setpoint_pose_local.pose.position);
+	float distance = getDistance(nav_start.pose.position, setpoint_position_transformed.pose.position);
 	float time = distance / speed;
 	float passed = std::min((stamp - nav_start.header.stamp).toSec() / time, 1.0);
 
-	nav_setpoint.x = nav_start.pose.position.x + (setpoint_pose_local.pose.position.x - nav_start.pose.position.x) * passed;
-	nav_setpoint.y = nav_start.pose.position.y + (setpoint_pose_local.pose.position.y - nav_start.pose.position.y) * passed;
-	nav_setpoint.z = nav_start.pose.position.z + (setpoint_pose_local.pose.position.z - nav_start.pose.position.z) * passed;
+	nav_setpoint.x = nav_start.pose.position.x + (setpoint_position_transformed.pose.position.x - nav_start.pose.position.x) * passed;
+	nav_setpoint.y = nav_start.pose.position.y + (setpoint_position_transformed.pose.position.y - nav_start.pose.position.y) * passed;
+	nav_setpoint.z = nav_start.pose.position.z + (setpoint_position_transformed.pose.position.z - nav_start.pose.position.z) * passed;
 }
 
 PoseStamped globalToLocal(double lat, double lon)
@@ -427,101 +382,44 @@ PoseStamped globalToLocal(double lat, double lon)
 	return pose;
 }
 
-// publish navigate_target frame
-void publishTarget(ros::Time stamp, bool _static = false)
-{
-	bool single_frame = (setpoint_position.header.frame_id == setpoint_altitude.header.frame_id);
-
-	// handle yaw for target frame
-	if (setpoint_yaw_type == YAW || setpoint_yaw_type == YAW_RATE) { // use last set yaw for yaw_rate
-		if (setpoint_altitude.header.frame_id == yaw_frame_id) {
-			target.transform.rotation = tf::createQuaternionMsgFromYaw(setpoint_yaw);
-		} else {
-			single_frame = false;
-			target.transform.rotation = tf::createQuaternionMsgFromYaw(yaw_local);
-		}
-	} else if (setpoint_yaw_type == TOWARDS) {
-		single_frame = false;
-		target.transform.rotation = tf::createQuaternionMsgFromYaw(yaw_local);
-	}
-
-	if (_static && single_frame) {
-		// publish at user's command, if all frames are the same
-		target.header.frame_id = setpoint_position.header.frame_id;
-		target.header.stamp = stamp;
-		target.transform.translation.x = setpoint_position.point.x;
-		target.transform.translation.y = setpoint_position.point.y;
-		target.transform.translation.z = setpoint_position.point.z;
-
-	} else if (!_static) {
-		// publish at each iteration, if frames are different
-		target.header = setpoint_pose_local.header;
-		target.transform.translation.x = setpoint_pose_local.pose.position.x;
-		target.transform.translation.y = setpoint_pose_local.pose.position.y;
-		target.transform.translation.z = setpoint_pose_local.pose.position.z;
-	}
-
-	static_transform_broadcaster->sendTransform(target);
-}
-
 void publish(const ros::Time stamp)
 {
 	if (setpoint_type == NONE) return;
 
 	position_raw_msg.header.stamp = stamp;
+	thrust_msg.header.stamp = stamp;
+	rates_msg.header.stamp = stamp;
 
-	// transform position
-	if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == POSITION) {
-		setpoint_position.header.stamp = stamp;
-		setpoint_altitude.header.stamp = stamp;
-		// transform xy
-		try {
-			auto xy = tf_buffer.transform(setpoint_position, local_frame, ros::Duration(0.05));
-			setpoint_pose_local.header = xy.header;
-			setpoint_pose_local.pose.position.x = xy.point.x;
-			setpoint_pose_local.pose.position.y = xy.point.y;
-		} catch (tf2::TransformException& ex) {
-			// can't transform xy, use last known
-			ROS_WARN_THROTTLE(10, "can't transform: %s", ex.what());
+	try {
+		// transform position and/or yaw
+		if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == POSITION || setpoint_type == VELOCITY || setpoint_type == ATTITUDE) {
+			setpoint_position.header.stamp = stamp;
+			tf_buffer.transform(setpoint_position, setpoint_position_transformed, local_frame, ros::Duration(0.05));
 		}
-		// transform altitude
-		try {
-			setpoint_pose_local.pose.position.z = tf_buffer.transform(setpoint_altitude, local_frame, ros::Duration(0.05)).point.z;
-		} catch (tf2::TransformException& ex) {
-			// can't transform altitude, use last known
-			ROS_WARN_THROTTLE(10, "can't transform: %s", ex.what());
+
+		// transform velocity
+		if (setpoint_type == VELOCITY) {
+			setpoint_velocity.header.stamp = stamp;
+			tf_buffer.transform(setpoint_velocity, setpoint_velocity_transformed, local_frame, ros::Duration(0.05));
 		}
+
+	} catch (const tf2::TransformException& e) {
+		ROS_WARN_THROTTLE(10, "can't transform");
 	}
 
-	// transform yaw
-	if (setpoint_yaw_type == YAW) {
-		try {
-			QuaternionStamped q;
-			q.header.stamp = stamp;
-			q.header.frame_id = yaw_frame_id;
-			q.quaternion = tf::createQuaternionMsgFromYaw(setpoint_yaw);
-			yaw_local = tf2::getYaw(tf_buffer.transform(q, local_frame, ros::Duration(0.05)).quaternion);
-		} catch (tf2::TransformException& ex) {
-			// can't transform yaw, use last known
-			ROS_WARN_THROTTLE(10, "can't transform: %s", ex.what());
-		}
-	}
-
-	// compute navigate setpoint
 	if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL) {
+		position_msg.pose.orientation = setpoint_position_transformed.pose.orientation; // copy yaw
 		getNavigateSetpoint(stamp, nav_speed, position_msg.pose.position);
 
 		if (setpoint_yaw_type == TOWARDS) {
-			yaw_local = atan2(position_msg.pose.position.y - nav_start.pose.position.y,
-			                  position_msg.pose.position.x - nav_start.pose.position.x);
+			double yaw_towards = atan2(position_msg.pose.position.y - nav_start.pose.position.y,
+			                           position_msg.pose.position.x - nav_start.pose.position.x);
+			position_msg.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, yaw_towards);
 		}
-
-		position_msg.pose.orientation = tf::createQuaternionMsgFromYaw(yaw_local);
 	}
 
 	if (setpoint_type == POSITION) {
-		position_msg = setpoint_pose_local;
-		position_msg.pose.orientation = tf::createQuaternionMsgFromYaw(yaw_local);
+		position_msg = setpoint_position_transformed;
 	}
 
 	if (setpoint_type == POSITION || setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL) {
@@ -538,14 +436,14 @@ void publish(const ros::Time stamp)
 			                             PositionTarget::IGNORE_AFY +
 			                             PositionTarget::IGNORE_AFZ +
 			                             PositionTarget::IGNORE_YAW;
-			position_raw_msg.yaw_rate = setpoint_rates.z;
+			position_raw_msg.yaw_rate = setpoint_yaw_rate;
 			position_raw_msg.position = position_msg.pose.position;
 			position_raw_pub.publish(position_raw_msg);
 		}
 
 		// publish setpoint frame
 		if (!setpoint.child_frame_id.empty()) {
-			if (setpoint.header.stamp >= position_msg.header.stamp) {
+			if (setpoint.header.stamp == position_msg.header.stamp) {
 				return; // avoid TF_REPEATED_DATA warnings
 			}
 
@@ -557,22 +455,9 @@ void publish(const ros::Time stamp)
 			setpoint.header.stamp = position_msg.header.stamp;
 			transform_broadcaster->sendTransform(setpoint);
 		}
-
-		// publish dynamic target frame
-		publishTarget(stamp);
 	}
 
 	if (setpoint_type == VELOCITY) {
-		// transform velocity to local frame
-		setpoint_velocity.header.stamp = stamp;
-		try {
-			setpoint_velocity_local = tf_buffer.transform(setpoint_velocity, local_frame, ros::Duration(0.05));
-		} catch (tf2::TransformException& ex) {
-			// can't transform velocity, use last known
-			ROS_WARN_THROTTLE(10, "can't transform: %s", ex.what());
-		}
-
-		// publish velocity
 		position_raw_msg.type_mask = PositionTarget::IGNORE_PX +
 		                             PositionTarget::IGNORE_PY +
 		                             PositionTarget::IGNORE_PZ +
@@ -580,22 +465,14 @@ void publish(const ros::Time stamp)
 		                             PositionTarget::IGNORE_AFY +
 		                             PositionTarget::IGNORE_AFZ;
 		position_raw_msg.type_mask += setpoint_yaw_type == YAW ? PositionTarget::IGNORE_YAW_RATE : PositionTarget::IGNORE_YAW;
-		position_raw_msg.velocity = setpoint_velocity_local.vector;
-		position_raw_msg.yaw = yaw_local;
-		position_raw_msg.yaw_rate = setpoint_rates.z;
+		position_raw_msg.velocity = setpoint_velocity_transformed.vector;
+		position_raw_msg.yaw = tf2::getYaw(setpoint_position_transformed.pose.orientation);
+		position_raw_msg.yaw_rate = setpoint_yaw_rate;
 		position_raw_pub.publish(position_raw_msg);
 	}
 
 	if (setpoint_type == ATTITUDE) {
-		PoseStamped msg;
-		msg.header.stamp = stamp;
-		msg.header.frame_id = local_frame;
-		msg.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(setpoint_roll, setpoint_pitch, yaw_local);
-		attitude_pub.publish(msg);
-
-		Thrust thrust_msg;
-		thrust_msg.header.stamp = stamp;
-		thrust_msg.thrust = setpoint_thrust;
+		attitude_pub.publish(setpoint_position_transformed);
 		thrust_pub.publish(thrust_msg);
 	}
 
@@ -604,12 +481,11 @@ void publish(const ros::Time stamp)
 		// thrust_pub.publish(thrust_msg);
 		// mavros rates topics waits for rates in local frame
 		// use rates in body frame for simplicity
-		AttitudeTarget att_raw_msg;
 		att_raw_msg.header.stamp = stamp;
 		att_raw_msg.header.frame_id = fcu_frame;
 		att_raw_msg.type_mask = AttitudeTarget::IGNORE_ATTITUDE;
-		att_raw_msg.body_rate = setpoint_rates;
-		att_raw_msg.thrust = setpoint_thrust;
+		att_raw_msg.body_rate = rates_msg.twist.angular;
+		att_raw_msg.thrust = thrust_msg.thrust;
 		attitude_raw_pub.publish(att_raw_msg);
 	}
 }
@@ -627,10 +503,10 @@ inline void checkManualControl()
 
 	if (check_kill_switch) {
 		// switch values: https://github.com/PX4/PX4-Autopilot/blob/c302514a0809b1765fafd13c014d705446ae1113/msg/manual_control_setpoint.msg#L3
-		const uint8_t SWITCH_POS_NONE = 0; // switch is not mapped
-		const uint8_t SWITCH_POS_ON = 1; // switch activated
-		const uint8_t SWITCH_POS_MIDDLE = 2; // middle position
-		const uint8_t SWITCH_POS_OFF = 3; // switch not activated
+		[[maybe_unused]] const uint8_t SWITCH_POS_NONE = 0; // switch is not mapped
+		[[maybe_unused]] const uint8_t SWITCH_POS_ON = 1; // switch activated
+		[[maybe_unused]] const uint8_t SWITCH_POS_MIDDLE = 2; // middle position
+		[[maybe_unused]] const uint8_t SWITCH_POS_OFF = 3; // switch not activated
 
 		const int KILL_SWITCH_BIT = 12; // https://github.com/PX4/Firmware/blob/c302514a0809b1765fafd13c014d705446ae1113/src/modules/mavlink/mavlink_messages.cpp#L3975
 		uint8_t kill_switch = (manual_control.buttons & (0b11 << KILL_SWITCH_BIT)) >> KILL_SWITCH_BIT;
@@ -649,59 +525,10 @@ inline void checkState()
 		throw std::runtime_error("No connection to FCU, https://clover.coex.tech/connection");
 }
 
-void publishState()
-{
-	clover::State msg;
-	msg.mode = setpoint_type;
-	msg.yaw_mode = setpoint_yaw_type;
-
-	if (setpoint_position.header.frame_id.empty()) {
-		msg.x = NAN;
-		msg.y = NAN;
-		msg.z = NAN;
-	} else {
-		msg.x = setpoint_position.point.x;
-		msg.y = setpoint_position.point.y;
-		msg.z = setpoint_altitude.point.z;
-	}
-
-	msg.speed = nav_speed;
-	msg.lat = setpoint_lat;
-	msg.lon = setpoint_lon;
-	msg.vx = setpoint_velocity.vector.x;
-	msg.vy = setpoint_velocity.vector.y;
-	msg.vz = setpoint_velocity.vector.z;
-	msg.roll = setpoint_roll;
-	msg.pitch = setpoint_pitch;
-	msg.yaw = !yaw_frame_id.empty() ? setpoint_yaw : NAN;
-
-	msg.roll_rate = setpoint_rates.x;
-	msg.pitch_rate = setpoint_rates.y;
-	msg.yaw_rate = setpoint_rates.z;
-	msg.thrust = setpoint_thrust;
-
-	if (setpoint_type == VELOCITY) {
-		msg.xy_frame_id = setpoint_velocity.header.frame_id;
-		msg.z_frame_id = setpoint_velocity.header.frame_id;
-	} else {
-		msg.xy_frame_id = setpoint_position.header.frame_id;
-		msg.z_frame_id = setpoint_altitude.header.frame_id;
-	}
-	msg.yaw_frame_id = yaw_frame_id;
-
-	state_pub.publish(msg);
-}
-
-inline float safe(float value) {
-	return std::isfinite(value) ? value : 0;
-}
-
 #define ENSURE_FINITE(var) { if (!std::isfinite(var)) throw std::runtime_error(#var " argument cannot be NaN or Inf"); }
 
-#define ENSURE_NON_INF(var) { if (std::isinf(var)) throw std::runtime_error(#var " argument cannot be Inf"); }
-
 bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, float vy, float vz,
-           float roll, float pitch, float yaw, float roll_rate, float pitch_rate, float yaw_rate, // editorconfig-checker-disable-line
+           float pitch, float roll, float yaw, float pitch_rate, float roll_rate, float yaw_rate, // editorconfig-checker-disable-line
            float lat, float lon, float thrust, float speed, string frame_id, bool auto_arm, // editorconfig-checker-disable-line
            uint8_t& success, string& message) // editorconfig-checker-disable-line
 {
@@ -728,40 +555,69 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl
 		auto search = reference_frames.find(frame_id);
 		const string& reference_frame = search == reference_frames.end() ? frame_id : search->second;
 
-		ENSURE_NON_INF(x);
-		ENSURE_NON_INF(y);
-		ENSURE_NON_INF(z);
-		ENSURE_NON_INF(speed); // TODO: allow inf
-		ENSURE_NON_INF(vx);
-		ENSURE_NON_INF(vy);
-		ENSURE_NON_INF(vz);
-		ENSURE_NON_INF(roll);
-		ENSURE_NON_INF(pitch);
-		ENSURE_NON_INF(roll_rate);
-		ENSURE_NON_INF(pitch_rate);
-		ENSURE_NON_INF(yaw_rate);
-		ENSURE_NON_INF(thrust);
+		// Serve "partial" commands
 
-		if (sp_type == NAVIGATE_GLOBAL) {
+		if (!auto_arm && std::isfinite(yaw) &&
+		    isnan(x) && isnan(y) && isnan(z) && isnan(vx) && isnan(vy) && isnan(vz) &&
+		    isnan(pitch) && isnan(roll) && isnan(thrust) &&
+		    isnan(lat) && isnan(lon)) {
+			// change only the yaw
+			if (setpoint_type == POSITION || setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == VELOCITY) {
+				if (!waitTransform(setpoint_position.header.frame_id, frame_id, stamp, transform_timeout))
+					throw std::runtime_error("Can't transform from " + frame_id + " to " + setpoint_position.header.frame_id);
+
+				message = "Changing yaw only";
+
+				QuaternionStamped q;
+				q.header.frame_id = frame_id;
+				q.header.stamp = stamp;
+				q.quaternion = tf::createQuaternionMsgFromYaw(yaw); // TODO: pitch=0, roll=0 is not totally correct
+				setpoint_position.pose.orientation = tf_buffer.transform(q, setpoint_position.header.frame_id).quaternion;
+				setpoint_yaw_type = YAW;
+				goto publish_setpoint;
+			} else {
+				throw std::runtime_error("Setting yaw is possible only when position or velocity setpoints active");
+			}
+		}
+
+		if (!auto_arm && std::isfinite(yaw_rate) &&
+		    isnan(x) && isnan(y) && isnan(z) && isnan(vx) && isnan(vy) && isnan(vz) &&
+		    isnan(pitch) && isnan(roll) && isnan(yaw) && isnan(thrust) &&
+		    isnan(lat) && isnan(lon)) {
+			// change only the yaw rate
+			if (setpoint_type == POSITION || setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == VELOCITY) {
+				message = "Changing yaw rate only";
+
+				setpoint_yaw_type = YAW_RATE;
+				setpoint_yaw_rate = yaw_rate;
+				goto publish_setpoint;
+			} else {
+				throw std::runtime_error("Setting yaw rate is possible only when position or velocity setpoints active");
+			}
+		}
+
+		// Serve normal commands
+
+		if (sp_type == NAVIGATE || sp_type == POSITION) {
+			ENSURE_FINITE(x);
+			ENSURE_FINITE(y);
+			ENSURE_FINITE(z);
+		} else if (sp_type == NAVIGATE_GLOBAL) {
 			ENSURE_FINITE(lat);
 			ENSURE_FINITE(lon);
-		}
-
-		if (isfinite(x) != isfinite(y)) {
-			throw std::runtime_error("x and y can be set only together");
-		}
-
-		if (isfinite(yaw_rate)) {
-			if (sp_type > RATES && setpoint_type == ATTITUDE) {
-				throw std::runtime_error("Yaw rate cannot be set in attitude mode.");
-			}
-		}
-
-		// set_altitude
-		if (sp_type == _ALTITUDE) {
-			if (setpoint_type == VELOCITY || setpoint_type == ATTITUDE || setpoint_type == RATES) {
-				throw std::runtime_error("Altitude cannot be set in velocity, attitude or rates mode.");
-			}
+			ENSURE_FINITE(z);
+		} else if (sp_type == VELOCITY) {
+			ENSURE_FINITE(vx);
+			ENSURE_FINITE(vy);
+			ENSURE_FINITE(vz);
+		} else if (sp_type == ATTITUDE) {
+			ENSURE_FINITE(pitch);
+			ENSURE_FINITE(roll);
+			ENSURE_FINITE(thrust);
+		} else if (sp_type == RATES) {
+			ENSURE_FINITE(pitch_rate);
+			ENSURE_FINITE(roll_rate);
+			ENSURE_FINITE(thrust);
 		}
 
 		if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL) {
@@ -775,13 +631,20 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl
 				speed = default_speed;
 		}
 
+		if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == POSITION || sp_type == VELOCITY) {
+			if (yaw_rate != 0 && !std::isnan(yaw))
+				throw std::runtime_error("Yaw value should be NaN for setting yaw rate");
+
+			if (std::isnan(yaw_rate) && std::isnan(yaw))
+				throw std::runtime_error("Both yaw and yaw_rate cannot be NaN");
+		}
+
 		if (sp_type == NAVIGATE_GLOBAL) {
 			if (TIMEOUT(global_position, global_position_timeout))
 				throw std::runtime_error("No global position");
 		}
 
-		// if any value need to be transformed to reference frame
-		if (isfinite(x) || isfinite(y) || isfinite(z) || isfinite(vx) || isfinite(vy) || isfinite(vz) || isfinite(yaw)) {
+		if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == POSITION || sp_type == VELOCITY || sp_type == ATTITUDE) {
 			// make sure transform from frame_id to reference frame available
 			if (!waitTransform(reference_frame, frame_id, stamp, transform_timeout))
 				throw std::runtime_error("Can't transform from " + frame_id + " to " + reference_frame);
@@ -798,27 +661,15 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl
 			auto xy_in_req_frame = tf_buffer.transform(pose_local, frame_id);
 			x = xy_in_req_frame.pose.position.x;
 			y = xy_in_req_frame.pose.position.y;
-			setpoint_lat = lat;
-			setpoint_lon = lon;
 		}
 
 		// Everything fine - switch setpoint type
-		if (sp_type <= RATES) {
-			setpoint_type = sp_type;
-		}
+		setpoint_type = sp_type;
 
-		if (setpoint_type != NAVIGATE && setpoint_type != NAVIGATE_GLOBAL) {
+		if (sp_type != NAVIGATE && sp_type != NAVIGATE_GLOBAL) {
 			nav_from_sp_flag = false;
 		}
 
-		bool to_auto_arm = auto_arm && (state.mode != "OFFBOARD" || !state.armed);
-		if (to_auto_arm || setpoint_type == VELOCITY || setpoint_type == ATTITUDE || setpoint_type == RATES) {
-			// invalidate position setpoint
-			setpoint_position.header.frame_id = "";
-			setpoint_altitude.header.frame_id = "";
-			yaw_frame_id = "";
-		}
-
 		if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL) {
 			// starting point
 			if (nav_from_sp && nav_from_sp_flag) {
@@ -827,139 +678,89 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl
 			} else {
 				nav_start = local_position;
 			}
-
-			if (!isnan(speed)) {
-				nav_speed = speed;
-			}
-
+			nav_speed = speed;
 			nav_from_sp_flag = true;
 		}
 
-		// handle position
-		if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == POSITION) {
+		// if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == POSITION || sp_type == VELOCITY) {
+		// 	if (std::isnan(yaw) && yaw_rate == 0) {
+		// 		// keep yaw unchanged
+		//		// TODO: this is incorrect, because we need yaw in desired frame
+		// 		yaw = tf2::getYaw(local_position.pose.orientation);
+		// 	}
+		// }
 
-			PointStamped desired;
-			desired.header.frame_id = frame_id;
-			desired.header.stamp = stamp;
-			desired.point.x = safe(x);
-			desired.point.y = safe(y);
-			desired.point.z = safe(z);
+		if (sp_type == POSITION || sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == VELOCITY || sp_type == ATTITUDE) {
+			// destination point and/or attitude
+			PoseStamped ps;
+			ps.header.frame_id = frame_id;
+			ps.header.stamp = stamp;
+			ps.pose.position.x = x;
+			ps.pose.position.y = y;
+			ps.pose.position.z = z;
+			ps.pose.orientation.w = 1.0; // Ensure quaternion is always valid
 
-			// transform to reference frame
-			desired = tf_buffer.transform(desired, reference_frame);
-
-			// set horizontal position
-			if (isfinite(x) && isfinite(y)) {
-				setpoint_position = desired;
-			} else if (setpoint_position.header.frame_id.empty()) {
-				 // TODO: use transform for current stamp
-				setpoint_position.header = local_position.header;
-				setpoint_position.point = local_position.pose.position;
-			}
-
-			// set altitude
-			if (isfinite(z)) {
-				setpoint_altitude = desired;
-			} else if (setpoint_altitude.header.frame_id.empty()) {
-				setpoint_altitude.header = local_position.header;
-				setpoint_altitude.point = local_position.pose.position;
-			}
-		}
-
-		// handle velocity
-		if (sp_type == VELOCITY) {
-			// TODO: allow setting different modes by altitude and xy
-			Vector3Stamped desired;
-			desired.header.frame_id = frame_id;
-			desired.header.stamp = stamp;
-			desired.vector.x = safe(vx);
-			desired.vector.y = safe(vy);
-			desired.vector.z = safe(vz);
-
-			// transform to reference frame
-			desired = tf_buffer.transform(desired, reference_frame);
-			setpoint_velocity.header = desired.header;
-
-			// set horizontal velocity
-			if (isfinite(vx) && isfinite(vy)) {
-				setpoint_velocity.vector.x = desired.vector.x;
-				setpoint_velocity.vector.y = desired.vector.y;
-			}
-
-			// set vertical velocity
-			if (isfinite(vz)) {
-				setpoint_velocity.vector.z = desired.vector.z;
-			}
-		}
-
-		// handle yaw
-		if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == POSITION || sp_type == VELOCITY || sp_type == ATTITUDE || sp_type == _YAW) {
-			if (isfinite(yaw)) {
-				setpoint_yaw_type = YAW;
-				QuaternionStamped desired;
-				desired.header.frame_id = frame_id;
-				desired.header.stamp = stamp;
-				desired.quaternion = tf::createQuaternionMsgFromYaw(yaw);
-
-				// transform to reference frame
-				desired = tf_buffer.transform(desired, reference_frame);
-				setpoint_yaw = tf2::getYaw(desired.quaternion);
-				yaw_frame_id = reference_frame;
-
-			} else if (isinf(yaw) && yaw > 0) {
+			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) {
 				// yaw towards
 				setpoint_yaw_type = TOWARDS;
-
-			} else if (yaw_frame_id.empty() || sp_type == _YAW) {
-				// yaw is nan and not set previously OR set_yaw(yaw=nan) was called
+				yaw = 0;
+				setpoint_yaw_rate = 0;
+			} else {
 				setpoint_yaw_type = YAW;
-				setpoint_yaw = tf2::getYaw(local_position.pose.orientation); // set yaw to current yaw
-				yaw_frame_id = local_position.header.frame_id;
+				setpoint_yaw_rate = 0;
+				ps.pose.orientation = tf::createQuaternionMsgFromYaw(yaw);
 			}
+
+			tf_buffer.transform(ps, setpoint_position, reference_frame);
 		}
 
-		// handle roll
-		if (isfinite(roll)) {
-			setpoint_roll = roll;
+		if (sp_type == VELOCITY) {
+			Vector3Stamped vel;
+			vel.header.frame_id = frame_id;
+			vel.header.stamp = stamp;
+			vel.vector.x = vx;
+			vel.vector.y = vy;
+			vel.vector.z = vz;
+			tf_buffer.transform(vel, setpoint_velocity, reference_frame);
 		}
 
-		// handle pitch
-		if (isfinite(pitch)) {
-			setpoint_pitch = pitch;
+		if (sp_type == ATTITUDE || sp_type == RATES) {
+			thrust_msg.thrust = thrust;
 		}
 
-		// handle yaw rate
-		if (isfinite(yaw_rate)) {
-			setpoint_yaw_type = YAW_RATE;
-			setpoint_rates.z = yaw_rate;
-		}
-
-		// handle pitch rate
-		if (isfinite(roll_rate)) {
-			setpoint_rates.x = roll_rate;
-		}
-
-		// handle roll rate
-		if (isfinite(pitch_rate)) {
-			setpoint_rates.y = pitch_rate;
-		}
-
-		// handle thrust
-		if (isfinite(thrust)) {
-			setpoint_thrust = thrust;
+		if (sp_type == RATES) {
+			rates_msg.twist.angular.x = roll_rate;
+			rates_msg.twist.angular.y = pitch_rate;
+			rates_msg.twist.angular.z = yaw_rate;
 		}
 
 		wait_armed = auto_arm;
 
+publish_setpoint:
 		publish(stamp); // calculate initial transformed messages first
 		setpoint_timer.start();
 
-		if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == POSITION) {
-			publishTarget(stamp, true);
+		// publish target frame
+		if (!target.child_frame_id.empty()) {
+			if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == POSITION) {
+				target.header.frame_id = setpoint_position.header.frame_id;
+				target.header.stamp = stamp;
+				target.transform.translation.x = setpoint_position.pose.position.x;
+				target.transform.translation.y = setpoint_position.pose.position.y;
+				target.transform.translation.z = setpoint_position.pose.position.z;
+				target.transform.rotation = setpoint_position.pose.orientation;
+				static_transform_broadcaster->sendTransform(target);
+			}
 		}
 
-		publishState();
-
 		if (auto_arm) {
 			offboardAndArm();
 			wait_armed = false;
@@ -984,42 +785,30 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl
 }
 
 bool navigate(Navigate::Request& req, Navigate::Response& res) {
-	return serve(NAVIGATE, req.x, req.y, req.z, NAN, NAN, NAN, NAN, NAN, req.yaw, NAN, NAN, NAN, NAN, NAN, NAN, req.speed, req.frame_id, req.auto_arm, res.success, res.message);
+	return serve(NAVIGATE, req.x, req.y, req.z, NAN, NAN, NAN, NAN, NAN, req.yaw, NAN, NAN, req.yaw_rate, NAN, NAN, NAN, req.speed, req.frame_id, req.auto_arm, res.success, res.message);
 }
 
 bool navigateGlobal(NavigateGlobal::Request& req, NavigateGlobal::Response& res) {
-	return serve(NAVIGATE_GLOBAL, NAN, NAN, req.z, NAN, NAN, NAN, NAN, NAN, req.yaw, NAN, NAN, NAN, req.lat, req.lon, NAN, req.speed, req.frame_id, req.auto_arm, res.success, res.message);
-}
-
-bool setAltitude(SetAltitude::Request& req, SetAltitude::Response& res) {
-	return serve(_ALTITUDE, NAN, NAN, req.z, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, req.frame_id, false, res.success, res.message);
-}
-
-bool setYaw(SetYaw::Request& req, SetYaw::Response& res) {
-	return serve(_YAW, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, req.yaw, NAN, NAN, NAN, NAN, NAN, NAN, NAN, req.frame_id, false, res.success, res.message);
-}
-
-bool setYawRate(SetYawRate::Request& req, SetYawRate::Response& res) {
-	return serve(_YAW_RATE, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, req.yaw_rate, NAN, NAN, NAN, NAN, "", false, res.success, res.message);
+	return serve(NAVIGATE_GLOBAL, NAN, NAN, req.z, NAN, NAN, NAN, NAN, NAN, req.yaw, NAN, NAN, req.yaw_rate, req.lat, req.lon, NAN, req.speed, req.frame_id, req.auto_arm, res.success, res.message);
 }
 
 bool setPosition(SetPosition::Request& req, SetPosition::Response& res) {
-	return serve(POSITION, req.x, req.y, req.z, NAN, NAN, NAN, NAN, NAN, req.yaw, NAN, NAN, NAN, NAN, NAN, NAN, NAN, req.frame_id, req.auto_arm, res.success, res.message);
+	return serve(POSITION, req.x, req.y, req.z, NAN, NAN, NAN, NAN, NAN, req.yaw, NAN, NAN, req.yaw_rate, NAN, NAN, NAN, NAN, req.frame_id, req.auto_arm, res.success, res.message);
 }
 
 bool setVelocity(SetVelocity::Request& req, SetVelocity::Response& res) {
-	return serve(VELOCITY, NAN, NAN, NAN, req.vx, req.vy, req.vz, NAN, NAN, req.yaw, NAN, NAN, NAN, NAN, NAN, NAN, NAN, req.frame_id, req.auto_arm, res.success, res.message);
+	return serve(VELOCITY, NAN, NAN, NAN, req.vx, req.vy, req.vz, NAN, NAN, req.yaw, NAN, NAN, req.yaw_rate, NAN, NAN, NAN, NAN, req.frame_id, req.auto_arm, res.success, res.message);
 }
 
 bool setAttitude(SetAttitude::Request& req, SetAttitude::Response& res) {
-	return serve(ATTITUDE, NAN, NAN, NAN, NAN, NAN, NAN, req.roll, req.pitch, req.yaw, NAN, NAN, NAN, NAN, NAN, req.thrust, NAN, req.frame_id, req.auto_arm, res.success, res.message);
+	return serve(ATTITUDE, NAN, NAN, NAN, NAN, NAN, NAN, req.pitch, req.roll, req.yaw, NAN, NAN, NAN, NAN, NAN, req.thrust, NAN, req.frame_id, req.auto_arm, res.success, res.message);
 }
 
 bool setRates(SetRates::Request& req, SetRates::Response& res) {
-	return serve(RATES, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, req.roll_rate, req.pitch_rate, req.yaw_rate, NAN, NAN, req.thrust, NAN, "", req.auto_arm, res.success, res.message);
+	return serve(RATES, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, req.pitch_rate, req.roll_rate, req.yaw_rate, NAN, NAN, req.thrust, NAN, "", req.auto_arm, res.success, res.message);
 }
 
-bool land(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)
+bool land([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)
 {
 	try {
 		if (busy)
@@ -1048,7 +837,9 @@ bool land(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)
 		auto start = ros::Time::now();
 		while (ros::ok()) {
 			if (state.mode == "AUTO.LAND") {
-				break;
+				res.success = true;
+				busy = false;
+				return true;
 			}
 			if (ros::Time::now() - start > land_timeout)
 				throw std::runtime_error("Land request timed out");
@@ -1057,18 +848,6 @@ bool land(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)
 			r.sleep();
 		}
 
-		// stop setpoints and invalidate position setpoint
-		setpoint_timer.stop();
-		setpoint_type = NONE;
-		setpoint_position.header.frame_id = "";
-		setpoint_altitude.header.frame_id = "";
-		yaw_frame_id = "";
-		publishState();
-
-		res.success = true;
-		busy = false;
-		return true;
-
 	} catch (const std::exception& e) {
 		res.message = e.what();
 		ROS_INFO("%s", e.what());
@@ -1078,18 +857,6 @@ bool land(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)
 	return false;
 }
 
-bool release(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)
-{
-	setpoint_timer.stop();
-	setpoint_type = NONE;
-	setpoint_position.header.frame_id = "";
-	setpoint_altitude.header.frame_id = "";
-	yaw_frame_id = "";
-	publishState();
-	res.success = true;
-	return true;
-}
-
 int main(int argc, char **argv)
 {
 	ros::init(argc, argv, "simple_offboard");
@@ -1111,8 +878,6 @@ int main(int argc, char **argv)
 	nh_priv.param("check_kill_switch", check_kill_switch, true);
 	nh_priv.param("default_speed", default_speed, 0.5f);
 	nh_priv.param<string>("body_frame", body.child_frame_id, "body");
-	nh_priv.param<string>("terrain_frame", terrain.child_frame_id, "terrain");
-	nh_priv.param<string>("terrain_frame_mode", terrain_frame_mode, "altitude");
 	nh_priv.getParam("reference_frames", reference_frames);
 
 	// Default reference frames
@@ -1148,20 +913,6 @@ int main(int argc, char **argv)
 	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);
 
-	ros::Subscriber altitude_sub;
-	if (!body.child_frame_id.empty() && !terrain.child_frame_id.empty()) {
-		terrain.header.frame_id = local_frame;
-		if (terrain_frame_mode == "altitude") {
-			altitude_sub = nh.subscribe(mavros + "/altitude", 1, &handleAltitude);
-		} else if (terrain_frame_mode == "range") {
-			string range_topic = nh_priv.param("range_topic", string("rangefinder/range"));
-			altitude_sub = nh.subscribe(range_topic, 1, &handleRange);
-		} else {
-			ROS_FATAL("Unknown terrain_frame_mode: %s, valid values: altitude, range", terrain_frame_mode.c_str());
-			ros::shutdown();
-		}
-	}
-
 	// Setpoint publishers
 	position_pub = nh.advertise<PoseStamped>(mavros + "/setpoint_position/local", 1);
 	position_raw_pub = nh.advertise<PositionTarget>(mavros + "/setpoint_raw/local", 1);
@@ -1170,22 +921,15 @@ int main(int argc, char **argv)
 	rates_pub = nh.advertise<TwistStamped>(mavros + "/setpoint_attitude/cmd_vel", 1);
 	thrust_pub = nh.advertise<Thrust>(mavros + "/setpoint_attitude/thrust", 1);
 
-	// State publisher
-	state_pub = nh_priv.advertise<clover::State>("state", 1, true);
-
 	 // Service servers
 	auto gt_serv = nh.advertiseService("get_telemetry", &getTelemetry);
 	auto na_serv = nh.advertiseService("navigate", &navigate);
 	auto ng_serv = nh.advertiseService("navigate_global", &navigateGlobal);
-	auto sl_serv = nh.advertiseService("set_altitude", &setAltitude);
-	auto ya_serv = nh.advertiseService("set_yaw", &setYaw);
-	auto yr_serv = nh.advertiseService("set_yaw_rate", &setYawRate);
 	auto sp_serv = nh.advertiseService("set_position", &setPosition);
 	auto sv_serv = nh.advertiseService("set_velocity", &setVelocity);
 	auto sa_serv = nh.advertiseService("set_attitude", &setAttitude);
 	auto sr_serv = nh.advertiseService("set_rates", &setRates);
 	auto ld_serv = nh.advertiseService("land", &land);
-	auto rl_serv = nh_priv.advertiseService("release", &release);
 
 	// Setpoint timer
 	setpoint_timer = nh.createTimer(ros::Duration(1 / nh_priv.param("setpoint_rate", 30.0)), &publishSetpoint, false, false);
@@ -1193,7 +937,7 @@ int main(int argc, char **argv)
 	position_msg.header.frame_id = local_frame;
 	position_raw_msg.header.frame_id = local_frame;
 	position_raw_msg.coordinate_frame = PositionTarget::FRAME_LOCAL_NED;
-	//rates_msg.header.frame_id = fcu_frame;
+	rates_msg.header.frame_id = fcu_frame;
 
 	ROS_INFO("ready");
 	ros::spin();

clover/src/vpe_publisher.cpp

@@ -11,14 +11,12 @@
 
 #include <string>
 #include <ros/ros.h>
-#include <tf/transform_datatypes.h>
 #include <tf2/transform_datatypes.h>
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
 #include <tf2_ros/static_transform_broadcaster.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 #include <geometry_msgs/TransformStamped.h>
-#include <geometry_msgs/Quaternion.h>
 #include <geometry_msgs/PoseStamped.h>
 #include <geometry_msgs/PoseWithCovarianceStamped.h>
 #include <std_srvs/Trigger.h>
@@ -27,7 +25,7 @@
 using std::string;
 using namespace geometry_msgs;
 
-bool reset_flag = true; // offset should be reset on the start
+bool reset_flag = false;
 string local_frame_id, frame_id, child_frame_id, offset_frame_id;
 tf2_ros::Buffer tf_buffer;
 ros::Publisher vpe_pub;
@@ -35,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_timeout, publish_zero_duration, offset_timeout;
+ros::Duration publish_zero_timout, publish_zero_duration, offset_timeout;
 TransformStamped offset;
 
 void publishZero(const ros::TimerEvent& e)
 {
-	if (!vpe.header.stamp.isZero() && e.current_real - vpe.header.stamp < publish_zero_timeout) return; // have vpe
+	if (!vpe.header.stamp.isZero() && e.current_real - vpe.header.stamp < publish_zero_timout) return; // have vpe
 
-	if (!pose.header.stamp.isZero() && e.current_real - pose.header.stamp < publish_zero_timeout) { // have local position
+	if (!pose.header.stamp.isZero() && e.current_real - pose.header.stamp < publish_zero_timout) { // have local position
 		if (got_local_pos.isZero()) {
 			ROS_INFO("got local position");
 			got_local_pos = e.current_real;
@@ -68,13 +66,6 @@ inline Pose getPose(const PoseStampedConstPtr& pose) { return pose->pose; }
 
 inline Pose getPose(const PoseWithCovarianceStampedConstPtr& pose) { return pose->pose.pose; }
 
-inline void keepYaw(Quaternion& quaternion)
-{
-	tf::Quaternion q;
-	q.setRPY(0, 0, tf::getYaw(quaternion));
-	tf::quaternionTFToMsg(q, quaternion);
-}
-
 template <typename T>
 void callback(const T& msg)
 {
@@ -97,29 +88,10 @@ void callback(const T& msg)
 		if (!offset_frame_id.empty()) {
 			if (reset_flag || msg->header.stamp - vpe.header.stamp > offset_timeout) {
 				// calculate the offset
-				if (!frame_id.empty()) {
-					// calculate from TF
-					offset = tf_buffer.lookupTransform(local_frame_id, frame_id,
-					                                   msg->header.stamp, ros::Duration(0.02));
-					// offset.header.frame_id = vpe.header.frame_id;
-					offset.child_frame_id = offset_frame_id;
-
-				} else {
-					// calculate transform between pose in vpe frame and pose in local frame
-					TransformStamped local_pose = tf_buffer.lookupTransform(local_frame_id, child_frame_id,
-					                                                        msg->header.stamp, ros::Duration(0.02));
-					keepYaw(local_pose.transform.rotation);
-
-					tf::Transform vpeTransform, poseTransform;
-					tf::poseMsgToTF(vpe.pose, vpeTransform);
-					tf::transformMsgToTF(local_pose.transform, poseTransform);
-					tf::Transform offset_tf = vpeTransform.inverseTimes(poseTransform);
-					tf::transformTFToMsg(offset_tf, offset.transform);
-					offset.header.frame_id = local_frame_id;
-					offset.header.stamp = msg->header.stamp;
-					offset.child_frame_id = offset_frame_id;
-				}
-
+				offset = tf_buffer.lookupTransform(local_frame_id, frame_id,
+				                                   msg->header.stamp, ros::Duration(0.02));
+				// offset.header.frame_id = vpe.header.frame_id;
+				offset.child_frame_id = offset_frame_id;
 				br.sendTransform(offset);
 				reset_flag = false;
 				ROS_INFO("offset reset");
@@ -137,7 +109,7 @@ void callback(const T& msg)
 	}
 }
 
-bool reset(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)
+bool reset([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)
 {
 	reset_flag = true;
 	res.success = true;
@@ -150,11 +122,10 @@ int main(int argc, char **argv) {
 
 	tf2_ros::TransformListener tf_listener(tf_buffer);
 
-	nh_priv.param<string>("frame_id", frame_id, ""); // name for used visual pose frame
-	nh_priv.param<string>("offset_frame_id", offset_frame_id, ""); // name for published offset frame
-
-	nh.param<string>("mavros/local_position/frame_id", local_frame_id, "map");
-	nh.param<string>("mavros/local_position/tf/child_frame_id", child_frame_id, "base_link");
+	nh_priv.param<string>("frame_id", frame_id, "");
+	nh_priv.param<string>("offset_frame_id", offset_frame_id, "");
+	nh_priv.param<string>("mavros/local_position/frame_id", local_frame_id, "map");
+	nh_priv.param<string>("mavros/local_position/tf/child_frame_id", child_frame_id, "base_link");
 	offset_timeout = ros::Duration(nh_priv.param("offset_timeout", 3.0));
 
 	if (!frame_id.empty()) {
@@ -173,7 +144,7 @@ int main(int argc, char **argv) {
 	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_timeout = ros::Duration(nh_priv.param("force_init_timeout", 5.0));
+		publish_zero_timout = ros::Duration(nh_priv.param("force_init_timeout", 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/src/www

@@ -1,4 +0,0 @@
-#!/usr/bin/env bash
-
-export ROSWWW_DEFAULT=clover
-rosrun roswww_static update

clover/srv/GetTelemetry.srv

@@ -13,11 +13,11 @@ float32 alt
 float32 vx
 float32 vy
 float32 vz
-float32 roll
 float32 pitch
+float32 roll
 float32 yaw
-float32 roll_rate
 float32 pitch_rate
+float32 roll_rate
 float32 yaw_rate
 float32 voltage
 float32 cell_voltage

clover/srv/Navigate.srv

@@ -2,6 +2,7 @@ float32 x
 float32 y
 float32 z
 float32 yaw
+float32 yaw_rate
 float32 speed
 string frame_id
 bool auto_arm

clover/srv/NavigateGlobal.srv

@@ -2,6 +2,7 @@ float64 lat
 float64 lon
 float32 z
 float32 yaw
+float32 yaw_rate
 float32 speed
 string frame_id
 bool auto_arm

clover/srv/SetAltitude.srv

@@ -1,5 +0,0 @@
-float32 z
-string frame_id
----
-bool success
-string message

clover/srv/SetAttitude.srv

@@ -1,5 +1,5 @@
-float32 roll
 float32 pitch
+float32 roll
 float32 yaw
 float32 thrust
 string frame_id

clover/srv/SetPosition.srv

@@ -2,6 +2,7 @@ float32 x
 float32 y
 float32 z
 float32 yaw
+float32 yaw_rate
 string frame_id
 bool auto_arm
 ---

clover/srv/SetRates.srv

@@ -1,5 +1,5 @@
-float32 roll_rate
 float32 pitch_rate
+float32 roll_rate
 float32 yaw_rate
 float32 thrust
 bool auto_arm

clover/srv/SetVelocity.srv

@@ -2,6 +2,7 @@ float32 vx
 float32 vy
 float32 vz
 float32 yaw
+float32 yaw_rate
 string frame_id
 bool auto_arm
 ---

clover/srv/SetYaw.srv

@@ -1,5 +0,0 @@
-float32 yaw
-string frame_id
----
-bool success
-string message

clover/srv/SetYawRate.srv

@@ -1,4 +0,0 @@
-float32 yaw_rate
----
-bool success
-string message

clover/test/basic.py

@@ -3,7 +3,6 @@ import rospy
 import pytest
 from mavros_msgs.msg import State
 from clover import srv
-import time
 
 @pytest.fixture()
 def node():
@@ -25,7 +24,6 @@ def test_simple_offboard_services_available():
     rospy.wait_for_service('set_attitude', timeout=5)
     rospy.wait_for_service('set_rates', timeout=5)
     rospy.wait_for_service('land', timeout=5)
-    rospy.wait_for_service('simple_offboard/release', timeout=5)
 
 def test_web_video_server(node):
     try:
@@ -61,18 +59,3 @@ def test_blocks(node):
 
     t.join()
     assert wait_print.result == 'test'
-
-def test_long_callback():
-    from clover import long_callback
-    from time import sleep
-
-    # very basic test for long_callback
-    @long_callback
-    def cb(i):
-        cb.counter += i
-    cb.counter = 0
-    cb(2)
-    sleep(0.1)
-    cb(3)
-    sleep(1)
-    assert cb.counter == 5

clover/test/basic.test

@@ -37,9 +37,6 @@
 
     <node name="clover_blocks" pkg="clover_blocks" type="clover_blocks" output="screen" required="true"/>
 
-    <node pkg="topic_tools" name="main_camera_throttle" type="throttle" ns="main_camera"
-        args="messages image_raw 5.0 image_raw_throttled" 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/test/offboard.py

@@ -1,437 +0,0 @@
-import rospy
-import pytest
-from pytest import approx
-import threading
-import mavros_msgs.msg
-from mavros_msgs.srv import SetMode
-from geometry_msgs.msg import PoseStamped
-from clover import srv
-from clover.msg import State
-from std_srvs.srv import Trigger
-from math import nan, inf
-import tf2_ros
-import tf2_geometry_msgs
-
-@pytest.fixture()
-def node():
-    return rospy.init_node('offboard_test', anonymous=True)
-
-@pytest.fixture
-def tf_buffer():
-    buf = tf2_ros.Buffer()
-    tf2_ros.TransformListener(buf)
-    return buf
-
-def get_state():
-    return rospy.wait_for_message('/simple_offboard/state', State, timeout=1)
-
-def get_navigate_target(tf_buffer):
-    target = tf_buffer.lookup_transform('map', 'navigate_target', rospy.get_rostime(), rospy.Duration(1))
-    assert target.child_frame_id == 'navigate_target'
-    return target
-
-def test_offboard(node, tf_buffer):
-    navigate = rospy.ServiceProxy('navigate', srv.Navigate)
-    set_position = rospy.ServiceProxy('set_position', srv.SetPosition)
-    set_altitude = rospy.ServiceProxy('set_altitude', srv.SetAltitude)
-    set_yaw = rospy.ServiceProxy('set_yaw', srv.SetYaw)
-    set_yaw_rate = rospy.ServiceProxy('set_yaw_rate', srv.SetYawRate)
-    set_velocity = rospy.ServiceProxy('set_velocity', srv.SetVelocity)
-    set_attitude = rospy.ServiceProxy('set_attitude', srv.SetAttitude)
-    set_rates = rospy.ServiceProxy('set_rates', srv.SetRates)
-    get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry)
-    land = rospy.ServiceProxy('land', Trigger)
-
-    res = navigate()
-    assert res.success == False
-    assert res.message.startswith('State timeout')
-
-    telem = get_telemetry()
-    assert telem.connected == False
-
-    # mocked state publisher
-    state_pub = rospy.Publisher('/mavros/state', mavros_msgs.msg.State, latch=True, queue_size=1)
-    state_msg = mavros_msgs.msg.State(mode='OFFBOARD', armed=True)
-
-    def publish_state():
-        r = rospy.Rate(2)
-        while not rospy.is_shutdown():
-            state_msg.header.stamp = rospy.Time.now()
-            state_pub.publish(state_msg)
-            r.sleep()
-
-    # start publishing state
-    threading.Thread(target=publish_state, daemon=True).start()
-    rospy.sleep(0.5)
-
-    # set_mode service mock
-    def set_mode(req):
-        state_msg.mode = req.custom_mode # set mocked mode to requested
-        return True,
-
-    rospy.Service('/mavros/set_mode', SetMode, set_mode)
-
-    telem = get_telemetry()
-    assert telem.connected == False
-
-    res = navigate()
-    assert res.success == False
-    assert res.message.startswith('No connection to FCU')
-
-    state_msg.connected = True
-    rospy.sleep(1)
-
-    telem = get_telemetry()
-    assert telem.connected == True
-
-    res = navigate()
-    assert res.success == False
-    assert res.message.startswith('No local position')
-
-    local_position_pub = rospy.Publisher('/mavros/local_position/pose', PoseStamped, latch=True, queue_size=1)
-    local_position_msg = PoseStamped()
-    local_position_msg.header.frame_id = 'map'
-    local_position_msg.pose.position.x = 1
-    local_position_msg.pose.position.y = 2
-    local_position_msg.pose.position.z = 3
-    local_position_msg.pose.orientation.w = 1
-
-    def publish_local_position():
-        r = rospy.Rate(30)
-        while not rospy.is_shutdown():
-            local_position_msg.header.stamp = rospy.Time.now()
-            local_position_pub.publish(local_position_msg)
-            r.sleep()
-
-    # start publishing local position
-    threading.Thread(target=publish_local_position, daemon=True).start()
-    rospy.sleep(0.5)
-
-    # check body frame
-    body = tf_buffer.lookup_transform('map', 'body', rospy.get_rostime(), rospy.Duration(1))
-    assert body.child_frame_id == 'body'
-    assert body.transform.translation.x == approx(1)
-    assert body.transform.translation.y == approx(2)
-    assert body.transform.translation.z == approx(3)
-
-    res = navigate(x=3, y=2, z=1, frame_id='map')
-    assert res.success == True
-    state = get_state()
-    assert state.mode == State.MODE_NAVIGATE
-    assert state.yaw_mode == State.YAW_MODE_YAW
-    assert state.x == 3
-    assert state.y == 2
-    assert state.z == 1
-    assert state.yaw == 0
-    assert state.xy_frame_id == 'map'
-    assert state.z_frame_id == 'map'
-    assert state.yaw_frame_id == 'map'
-    target = get_navigate_target(tf_buffer)
-    assert target.header.frame_id == 'map'
-    assert target.transform.translation.x == approx(3)
-    assert target.transform.translation.y == approx(2)
-    assert target.transform.translation.z == approx(1)
-    assert target.transform.rotation.x == 0
-    assert target.transform.rotation.y == 0
-    assert target.transform.rotation.z == 0
-    assert target.transform.rotation.w == 1
-
-    # try to set only the y
-    res = navigate(x=nan, y=1, z=nan)
-    assert res.success == False
-    assert res.message.startswith('x and y can be set only together')
-
-    # set z in body frame
-    res = navigate(x=nan, y=nan, z=1, frame_id='body')
-    assert res.success == True
-    state = get_state()
-    assert state.mode == State.MODE_NAVIGATE
-    assert state.yaw_mode == State.YAW_MODE_YAW
-    assert state.x == 3
-    assert state.y == 2
-    assert state.z == 4
-    assert state.yaw == 0
-    assert state.xy_frame_id == 'map'
-    assert state.z_frame_id == 'map'
-    assert state.yaw_frame_id == 'map'
-
-    # set xy in test frame
-    res = navigate(x=1, y=2, z=nan, frame_id='test')
-    assert res.success == True
-    state = get_state()
-    assert state.mode == State.MODE_NAVIGATE
-    assert state.yaw_mode == State.YAW_MODE_YAW
-    assert state.x == 1
-    assert state.y == 2
-    assert state.z == 4
-    assert state.yaw == 0
-    assert state.xy_frame_id == 'test'
-    assert state.z_frame_id == 'map'
-    assert state.yaw_frame_id == 'test'
-
-    # auto_arm should not invalidate the setpoint if not effective
-    res = navigate(x=nan, y=nan, z=1, frame_id='map', auto_arm=True)
-    assert res.success == True
-    state = get_state()
-    assert state.mode == State.MODE_NAVIGATE
-    assert state.yaw_mode == State.YAW_MODE_YAW
-    assert state.x == 1
-    assert state.y == 2
-    assert state.z == 1
-    assert state.yaw == 0
-    assert state.xy_frame_id == 'test'
-    assert state.z_frame_id == 'map'
-    assert state.yaw_frame_id == 'map'
-
-    # auto_arm should invalidate the setpoint if effective
-    state_msg.mode = 'STABILIZED' # pretend we are not in OFFBOARD mode
-    rospy.sleep(1)
-    res = navigate(x=nan, y=nan, z=1, frame_id='map', auto_arm=True)
-    assert res.success == True
-    state = get_state()
-    assert state.mode == State.MODE_NAVIGATE
-    assert state.yaw_mode == State.YAW_MODE_YAW
-    assert state.x == 1
-    assert state.y == 2
-    assert state.z == 1
-    assert state.yaw == 0
-    assert state.xy_frame_id == 'map'
-    assert state.z_frame_id == 'map'
-    assert state.yaw_frame_id == 'map'
-    state_msg.mode = 'OFFBOARD'
-    rospy.sleep(1)
-
-    # set_attitude should invalidate the setpoint
-    res = set_attitude()
-    assert res.success == True
-
-    res = navigate(x=5, y=6, z=nan, yaw=nan, frame_id='map')
-    assert res.success == True
-    state = get_state()
-    assert state.mode == State.MODE_NAVIGATE
-    assert state.yaw_mode == State.YAW_MODE_YAW
-    assert state.x == 5
-    assert state.y == 6
-    assert state.z == 3
-    assert state.yaw == 0
-    assert state.xy_frame_id == 'map'
-    assert state.z_frame_id == 'map'
-    assert state.yaw_frame_id == 'map'
-
-    # test set_altitude
-    res = set_altitude(z=7, frame_id='test')
-    assert res.success == True
-    state = get_state()
-    assert state.mode == State.MODE_NAVIGATE
-    assert state.yaw_mode == State.YAW_MODE_YAW
-    assert state.x == 5
-    assert state.y == 6
-    assert state.z == 7
-    assert state.yaw == 0
-    assert state.xy_frame_id == 'map'
-    assert state.z_frame_id == 'test'
-    assert state.yaw_frame_id == 'map'
-
-    # test set_yaw
-    res = set_yaw(yaw=0.5, frame_id='test2')
-    assert res.success == True
-    state = get_state()
-    assert state.mode == State.MODE_NAVIGATE
-    assert state.yaw_mode == State.YAW_MODE_YAW
-    assert state.x == 5
-    assert state.y == 6
-    assert state.z == 7
-    assert state.yaw == 0.5
-    assert state.xy_frame_id == 'map'
-    assert state.z_frame_id == 'test'
-    assert state.yaw_frame_id == 'test2'
-
-    # test set_yaw_rate
-    res = set_yaw_rate(yaw_rate=2)
-    assert res.success == True
-    state = get_state()
-    assert state.mode == State.MODE_NAVIGATE
-    assert state.yaw_mode == State.YAW_MODE_YAW_RATE
-    assert state.x == 5
-    assert state.y == 6
-    assert state.z == 7
-    assert state.yaw_rate == 2
-    assert state.xy_frame_id == 'map'
-    assert state.z_frame_id == 'test'
-
-    # navigate(yaw=nan) should keep yaw rate mode
-    res = navigate(x=nan, y=nan, z=nan, yaw=nan)
-    assert res.success == True
-    state = get_state()
-    assert state.mode == State.MODE_NAVIGATE
-    assert state.yaw_mode == State.YAW_MODE_YAW_RATE
-    assert state.x == 5
-    assert state.y == 6
-    assert state.z == 7
-    assert state.yaw_rate == 2
-    assert state.xy_frame_id == 'map'
-    assert state.z_frame_id == 'test'
-
-    # set_yaw(nan) should change back to yaw mode
-    res = set_yaw(yaw=nan)
-    assert res.success == True
-    state = get_state()
-    assert state.mode == State.MODE_NAVIGATE
-    assert state.yaw_mode == State.YAW_MODE_YAW
-    assert state.yaw == 0
-    assert state.yaw_frame_id == 'map'
-
-    # test set_position
-    res = set_position(x=nan, y=nan, z=13, yaw=nan, frame_id='test2')
-    assert res.success == True
-    state = get_state()
-    assert state.mode == State.MODE_POSITION
-    assert state.yaw_mode == State.YAW_MODE_YAW
-    assert state.x == 5
-    assert state.y == 6
-    assert state.z == 13
-    assert state.yaw == 0
-    assert state.xy_frame_id == 'map'
-    assert state.z_frame_id == 'test2'
-    assert state.yaw_frame_id == 'map'
-
-    # set_altitude should not change the mode
-    res = set_altitude(z=3, frame_id='test')
-    assert res.success == True
-    state = get_state()
-    assert state.mode == State.MODE_POSITION
-    assert state.yaw_mode == State.YAW_MODE_YAW
-    assert state.x == 5
-    assert state.y == 6
-    assert state.z == 3
-    assert state.yaw == 0
-    assert state.xy_frame_id == 'map'
-    assert state.z_frame_id == 'test'
-    assert state.yaw_frame_id == 'map'
-
-    # set_yaw should not change the main mode
-    res = set_yaw(yaw=1, frame_id='test2')
-    assert res.success == True
-    state = get_state()
-    assert state.mode == State.MODE_POSITION
-    assert state.yaw_mode == State.YAW_MODE_YAW
-    assert state.x == 5
-    assert state.y == 6
-    assert state.z == 3
-    assert state.yaw == 1
-    assert state.xy_frame_id == 'map'
-    assert state.z_frame_id == 'test'
-    assert state.yaw_frame_id == 'test2'
-
-    # test set_velocity
-    res = set_velocity(vx=1, frame_id='body')
-    state = get_state()
-    assert state.mode == State.MODE_VELOCITY
-    assert state.yaw_mode == State.YAW_MODE_YAW
-    assert state.vx == 1
-    assert state.vy == 0
-    assert state.vz == 0
-    assert state.yaw == 0
-    assert state.xy_frame_id == 'map'
-    assert state.z_frame_id == 'map'
-    assert state.yaw_frame_id == 'map'
-
-    # set_altitude should not work in velocity mode
-    res = set_altitude(z=3, frame_id='test')
-    assert res.success == False
-    assert res.message.startswith('Altitude cannot be set in')
-
-    # test set_attitude
-    res = set_attitude(roll=0.1, pitch=0.2, yaw=0.3, thrust=0.5)
-    assert res.success == True
-    state = get_state()
-    assert state.mode == State.MODE_ATTITUDE
-    assert state.yaw_mode == State.YAW_MODE_YAW
-    assert state.roll == approx(0.1)
-    assert state.pitch == approx(0.2)
-    assert state.yaw == approx(0.3)
-    assert state.thrust == approx(0.5)
-    assert state.yaw_frame_id == 'map'
-    msg = rospy.wait_for_message('/mavros/setpoint_attitude/attitude', PoseStamped, timeout=3)
-    # Tait-Bryan ZYX angle (rzyx) converted to quaternion
-    assert msg.pose.orientation.x == approx(0.0342708)
-    assert msg.pose.orientation.y == approx(0.10602051)
-    assert msg.pose.orientation.z == approx(0.14357218)
-    assert msg.pose.orientation.w == approx(0.98334744)
-    msg = rospy.wait_for_message('/mavros/setpoint_attitude/thrust', mavros_msgs.msg.Thrust, timeout=3)
-    assert msg.thrust == approx(0.5)
-
-    # set_yaw should work in attitude mode
-    res = set_yaw(yaw=0.7, frame_id='test2')
-    assert res.success == True
-    state = get_state()
-    assert state.mode == State.MODE_ATTITUDE
-    assert state.yaw_mode == State.YAW_MODE_YAW
-    assert state.roll == approx(0.1)
-    assert state.pitch == approx(0.2)
-    assert state.yaw == approx(0.7)
-    assert state.thrust == approx(0.5)
-    assert state.yaw_frame_id == 'test2'
-
-    # set_yaw_rate should not work in attitude mode
-    res = set_yaw_rate(yaw_rate=0.3)
-    assert res.success == False
-    assert res.message.startswith('Yaw rate cannot be set in')
-
-    # test set_rates
-    res = set_rates(roll_rate=nan, pitch_rate=nan, yaw_rate=0.3, thrust=0.6)
-    assert res.success == True
-    state = get_state()
-    assert state.mode == State.MODE_RATES
-    assert state.yaw_mode == State.YAW_MODE_YAW_RATE
-    assert state.roll_rate == approx(0)
-    assert state.pitch_rate == approx(0)
-    assert state.yaw_rate == approx(0.3)
-    assert state.thrust == approx(0.6)
-    msg = rospy.wait_for_message('/mavros/setpoint_raw/attitude', mavros_msgs.msg.AttitudeTarget, timeout=3)
-    assert msg.thrust == approx(0.6)
-
-    res = set_rates(roll_rate=0.3, pitch_rate=0.2, yaw_rate=0.1, thrust=0.4)
-    assert res.success == True
-    state = get_state()
-    assert state.mode == State.MODE_RATES
-    assert state.yaw_mode == State.YAW_MODE_YAW_RATE
-    assert state.roll_rate == approx(0.3)
-    assert state.pitch_rate == approx(0.2)
-    assert state.yaw_rate == approx(0.1)
-    assert state.thrust == approx(0.4)
-
-    res = set_rates(roll_rate=nan, pitch_rate=nan, yaw_rate=nan, thrust=0.3)
-    assert res.success == True
-    state = get_state()
-    assert state.mode == State.MODE_RATES
-    assert state.yaw_mode == State.YAW_MODE_YAW_RATE
-    assert state.roll_rate == approx(0.3)
-    assert state.pitch_rate == approx(0.2)
-    assert state.yaw_rate == approx(0.1)
-    assert state.thrust == approx(0.3)
-    msg = rospy.wait_for_message('/mavros/setpoint_raw/attitude', mavros_msgs.msg.AttitudeTarget, timeout=3)
-    assert msg.type_mask == mavros_msgs.msg.AttitudeTarget.IGNORE_ATTITUDE
-    assert msg.body_rate.x == approx(0.3)
-    assert msg.body_rate.y == approx(0.2)
-    assert msg.body_rate.z == approx(0.1)
-
-    # set_yaw_rate should work in rates mode
-    res = set_yaw_rate(yaw_rate=0.4)
-    assert res.success == True
-    state = get_state()
-    assert state.mode == State.MODE_RATES
-    assert state.yaw_mode == State.YAW_MODE_YAW_RATE
-    assert state.roll_rate == approx(0.3)
-    assert state.pitch_rate == approx(0.2)
-    assert state.yaw_rate == approx(0.4)
-    assert state.thrust == approx(0.3)
-
-    res = set_rates(roll_rate=inf)
-    assert res.success == False
-    assert res.message == 'roll_rate argument cannot be Inf'
-
-    # test land service
-    res = land()
-    assert res.success == True
-    assert state_msg.mode == 'AUTO.LAND' # check that the mode was set correctly

clover/test/offboard.test

@@ -1,10 +0,0 @@
-<launch>
-    <node name="simple_offboard" pkg="clover" type="simple_offboard" required="true" output="screen"/>
-
-    <node pkg="tf2_ros" type="static_transform_publisher" name="test_frame" args="10 20 30 0 0 0 map test"/>
-
-    <node pkg="tf2_ros" type="static_transform_publisher" name="test2_frame" args="100 200 300 0 0 0 map test2"/>
-
-    <param name="test_module" value="$(find clover)/test/offboard.py"/>
-    <test test-name="offboard_test" pkg="ros_pytest" type="ros_pytest_runner"/>
-</launch>

clover/www/js/topics.js

@@ -40,7 +40,6 @@ function viewTopicsList() {
 let rosdistro;
 
 function viewTopic(topic) {
-	let counter = 0;
 	let index = '<a href=topics.html>Topics</a>';
 	title.innerHTML = `${index}: ${topic}`;
 	topicMessage.style.display = 'block';
@@ -52,11 +51,10 @@ function viewTopic(topic) {
 	});
 
 	new ROSLIB.Topic({ ros: ros, name: topic }).subscribe(function(msg) {
-		counter++;
 		document.title = topic;
 		if (mouseDown) return;
 
-		if (msg.header && msg.header.stamp) {
+		if (msg.header.stamp) {
 			if (params.date || params.offset) {
 				let date = new Date(msg.header.stamp.secs * 1e3 + msg.header.stamp.nsecs * 1e-6);
 				if (params.date) msg.header.date = date.toISOString();
@@ -64,8 +62,7 @@ function viewTopic(topic) {
 			}
 		}
 
-		let txt = `<div class=counter>${counter} received</div>${yamlStringify(msg)}`; // JSON.stringify(msg, null, 4);
-		topicMessage.innerHTML = txt;
+		topicMessage.innerHTML = yamlStringify(msg); // JSON.stringify(msg, null, 4);
 	});
 }
 

clover/www/topics.html

@@ -15,7 +15,6 @@
 				white-space: pre;
 				font-family: monospace;
 			}
-			.counter { color: #b9b9b9; margin-bottom: 1em; }
 			#topic-type { font-family: monospace; font-size: 0.5em; vertical-align: super; font-weight: normal; }
 			.topic { font-family: monospace; }
 			body.closed { background-color: rgb(207, 207, 207); }

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/noetic/api/std_srvs/html/srv/Trigger.html)) – terminate the running program.
+* `~stop` ([*std_srvs/Trigger*](http://docs.ros.org/melodic/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,12 +45,11 @@ http://<hostname>/clover_blocks/?navigate_tolerance=0.5&sleep_time=0.1
 
 #### Published
 
-* `~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/noetic/api/std_msgs/html/msg/String.html)) – current executing block (maximum topic rate is limited).
-* `~print` ([*std_msgs/String*](http://docs.ros.org/noetic/api/std_msgs/html/msg/String.html)) – user program output messages (published in *print* blocks).
-* `~error` ([*std_msgs/String*](http://docs.ros.org/noetic/api/std_msgs/html/msg/String.html)) – user program errors and exceptions.
+* `~running` ([*std_msgs/Bool*](http://docs.ros.org/melodic/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).
+* `~error` ([*std_msgs/String*](http://docs.ros.org/melodic/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/noetic/api/std_msgs/html/msg/String.html)) – user input response.
+* `~prompt/<request_id>` ([*std_msgs/String*](http://docs.ros.org/melodic/api/std_msgs/html/msg/String.html)) – user input response.

clover_blocks/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>clover_blocks</name>
-  <version>0.24.0</version>
+  <version>0.23.0</version>
   <description>Blockly programming support for Clover</description>
   <maintainer email="okalachev@gmail.com">Oleg Kalachev</maintainer>
   <license>MIT</license>

clover_blocks/www/blocks.js

@@ -15,7 +15,6 @@ const COLOR_GPIO = 200;
 const DOCS_URL = 'https://clover.coex.tech/en/blocks.html';
 
 var frameIds = [["body", "BODY"], ["markers map", "ARUCO_MAP"], ["marker", "ARUCO"], ["last navigate target", "NAVIGATE_TARGET"], ["map", "MAP"]];
-var frameIdsWithTerrain = frameIds.concat([["terrain", "TERRAIN"]]);
 
 function considerFrameId(e) {
 	if (!(e instanceof Blockly.Events.Change || e instanceof Blockly.Events.Create)) return;
@@ -23,7 +22,7 @@ function considerFrameId(e) {
 	var frameId = this.getFieldValue('FRAME_ID');
 	// set appropriate coordinates labels
 	if (this.getInput('X')) { // block has x-y-z fields
-		if (frameId == 'BODY' || frameId == 'NAVIGATE_TARGET' || frameId == 'BASE_LINK' || frameId == 'TERRAIN') {
+		if (frameId == 'BODY' || frameId == 'NAVIGATE_TARGET' || frameId == 'BASE_LINK') {
 			this.getInput('X').fieldRow[0].setValue('forward');
 			this.getInput('Y').fieldRow[0].setValue('left');
 			this.getInput('Z').fieldRow[0].setValue('up');
@@ -60,8 +59,8 @@ function updateSetpointBlock(e) {
 	this.getInput('VY').setVisible(velocity);
 	this.getInput('VZ').setVisible(velocity);
 	this.getInput('YAW').setVisible(attitude);
-	this.getInput('ROLL').setVisible(attitude);
 	this.getInput('PITCH').setVisible(attitude);
+	this.getInput('ROLL').setVisible(attitude);
 	this.getInput('THRUST').setVisible(attitude);
 	this.getInput('RELATIVE_TO').setVisible(type != 'RATES');
 
@@ -74,7 +73,7 @@ function updateSetpointBlock(e) {
 
 Blockly.Blocks['navigate'] = {
 	init: function () {
-		let navFrameId = frameIdsWithTerrain.slice();
+		let navFrameId = frameIds.slice();
 		navFrameId.push(['global', 'GLOBAL_LOCAL'])
 		navFrameId.push(['global, WGS 84 alt.', 'GLOBAL'])
 		this.appendDummyInput()
@@ -164,14 +163,14 @@ Blockly.Blocks['setpoint'] = {
 		this.appendValueInput("VZ")
 			.setCheck("Number")
 			.appendField("vz");
-		this.appendValueInput("ROLL")
-			.setCheck("Number")
-			.appendField("roll")
-			.setVisible(false);
 		this.appendValueInput("PITCH")
 			.setCheck("Number")
 			.appendField("pitch")
 			.setVisible(false);
+		this.appendValueInput("ROLL")
+			.setCheck("Number")
+			.appendField("roll")
+			.setVisible(false);
 		this.appendValueInput("YAW")
 			.setCheck("Number")
 			.appendField("yaw")
@@ -214,7 +213,7 @@ Blockly.Blocks['get_position'] = {
 			.appendField("current")
 			.appendField(new Blockly.FieldDropdown([["x", "X"], ["y", "Y"], ["z", "Z"], ["vx", "VX"], ["vy", "VY"], ["vz", "VZ"]]), "FIELD")
 			.appendField("relative to")
-			.appendField(new Blockly.FieldDropdown(frameIdsWithTerrain), "FRAME_ID");
+			.appendField(new Blockly.FieldDropdown(frameIds), "FRAME_ID");
 		this.appendValueInput("ID")
 			.setCheck("Number")
 			.appendField("with ID")
@@ -248,7 +247,7 @@ Blockly.Blocks['get_attitude'] = {
 	init: function () {
 		this.appendDummyInput()
 			.appendField("current")
-			.appendField(new Blockly.FieldDropdown([["roll", "ROLL"], ["pitch", "PITCH"], ["roll rate", "ROLL_RATE"], ["pitch rate", "PITCH_RATE"], ["yaw rate", "YAW_RATE"]]), "FIELD");
+			.appendField(new Blockly.FieldDropdown([["pitch", "PITCH"], ["roll", "ROLL"], ["pitch rate", "PITCH_RATE"], ["roll rate", "ROLL_RATE"], ["yaw rate", "YAW_RATE"]]), "FIELD");
 		this.setOutput(true, "Number");
 		this.setColour(COLOR_STATE);
 		this.setTooltip("Returns current orientation or angle rates in degree or degree per second (not radian).");
@@ -269,19 +268,6 @@ Blockly.Blocks['voltage'] = {
 	}
 };
 
-Blockly.Blocks['get_rc'] = {
-	init: function () {
-		this.appendDummyInput()
-			.appendField("RC channel")
-		this.appendValueInput("CHANNEL")
-			.setCheck("Number");
-		this.setInputsInline(true);
-		this.setOutput(true, "Number");
-		this.setColour(COLOR_STATE);
-		this.setTooltip("Returns current RC channel value.");
-		this.setHelpUrl(DOCS_URL + '#' + this.type);
-	}
-}
 
 Blockly.Blocks['armed'] = {
 	init: function () {
@@ -523,7 +509,7 @@ Blockly.Blocks['distance'] = {
 			.appendField("z");
 		this.appendDummyInput()
 			.appendField("relative to")
-			.appendField(new Blockly.FieldDropdown([["markers map", "ARUCO_MAP"], ["marker", "ARUCO"], ["last navigate target", "NAVIGATE_TARGET"], ["terrain", "TERRAIN"]]), "FRAME_ID");
+			.appendField(new Blockly.FieldDropdown([["markers map", "ARUCO_MAP"], ["marker", "ARUCO"], ["last navigate target", "NAVIGATE_TARGET"]]), "FRAME_ID");
 		this.appendValueInput("ID")
 			.setCheck("Number")
 			.appendField("with ID")

clover_blocks/www/index.html

@@ -69,8 +69,8 @@
 				<value name="VX"><shadow type="math_number"><field name="NUM">0</field></shadow></value>
 				<value name="VY"><shadow type="math_number"><field name="NUM">0</field></shadow></value>
 				<value name="VZ"><shadow type="math_number"><field name="NUM">0</field></shadow></value>
-				<value name="ROLL"><shadow type="math_number"><field name="NUM">0</field></shadow></value>
 				<value name="PITCH"><shadow type="math_number"><field name="NUM">0</field></shadow></value>
+				<value name="ROLL"><shadow type="math_number"><field name="NUM">0</field></shadow></value>
 				<value name="YAW"><shadow type="math_number"><field name="NUM">0</field></shadow></value>
 				<value name="THRUST"><shadow type="math_number"><field name="NUM">0.5</field></shadow></value>
 				<value name="ID"><shadow type="math_number"><field name="NUM">0</field></shadow></value>
@@ -100,9 +100,6 @@
 			<block type="mode"></block>
 			<block type="armed"></block>
 			<block type="voltage"></block>
-			<block type="get_rc">
-				<value name="CHANNEL"><shadow type="math_number"><field name="NUM">0</field></shadow></value>
-			</block>
 		</category>
 		<category name="LED" colour="#02d754">
 			<block type="set_effect">

clover_blocks/www/python.js

@@ -81,10 +81,7 @@ function generateROSDefinitions() {
 		code += `get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry)\n`;
 		code += `navigate = rospy.ServiceProxy('navigate', srv.Navigate)\n`;
 		if (rosDefinitions.navigateGlobal) {
-			code += `navigate_global = rospy.ServiceProxy('navigate_global', srv.NavigateGlobal)\n`;
-		}
-		if (rosDefinitions.setYaw) {
-			code += `set_yaw = rospy.ServiceProxy('set_yaw', srv.SetYaw)\n`;
+		code += `navigate_global = rospy.ServiceProxy('navigate_global', srv.NavigateGlobal)\n`;
 		}
 		if (rosDefinitions.setVelocity) {
 			code += `set_velocity = rospy.ServiceProxy('set_velocity', srv.SetVelocity)\n`;
@@ -279,11 +276,10 @@ Blockly.Python.angle = function(block) {
 }
 
 Blockly.Python.set_yaw = function(block) {
-	rosDefinitions.setYaw = true;
 	simpleOffboard();
 	let yaw = Blockly.Python.valueToCode(block, 'YAW', Blockly.Python.ORDER_NONE);
 	let frameId = buildFrameId(block);
-	let code = `set_yaw(yaw=${yaw}, frame_id=${frameId})\n`;
+	let code = `navigate(x=float('nan'), y=float('nan'), z=float('nan'), yaw=${yaw}, frame_id=${frameId})\n`;
 	if (block.getFieldValue('WAIT') == 'TRUE') {
 		rosDefinitions.waitYaw = true;
 		simpleOffboard();
@@ -332,11 +328,11 @@ Blockly.Python.setpoint = function(block) {
 	} else if (type == 'ATTITUDE') {
 		rosDefinitions.setAttitude = true;
 		simpleOffboard();
-		return `set_attitude(roll=${roll}, pitch=${pitch}, yaw=${yaw}, thrust=${thrust}, frame_id=${frameId})\n`;
+		return `set_attitude(pitch=${pitch}, roll=${roll}, yaw=${yaw}, thrust=${thrust}, frame_id=${frameId})\n`;
 	} else if (type == 'RATES') {
 		rosDefinitions.setRates = true;
 		simpleOffboard();
-		return `set_rates(roll_rate=${roll}, pitch_rate=${pitch}, yaw_rate=${yaw}, thrust=${thrust})\n`;
+		return `set_rates(pitch_rate=${pitch}, roll_rate=${roll}, yaw_rate=${yaw}, thrust=${thrust})\n`;
 	}
 }
 
@@ -402,12 +398,6 @@ Blockly.Python.voltage = function(block) {
 	return [code, Blockly.Python.ORDER_FUNCTION_CALL];
 }
 
-Blockly.Python.get_rc = function(block) {
-	Blockly.Python.definitions_['import_rcin'] = 'from mavros_msgs.msg import RCIn';
-	var channel = Blockly.Python.valueToCode(block, 'CHANNEL', Blockly.Python.ORDER_NONE);
-	return [`rospy.wait_for_message('mavros/rc/in', RCIn).channels[${channel}]`, Blockly.Python.ORDER_FUNCTION_CALL]
-}
-
 function parseColor(color) {
 	return {
 		r: parseInt(color.substr(2, 2), 16),

clover_description/package.xml

@@ -1,6 +1,6 @@
 <package format="2">
   <name>clover_description</name>
-  <version>0.24.0</version>
+  <version>0.23.0</version>
   <description>The clover_description package provides URDF models of the Clover series of quadcopters.</description>
 
   <maintainer email="sfalexrog@gmail.com">Alexey Rogachevskiy</maintainer>

clover_description/urdf/sensors/distance_sensor.urdf.xacro

@@ -2,7 +2,7 @@
 <robot name="rpi_camera" xmlns:xacro="http://ros.org/wiki/xacro">
   <xacro:include filename="../camera_sensor.urdf.xacro"/>
 
-  <xacro:macro name="distance_sensor" params="name:=lidar_vl53l1x parent x:=0 y:=0 z:=0 roll:=0 pitch:=0 yaw:=0 range_min:=0.01 range_max:=4.0 resolution:=0.01 rate:=10 fov:=0.471239">
+  <xacro:macro name="distance_sensor" params="name:=lidar_vl53l1x parent x:=0 y:=0 z:=0 roll:=0 pitch:=0 yaw:=0 range_min:=0.01 range_max:=4.0 resolution:=0.01 rate:=10">
     <joint name="${name}_joint" type="fixed">
       <origin xyz="${x} ${y} ${z}"
               rpy="${roll} ${pitch} ${yaw}"/>
@@ -58,7 +58,7 @@
           <topicName>/rangefinder/range</topicName>
           <frameName>rangefinder</frameName>
           <radiation>infrared</radiation>
-          <fov>${fov}</fov>
+          <fov>0.01</fov>
           <gaussianNoise>0.001</gaussianNoise>
           <updateRate>${rate}</updateRate>
           <min_distance>${range_min}</min_distance>

clover_simulation/README.md

@@ -10,7 +10,7 @@ The simulation may be configured by a set of arguments:
 
 * `mav_id` (*integer*, default: *0*) - MAVLink identifier of the vehicle. **Note**: Multi-vehicle simulation is possible, but requires extensive changes to launch files;
 * `est` (*string*, default: *lpe*, possible values: *lpe*, *ekf2*) - PX4 estimator selection. Note that this may be overriden in the startup scripts for your craft;
-* `vehicle` (*string*, default: *clover*) - PX4 vehicle name. Depending on this parameter, different PX4 presets will be loaded.
+* `vehicle` (*string*, default: *clover*) - PX4 vehicle name. Depending on this parameter, different PX4 presets will be loaded. **Note**: The default value, *clover*, requires you to use [Clover-specific PX4 branch](https://github.com/CopterExpress/Firmware/tree/v1.10.1-clever);
 * `main_camera` (*boolean*, default: *true*) - controls whether the drone will have a vision position estimation camera;
 * `rangefinder` (*boolean*, default: *true*) - controls whether the drone will have a laser rangefinder;
 * `led` (*boolean*, default: *true*) - controls whether the drone will have a programmable LED strip;

clover_simulation/airframes/4500_clover

@@ -7,31 +7,30 @@
 
 . ${R}etc/init.d-posix/airframes/10016_iris # base on iris
 
-param set-default ATT_W_EXT_HDG 0.5
-param set-default ATT_EXT_HDG_M 1 # 0 = none, 1 = vision, 2 = mocap
+param set ATT_W_EXT_HDG 0.5
+param set ATT_EXT_HDG_M 1
 
-param set-default COM_DISARM_LAND 1.0
-param set-default COM_RCL_EXCEPT 4 # enable offboard flights without rc
+param set COM_DISARM_LAND 1.0
 
-param set-default LPE_FLW_SCALE 1.0
-param set-default LPE_FLW_R 0.2
-param set-default LPE_FLW_RR 0.0
-param set-default LPE_FLW_QMIN 10
-param set-default LPE_VIS_DELAY 0.0
-param set-default LPE_VIS_Z 0.1
-param set-default LPE_FUSION 86 # flow + vis + land detector + gyro comp
+param set LPE_FLW_SCALE 1.0
+param set LPE_FLW_R 0.2
+param set LPE_FLW_RR 0.0
+param set LPE_FLW_QMIN 10
+param set LPE_VIS_DELAY 0.0
+param set LPE_VIS_Z 0.1
+param set LPE_FUSION 86
 
-param set-default SENS_FLOW_ROT 0
-param set-default SENS_FLOW_MINHGT 0.0
-param set-default SENS_FLOW_MAXHGT 4.0
-param set-default SENS_FLOW_MAXR 10.0
+param set SENS_FLOW_ROT 0
+param set SENS_FLOW_MINHGT 0.0
+param set SENS_FLOW_MAXHGT 4.0
+param set SENS_FLOW_MAXR 10.0
 
-param set-default EKF2_AID_MASK 26 # flow + vis pos + vis yaw
-param set-default EKF2_OF_DELAY 0
-param set-default EKF2_OF_QMIN 10
-param set-default EKF2_OF_N_MIN 0.05
-param set-default EKF2_OF_N_MAX 0.2
-param set-default EKF2_HGT_MODE 3 # 0 = baro, 1 = gps, 2 = range, 3 = vision
-param set-default EKF2_EVA_NOISE 0.1
-param set-default EKF2_EVP_NOISE 0.1
-param set-default EKF2_EV_DELAY 0
+param set EKF2_AID_MASK 26 # flow + vis pos + vis yaw
+param set EKF2_OF_DELAY 0
+param set EKF2_OF_QMIN 10
+param set EKF2_OF_N_MIN 0.05
+param set EKF2_OF_N_MAX 0.2
+param set EKF2_HGT_MODE 2
+param set EKF2_EVA_NOISE 0.1
+param set EKF2_EVP_NOISE 0.1
+param set 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" required="true" 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" 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" required="true" type="jmavsim_run.sh" output="screen" if="$(eval type == 'jmavsim')">
+    <node name="jmavsim" pkg="px4" type="jmavsim_run.sh" output="screen" if="$(eval type == 'jmavsim')">
         <env name="HEADLESS" value="1" if="$(eval not gui)"/>
     </node>
 

clover_simulation/models/red_circle/materials/scripts/red_circle.material

@@ -1,16 +0,0 @@
-material red_circle
-{
-	technique
-	{
-		pass
-		{
-			scene_blend alpha_blend
-			texture_unit
-			{
-				texture red_circle.png
-				filtering none
-				scale 1.0 1.0
-			}
-		}
-	}
-}

clover_simulation/models/red_circle/model.config

@@ -1,13 +0,0 @@
-<?xml version="1.0"?>
-<model>
-    <name>Red Circle</name>
-    <version>1.0</version>
-    <sdf version="1.5">red_circle.sdf</sdf>
-    <author>
-        <name>Oleg Kalachev</name>
-        <email>okalachev@gmail.com</email>
-    </author>
-    <description>
-        Red circle of size 40 cm on the floor for recognizing by a drone
-    </description>
-</model>

clover_simulation/models/red_circle/red_circle.sdf

@@ -1,24 +0,0 @@
-<?xml version="1.0"?>
-<sdf version="1.5">
-    <model name="red_circle">
-        <static>true</static>
-        <link name="red_circle_link">
-            <pose>0 0 1e-3 0 0 0</pose>
-            <visual name="red_circle_texture">
-                <cast_shadows>false</cast_shadows>
-                <geometry>
-                    <box>
-                        <size>0.4 0.4 1e-3</size>
-                    </box>
-                </geometry>
-                <material>
-                    <script>
-                        <uri>model://red_circle/materials/scripts</uri>
-                        <uri>model://red_circle/materials/textures</uri>
-                        <name>red_circle</name>
-                    </script>
-                </material>
-            </visual>
-        </link>
-    </model>
-</sdf>

clover_simulation/models/red_circle/red_circle.svg

@@ -1,7 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<svg xmlns="http://www.w3.org/2000/svg" width="20" height="20" viewBox="0 0 20 20">
-	<title>
-		red_circle
-	</title><g fill="red">
-	<circle cx="10.05" cy="10.05" r="9.9"/>
-</g></svg>

clover_simulation/package.xml

@@ -1,6 +1,6 @@
-<package format="3">
+<package format="2">
     <name>clover_simulation</name>
-    <version>0.24.0</version>
+    <version>0.23.0</version>
     <description>The clover_simulation package provides worlds and launch files for Gazebo.</description>
 
     <maintainer email="okalachev@gmail.com">Oleg Kalachev</maintainer>
@@ -22,8 +22,6 @@
     <depend>gazebo_ros</depend>
     <depend>gazebo_plugins</depend>
     <depend>rospy</depend>
-    <depend condition="$ROS_PYTHON_VERSION == 2">python-docopt</depend>
-    <depend condition="$ROS_PYTHON_VERSION == 3">python3-docopt</depend>
 
     <export>
         <gazebo_ros gazebo_media_path="${prefix}"/>

clover_simulation/src/sim_leds.cpp

@@ -65,8 +65,7 @@ public:
 		}
 
 		role = (ros::this_node::getName() == "/gazebo") ? Role::Server : Role::Client;
-		ROS_INFO_NAMED(("LedController_" + robotNamespace).c_str(), "LedController has started (as %s) in namespace '%s'",
-			role == Role::Client ? "client" : "server", robotNamespace.c_str());
+		ROS_INFO_NAMED(("LedController_" + robotNamespace).c_str(), "LedController has started (as %s)", role == Role::Client ? "client" : "server");
 
 		nh.reset(new ros::NodeHandle(robotNamespace));
 
@@ -110,6 +109,7 @@ LedController& get(std::string robotNamespace)
 	std::lock_guard<std::mutex> lock(controllerMutex);
 	auto it = controllers.find(robotNamespace);
 	if (it == controllers.end()) {
+		gzwarn << "Creating new LED controller for namespace " << robotNamespace << "\n";
 		controllers[robotNamespace].reset(new LedController(robotNamespace));
 		return *controllers[robotNamespace];
 	}

docs/assets/copterhack2023.svg

@@ -1,61 +0,0 @@
-<?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>

docs/en/SUMMARY.md

@@ -36,7 +36,7 @@
   * [Optical Flow](optical_flow.md)
   * [Autonomous flight (OFFBOARD)](simple_offboard.md)
   * [Coordinate systems (frames)](frames.md)
-  * [Code examples](snippets.md)
+  * [Code snippets](snippets.md)
   * [Interfacing with a laser rangefinder](laser.md)
   * [LED strip](leds.md)
   * [Working with GPIO](gpio.md)
@@ -96,7 +96,6 @@
   * [Migration to v0.20](migrate20.md)
   * [Migration to v0.22](migrate22.md)
 * [Events](events.md)
-  * [CopterHack-2023](copterhack2023.md)
   * [CopterHack-2022](copterhack2022.md)
   * [CopterHack-2021](copterhack2021.md)
   * [CopterHack-2019](copterhack2019.md)
@@ -105,12 +104,6 @@
   * [Video contest](video_contest.md)
   * [Educational contests](educational_contests.md)
 * [Clover-based projects](projects.md)
-  * [Clover Cloud Platform](clover-cloud-platform.md)
-  * [Autonomous Racing Drone](djs_phoenix_chetak.md)
-  * [Motion Capture System](mocap_clover.md)
-  * [Swarm in Blocks 2](swarm_in_blocks_2.md)
-  * [Advanced Clover 2](advanced_clover_simulator_platform.md)
-  * [Network of charging stations](liceu128.md)
   * [Swarm-in-blocks](swarm_in_blocks.md)
   * [Obstacle avoidance using artificial potential fields method](obstacle-avoidance-potential-fields.md)
   * [The Clover Rescue Project](clover-rescue-team.md)

docs/en/advanced_clover_simulator_platform.md

@@ -1,161 +0,0 @@
-# Advanced Clover 3: The Platform
-
-[CopterHack-2023](copterhack2023.md), team **FTL**.
-
-## Team Information
-
-```cpp
-#include "veryInterestingCommandDescription.h"
-```
-
-Team members:
-
-- Maxim Ramanouski, [@max8rr8](https://t.me/max8rr8).
-
-Country: Belarus.
-
-## Project Description
-
-Last year at CopterHack 2022, we created a [project](../ru/advanced_clover_simulator.html) that simplified the simulation of Clover, and in 2021, we created a [project](../ru/advanced_clover.html) that simplified the development of products for Clover (IDE and library for writing). The time has come to combine them and achieve unlimited power.
-
-### Project Idea
-
-The idea of the project is to combine CloverIDE and CloverSim (a tool for running Clover simulations). Thus, a platform is planned that allows developing products based on Clover using a simulator and an advanced IDE. The platform will include the following features:
-
-- Add a web interface that allows using CloverSim without touching the command line.
-- Work both in the browser (without installing anything) and from CLI.
-- Have a course that covers different aspects of clover.
-- Simplify installation, especially in WSL.
-- Running a simulation on a remote device (more powerful computer or cloud).
-
-### Project videos
-
-Video presentation of the project: [link](https://www.youtube.com/watch?v=T4RU9sfxsSI).
-
-Live presentation at CopterHack: TBD.
-
-CLI demonstration: [link](https://www.youtube.com/watch?v=Ao-ukR58sSQ).
-
-## Installation
-
-Installation process is described in the [project documentation](https://ftl-team.github.io/clover_sim/#/?id=installation).
-
-## Usage
-
-The CloverSim platform offers a seamless workflow for users:
-
-1. Users can effortlessly select or create a workspace and task and
-   launch them with ease.
-
-   ![Step 1 screenshot](../assets/ftl/acp_workflow1.png)
-
-2. After launching the simulation, users are presented with CloverSim WebUI that
-   provides them with an intuitive way to view their scores and progress,
-   control the simulator, and access task descriptions and scoring information.
-   From it users can open terminal, gzweb and more importantly they can easily
-   access the CloverSim IDE to solve task.
-
-   ![Step 2 screenshot](../assets/ftl/acp_workflow2.png)
-
-3. The IDE provides a full suite of tools and features for writing and
-   debugging code. One example is autocompletion to help streamline the
-   development process, making it more efficient and effective.
-
-   ![Step 3 screenshot](../assets/ftl/acp_workflow3.png)
-
-4. Users can launch their programs with ease and monitor its progress via
-   the GZWeb, CopterStatus, and SimulatorStatus views of the IDE.
-
-   ![Step 4 screenshot](../assets/ftl/acp_workflow4.png)
-
-5. Users can track their progress and scores in real-time and effortlessly
-  restart the simulator if necessary. Additionally, different randomization
-  seed can be set to check various inputs and outcomes.
-
-We also have video demonstration/tutorial: [link](https://www.youtube.com/watch?v=aPOPHD3M3ZM).
-
-## More features
-
-- Easy installation process.
-- Efficient simulation launch, surpassing traditional virtual machines.
-- Generation of dynamic Gazebo worlds with randomization based on seed.
-- Real-time task completion verification and score presentation.
-- Execution with security in isolated containers.
-- Multiple project capability without the need for multiple virtual machine images.
-- WebUI for ease of use, removing the need to use the command line.
-- IDE similar to VSCode with support for C++ and Python, including autocompletion and autoformatting.
-- Custom-patched GZWeb with bug fixes and additional features, including the display of the Clover LED strip.
-- GZWeb provides a follow-objects feature superior to that of Gazebo.
-- IDE includes tools to interact with ROS, such as topic visualization, service calling, and image topic visualization.
-- IDE also includes Copter Status, displaying most of the drone's information, including position, camera, and LED strip, in one view.
-- IDE integrates with the simulator by providing control from it, viewing task descriptions, and opening GZWeb.
-
-We also have developed a learning course based on CloverSim: [link](https://github.com/FTL-team/CloverSim_course). It currently has the following tasks:
-
-- 1_thesquare - First task of CloverSim course with goal to fly square.
-- 2_iseeall - Task that teaches how to interact with camera.
-- 3_landmid - Find and land onto randomly positioned object.
-- 4_flybyline - Flying along the line.
-- 5_posknown - Find position of objects relative to ArUco map.
-
-## More details
-
-At this point, our platform consists of four major parts:
-
-- [CloverSim](https://github.com/FTL-team/clover_sim) - tool that manages simulation.
-- [CloverSim Basefs](https://github.com/FTL-team/clover_sim_basefs) - container image that is used in simulator.
-- [Clover IDE](https://github.com/FTL-team/cloverIDE) - clover ide tools and theia.
-- [CloverSim course](https://github.com/FTL-team/CloverSim_course) - course with tasks based on our platform.
-
-### CloverSim
-
-The simulation architecture is a continuation of work from CopterHack 2022, but while 2022 version was closer to Proof-of-Concept, the updated version is more robust.
-
-There are three major difference in simulator architecture
-
-- Replacement of `systemd-nspawn` with `runc` provides us higher degree of container control and seemingless support of non-systemd systems, for example WSL.
-- Migration to squash fs images, which greatly reduced size of installed CloverSim from 13 gigabytes to just 3.5 gigabytes.
-- Tasks are now mounted instead of being copied and also build before starting.
-
-Because of the way catkin_make works, it is incredibly slow when new packages are added (whole cmake configuration is rerun for all packages). catkin_make provides a way to build only some packages, but it caches this packages and to reset this cache you need to recompile whole catkin_make. But we have found a solution: `catkin_make -DCATKIN_WHITELIST_PACKAGES="task;CloverSim" --build build_CloverSim` This command, builds only CloverSim and task package in separate build directory, this drastically reduces time that catkin_make takes, and keeps expected behavior of catkin_make without arguments.
-
-There are also differences in tool that launches simulation:
-
-- Client-server architecture allows us to create web UI and run CloverSim on server.
-- More robust error handling improves user experience.
-- Rewritten in rust, better reliability and development experience.
-
-### CloverSim basefs
-
-Version 2 integrates CloverIDE into system. We also updated clover in simulator to v0.23 and added web terminal. Basefs is now squashed and doesn't require additional installation. It also uses patched(by us) version of gzweb that is more suitable for our use-case:
-
-- Unlike original GZWeb assets can be dynamically loaded, which is required to support dynamically generated tasks.
-- It also implements multiple bugfixes for rendering, UI.
-- Fixed performance, original gzweb had two constantly running loops that used 200% of cpu. We fixed this by instead using synchronization primitives.
-- Clover LED strip is rendered, our gzweb connects to ROS and pulls LED data from there to render LED strip like Gazebo does.
-- Users can now follow-objects like in Gazebo better actually.
-- Reconnect on disconnect, when simulator is restarted gzweb looses connection and it now can automatically reconnect.
-
-Patched gzweb available there: [FTL-team/gzweb](https://github.com/FTL-team/gzweb).
-
-### CloverIDE
-
-CloverIDE got some updates too:
-
-- We have updated theia and extensions used.
-- Better C++ support via clangd.
-- Clover IDE tools can now reconnect after simulator restart.
-- Copter Status now displays LED strip status.
-- Tools ui has better support for different themes.
-
-But the most important change is CloverSim integration, there are new tools (task description, simulator control and gzweb). While gzweb tool is just an iframe (though it's very cool to have it in IDE).
-
-Task description and simulator control are more interesting as they have to interact with both IDE and CloverSim, to maintain different versions support we use quite interesting trick, extension webview after being initialized dynamically loads JavaScript from CloverSim. That provides better integration between two.
-
-### CloverSim course
-
-CloverSim course is a new part of our platform. It uses robust task API of CloverSim to create practical learning course. It currently teaches different aspects of clover development that i encountered during my participation in different contests involving clover. But we are happy to accpet suggestions about other aspects we should teach in out course.
-
-## Conclusion
-
-This project is a final (or maybe there is more?) project of our advanced clover saga. AdvancedClover is a project that is easy to use and greatly improves experience during learning about clover, participating in clover based competitions and development clover based projects. We thank COEX team for their support and look forward to further cooperation.

docs/en/aruco_marker.md

@@ -72,6 +72,12 @@ Sample code to fly to a point 1 metre to the left and 2 metres above marker with
 navigate(frame_id='aruco_7', x=-1, y=0, z=2)
 ```
 
+Sample code to rotate counterclockwise while hovering 1.5 metres above marker id 10:
+
+```python
+navigate(frame_id='aruco_10', x=0, y=0, z=1.5, yaw_rate=0.5)
+```
+
 Note that if the required marker isn't detected for 0.5 seconds after the `navigate` command, the command will be ignored.
 
 These frames may also be used in other services that accept TF frames (like `get_telemetry`). The following code will get the drone's position relative to the marker with id 3:

docs/en/blocks.md

@@ -2,7 +2,7 @@
 
 <img src="../assets/blocks/blockly.svg" width=200 align="right">
 
-Visual blocks programming feature has been added to the [RPi image](image.md), starting with the version **0.21**. Blocks programming is implemented using [Google Blockly](https://developers.google.com/blockly) library. Blocks programming integration can lower the entry barrier to a minimum.
+Visual blocks programming feature has been added to the [RPi image](image.md), starting with the version **0.21**. Blocks programming is implemented using [Google Blockly](https://developers.google.com/blockly) platform. Blocks programming integration can lower the entry barrier to a minimum.
 
 ## Configuration
 

docs/en/camera.md

@@ -1,6 +1,6 @@
 # Working with the camera
 
-> **Note** The following applies to the [image version **0.24**](https://github.com/CopterExpress/clover/releases/tag/v0.24), which is not yet released. Older documentation is still available for [for version **0.23**](https://github.com/CopterExpress/clover/blob/f78a03ec8943b596d5a99b893188a159d5319888/docs/en/camera.md).
+> **Note** In the image version **0.20** `clever` package was renamed to `clover`. See [previous version of the article](https://github.com/CopterExpress/clover/blob/v0.19/docs/en/camera.md) for older images.
 
 Make sure the camera is enabled in the `~/catkin_ws/src/clover/clover/launch/clover.launch` file:
 
@@ -16,7 +16,7 @@ The `clover` service must be restarted after the launch-file has been edited:
 sudo systemctl restart clover
 ```
 
-You may use [rqt](rviz.md) or [web_video_server](web_video_server.md) to view the camera stream.
+You may use rqt or [web_video_server](web_video_server.md) to view the camera stream.
 
 ## Troubleshooting
 
@@ -54,6 +54,8 @@ The [SD card image](image.md) comes with a preinstalled [OpenCV](https://opencv.
 
 ### Python
 
+Main article: http://wiki.ros.org/cv_bridge/Tutorials/ConvertingBetweenROSImagesAndOpenCVImagesPython.
+
 An example of creating a subscriber for a topic with an image from the main camera for processing with OpenCV:
 
 ```python
@@ -61,14 +63,12 @@ import rospy
 import cv2
 from sensor_msgs.msg import Image
 from cv_bridge import CvBridge
-from clover import long_callback
 
-rospy.init_node('cv')
+rospy.init_node('computer_vision_sample')
 bridge = CvBridge()
 
-@long_callback
 def image_callback(data):
-    img = bridge.imgmsg_to_cv2(data, 'bgr8')  # OpenCV image
+    cv_image = bridge.imgmsg_to_cv2(data, 'bgr8')  # OpenCV image
     # Do any image processing with cv2...
 
 image_sub = rospy.Subscriber('main_camera/image_raw', Image, image_callback)
@@ -76,31 +76,19 @@ image_sub = rospy.Subscriber('main_camera/image_raw', Image, image_callback)
 rospy.spin()
 ```
 
-> **Note** Image processing may take significant time to finish. This can cause an [issue](https://github.com/ros/ros_comm/issues/1901) in rospy library, which would lead to processing stale camera frames. To solve this problem you need to use `long_callback` decorator from `clover` library, as in the example above.
-
-#### Limiting CPU usage
-
-When using the `main_camera/image_raw` topic, the script will process the maximum number of frames from the camera, actively utilizing the CPU (up to 100%). In tasks, where processing each camera frame is not critical, you can use the topic, where the frames are published at rate 5 Hz: `main_camera/image_raw_throttled`:
-
-```python
-image_sub = rospy.Subscriber('main_camera/image_raw_throttled', Image, image_callback, queue_size=1)
-```
-
-#### Publishing images
-
 To debug image processing, you can publish a separate topic with the processed image:
 
 ```python
 image_pub = rospy.Publisher('~debug', Image)
 ```
 
-Publishing the processed image:
+Publishing the processed image (at the end of the image_callback function):
 
 ```python
-image_pub.publish(bridge.cv2_to_imgmsg(img, 'bgr8'))
+image_pub.publish(bridge.cv2_to_imgmsg(cv_image, 'bgr8'))
 ```
 
-The published images can be viewed using [web_video_server](web_video_server.md) or [rqt](rviz.md).
+The obtained images can be viewed using [web_video_server](web_video_server.md).
 
 #### Retrieving one frame
 
@@ -111,7 +99,7 @@ import rospy
 from sensor_msgs.msg import Image
 from cv_bridge import CvBridge
 
-rospy.init_node('cv')
+rospy.init_node('computer_vision_sample')
 bridge = CvBridge()
 
 # ...
@@ -133,32 +121,38 @@ QR codes recognition in Python:
 ```python
 import rospy
 from pyzbar import pyzbar
-import cv2
 from cv_bridge import CvBridge
 from sensor_msgs.msg import Image
-from clover import long_callback
 
-rospy.init_node('cv')
 bridge = CvBridge()
 
-@long_callback
-def image_callback(msg):
-    img = bridge.imgmsg_to_cv2(msg, 'bgr8')
-    barcodes = pyzbar.decode(img)
+rospy.init_node('barcode_test')
+
+# Image subscriber callback function
+def image_callback(data):
+    cv_image = bridge.imgmsg_to_cv2(data, 'bgr8')  # OpenCV image
+    barcodes = pyzbar.decode(cv_image)
     for barcode in barcodes:
-        b_data = barcode.data.decode('utf-8')
+        b_data = barcode.data.decode("utf-8")
         b_type = barcode.type
         (x, y, w, h) = barcode.rect
         xc = x + w/2
         yc = y + h/2
-        print('Found {} with data {} with center at x={}, y={}'.format(b_type, b_data, xc, yc))
+        print("Found {} with data {} with center at x={}, y={}".format(b_type, b_data, xc, yc))
 
-image_sub = rospy.Subscriber('main_camera/image_raw_throttled', Image, image_callback, queue_size=1)
+image_sub = rospy.Subscriber('main_camera/image_raw', Image, image_callback, queue_size=1)
 
 rospy.spin()
 ```
 
-> **Hint** See other computer vision examples in the `~/examples` directory of the [RPi image](image.md).
+The script will take up to 100% CPU capacity. To slow down the script artificially, you can use [throttling](http://wiki.ros.org/topic_tools/throttle) of frames from the camera, for example, at 5 Hz (`main_camera.launch`):
+
+```xml
+<node pkg="topic_tools" name="cam_throttle" type="throttle"
+    args="messages main_camera/image_raw 5.0 main_camera/image_raw_throttled"/>
+```
+
+The topic for the subscriber in this case should be changed for `main_camera/image_raw_throttled`.
 
 ## Video recording
 

docs/en/camera_calibration.md

@@ -10,14 +10,14 @@ There are several tools allowing to calibrate the camera and store calculated pa
 
 Main tutorial: http://wiki.ros.org/camera_calibration/Tutorials/MonocularCalibration.
 
-In order to calibrate the camera with the `camera_calibration` ROS-package you need a computer with OS GNU/Linux and [ROS Noetic](http://wiki.ros.org/noetic/Installation/Ubuntu) installed.
+In order to calibrate the camera with the `camera_calibration` ROS-package you need a computer with OS GNU/Linux and [ROS Melodic](ros-install.md) installed.
 
 <img src="../assets/camera_calibration.png" alt="ROS Camera Calibrator" class="zoom center" width=600>
 
 1. Using the Terminal, install `camera_calibration` package to your computer:
 
     ```bash
-    sudo apt-get install ros-noetic-camera-calibration
+    sudo apt-get install ros-melodic-camera-calibration
     ```
 
 2. Download the chessboard – [chessboard.pdf](../assets/chessboard.pdf). Print the chessboard on paper or open it on the computer screen.

docs/en/clover-cloud-platform.md

@@ -1,93 +0,0 @@
-# Clover Cloud Platform
-
-[CopterHack-2023](copterhack2023.md), team **Clover Cloud Team**.
-
-The list of our team members:
-
-* Кирилл Лещинский / Kirill Leshchinskiy, [@k_leshchinskiy](https://t.me/k_leshchinskiy) - Team Lead.
-* Кузнецов Михаил / Mikhail Kuznetsov, [@bruhfloppa](https://t.me/bruhfloppa) - Frontend Developer.
-* Даниил Валишин / Daniil Valishin, [@Astel_1](https://t.me/Astel_1) - Backend Developer.
-
-## Table of contents
-
-* [Introduction](#introduction)
-* [Usability](#usability)
-* [How to work with our platform?](#how-to-work-with-our-platform)
-* [About the development of the platform](#about-the-development-of-the-platform)
-* [Conclusion](#conclusion)
-
-## Video demonstration
-
-<p align="center">
-  <a href="https://www.youtube.com/watch?v=FZPl2LOMgi4"><img img width="560" height="315" src="https://img.youtube.com/vi/FZPl2LOMgi4/maxresdefault.jpg" /></a>
-</p>
-
-## Introduction
-
-Clover Cloud Platform is an innovative platform that enables users to access COEX Clover drone simulation online, without the need to download any programs or virtual machines.
-
-> **Note** Visit our [documentation](https://docs.clovercloud.software) to learn all about the platform, its development and how to use it.
-
-## Unleash Your Coding Power: Develop Autonomous Flight Code at Lightning Speed on Clover Cloud Platform
-
-If you're a developer working on autonomous flight projects, you know how time-consuming and distracting all of the routine activities can be. Between managing your hardware, debugging, and configuring your environment, it can feel like the real work of coding gets lost in the shuffle.
-
-That's where our platform comes in. Our streamlined interface and powerful tools make it easy to tackle all of those essential tasks so you can focus on what really matters: developing flawless, high-performance code for your autonomous flight project.
-
-So why wait to unleash your coding power? Sign up for our platform today and discover the difference it can make in the speed, quality, and focus of your autonomous flight coding work.
-
-## Usability
-
-Our platform is incredibly user-friendly and provides seamless access to the simulation in just a few clicks. Together with a simulator that displays simulation data accurately and without delay, there is a map editor allows users to edit the ArUco marker map and add or modify other objects on the scene directly within the simulation window. Additionally, users can create pre-configured workspaces complete with autonomous flight code and simulation scene configuration. Each user can also create their templates or apply a pre-made one to their workspace in just a few clicks. In addition to its other features, Clover Cloud Platform provides users with a convenient code editor for autonomous flight coding. Users can write code in the built-in editor and run it directly from the editor, viewing program output in real-time in the terminal. The platform also includes a file manager that simplifies file manipulation tasks, further enhancing the user's overall experience. With these tools at your fingertips, Clover Cloud Platform delivers an unparalleled level of accessibility and convenience for autonomous flight simulation.
-
-<p align="center">
-  <img src="https://raw.githubusercontent.com/Clover-Cloud-Platform/clover-cloud-platform-frontend/master/docs/workspace.png" alt="Workspace screenshot">
-</p>
-
-## The CodeSandbox for COEX Clover
-
-You can describe the usability and relevance of our platform in another way. Have you heard of CodeSandbox? Our platform offers the same convenience, flexibility, and accessibility as CodeSandbox, but is specifically designed to work with the COEX Clover drone simulation.
-
-## How to work with our platform?
-
-Let's dive into the sea of functionality that our platform offers. Detailed description of each feature is available in our [documentation](https://docs.clovercloud.software), here we will provide a general overview of the platform.
-
-### Creating an account
-
-First, you should create an account on our site. You can do this by clicking on this [link](https://clovercloud.software/signup).
-
-### Instance management
-
-After creating an account, you will be taken to the [dashboard](https://clovercloud.software/instances). Here you can create, start, stop and delete workspaces.
-
->Workspaces are containers with Gazebo simulator and our software that provide data flow for simulation visualization, as well as handle requests from file manager, code editor and terminal.
-
-<p align="center">
-  <img src="https://raw.githubusercontent.com/Clover-Cloud-Platform/clover-cloud-platform-frontend/master/docs/instances.gif" alt="Instance management">
-</p>
-
-### Workspace overview
-
-In the workspace, in addition to the simulator, you have a file manager, code editor and terminal. There is also an editing mode in the simulator - one of the key features of our project. It allows you to quickly and conveniently edit the simulation scene, namely: move ArUco markers, change their size, change id of the marker, load instead of marker picture, add new markers or delete them. You can also add 3d objects to the scene and change their position, size and color. Below is an example of working with our workspace.
-
-<p align="center">
-  <img src="https://github.com/Clover-Cloud-Platform/clover-cloud-platform-frontend/raw/master/docs/workspace.gif" alt="Workspace overview">
-</p>
-
-### Templates
-
-Templates are another key feature of our platform.Is there something you can't do and you want to see how to properly perform a task? Look for the right template with ready-made code in the Template Browser and apply it to your workspace! Each user can create a template with an autonomous flight code and simulator configuration and share it.
-
-## About the development of the platform
-
-Our team has worked tirelessly to develop a simple yet multifunctional platform. We utilized the most modern standards and tools and implemented numerous optimization methods to ensure seamless performance and error-free operation. The frontend programming language chosen was JavaScript with the React framework, as a design system we utilizing Material Design style for an elegant and intuitive user interface. With the help of GitHub Actions the website is being built and deployed to Firebase hosting. The platform's backend is written in Python and contains multiple simultaneously running scripts. User data is secured and stored in a MongoDB database. Communication between the server and site is enabled through web sockets and the socket.io library, guaranteeing lightning-fast data transfer with minimal lag.
-
-You can view the source code of our platform by clicking on the links below:
-
-[Repository with the frontend-side code](https://github.com/Clover-Cloud-Platform/clover-cloud-platform-frontend)
-
-[Repository with the backend-side code](https://github.com/Clover-Cloud-Platform/clover-cloud-platform-backend)
-
-## Conclusion
-
-In conclusion, we have successfully created a truly convenient and useful platform, suitable for both novice and advanced COEX Clover drone users. Beginners can test their first autonomous flight code without the need for demanding simulator installation or virtual machines. They can also explore all of the drone's functions and capabilities without editing any configuration files. Advanced users benefit from access to their workspace from anywhere in the world and on any device, along with a convenient code-sharing system. In the future, we plan to add more new features to our platform, scale our network to serve a greater number of users, and collaborate with COEX to integrate their Clover quadcopter documentation into our platform, offering users a very simple and user-friendly way to learn to program autonomous drone flight. We also want to express gratitude to the COEX customer support team for their assistance in resolving complex issues that arose during development.

docs/en/connection.md

@@ -20,14 +20,15 @@ USB connection is the preferred way to connect to the flight controller.
 
 ## UART connection
 
+> **Note** In the image version **0.20** `clever` package and service was renamed to `clover`. See [previous version of the article](https://github.com/CopterExpress/clover/blob/v0.19/docs/en/connection.md) for older images.
+
 <!-- TODO: Connection scheme -->
 
 UART connection is another way for the Raspberry Pi and FCU to communicate.
 
-1. Connect the TELEM 2 port on the flight controller using a UART cable to the Raspberry Pi pins following this instruction: the black cable (*GND*) to Ground, the green cable (*UART_RX*) to *GPIO14*, the yellow cable (*UART_TX*) to *GPIO15*. Do not connect the red cable (*5V*).
-2. Set the PX4 parameters: `MAV_1_CONFIG` to TELEM 2, `SER_TEL2_BAUND` to 921600 8N1. In PX4 of version prior to v1.10.0 the parameter `SYS_COMPANION` should be set to 921600.
-3. [Connect to the Raspberry Pi over SSH](ssh.md).
-4. Change the connection type in `~/catkin_ws/src/clover/clover/launch/clover.launch` to UART:
+1. Connect Raspberry Pi to your FCU using a UART cable.
+2. [Connect to the Raspberry Pi over SSH](ssh.md).
+3. Change the connection type in `~/catkin_ws/src/clover/clover/launch/clover.launch` to UART:
 
     ```xml
     <arg name="fcu_conn" default="uart"/>
@@ -39,4 +40,15 @@ UART connection is another way for the Raspberry Pi and FCU to communicate.
     sudo systemctl restart clover
     ```
 
+> **Hint** Set the `SYS_COMPANION` PX4 parameter to 921600 to enable UART on the FCU.
+
+## SITL connection
+
+In order to connect to a local or a remote [SITL](sitl.md) instance set the `fcu_conn` parameter to `udp` and `fcu_ip` to the IP address of the SITL instance (`127.0.0.1` if you are running the instance locally):
+
+```xml
+<arg name="fcu_conn" default="udp"/>
+<arg name="fcu_ip" default="127.0.0.1"/>
+```
+
 **Next**: [Using QGroundControl over Wi-Fi](gcs_bridge.md)

docs/en/copterhack2023.md

@@ -1,177 +0,0 @@
-# CopterHack 2023
-
-<img src="../assets/copterhack2023.svg" width=300 align=right>
-
-CopterHack 2023 is an international open-source projects competition on aerial robotics. The main direction of the CopterHack is team competition with a free choice of the project topic. The competition’s official language is English.
-
-To learn more about the articles of the CopterHack finalist teams follow the links [CopterHack 2021](copterhack2021.md), [CopterHack 2022](copterhack2022.md).
-
-The proposed projects are supposed to be open-source and be compatible with the Clover quadcopter platform. Teams-participants are supposed to work on their projects throughout the competition, bringing them closer to the state of the finished product while being assisted by industry experts through lectures and regular feedback.
-
-Final of the CopterHack 2022 was held on May 27, 2023. The winner team was the team 🇷🇺 **[Clover Cloud Platform](clover-cloud-platform.md)**.
-
-## Full stream of the final
-
-<iframe width="560" height="315" src="https://www.youtube.com/embed/Hdl6Sah7nkE" frameborder="0" allow="accelerometer; autoplay; encrypted-media; gyroscope; picture-in-picture" allowfullscreen></iframe>
-
-## Projects of the contest's participants {#participants}
-
-|Place|Team|Project|Points|
-|:-:|-|-|-|
-|1|🇷🇺 Clover Cloud Team|[Clover Cloud Platform](clover-cloud-platform.md)|21.7|
-|2|🇧🇾 FTL|[Advanced Clover 2](advanced_clover_simulator_platform.md)|21|
-|3|🇨🇦 Clover with Motion Capture System|[Clover with Motion Capture System](mocap_clover.md)|20.5|
-|4|🇧🇷 Atena|[Swarm in Blocks 2](swarm_in_blocks_2.md)|20.3|
-|5|🇷🇺 C305|[Система радио-навигации](../ru/nav-beacon.html)|17.5|
-|6|🇮🇳 DJS PHOENIX|[Autonomous Racing Drone](djs_phoenix_chetak.md)|14.6|
-|7|🇷🇺 Lyceum №128|[Network of Clover charging stations](liceu128.md)|13.7|
-|✕|🇰🇬 Zavarka|[Система обмена грузами с помощью конвейера](https://github.com/aiurobotics/clover/blob/conveyance/docs/ru/conveyance.md)||
-|✕|🇷🇺 FSOTM|[Drone Interceptor](https://github.com/deadln/clover/blob/interceptor/docs/ru/interceptor.md)||
-|✕|🇰🇬 Homelesses|[Trash Collector](https://github.com/Isa-jesus/clover/blob/trash-collector/docs/ru/show_maker.md)||
-|✕|🇷🇺 Digital otters|[Digital otters](https://github.com/Mentalsupernova/clover_cool/blob/new-article.md/docs/ru/new-article.md)||
-|✕|🇷🇺 Light Flight|[Сопровождение БПЛА при посадке](https://github.com/SirSerow/clover_inertial_ns/blob/inertial-1/Description.md)||
-|✕|🇰🇬 LiveSavers|[LiveSavers](https://github.com/Sarvar00/clover/blob/livesavers/docs/ru/livesaver.md)||
-|✕|🇷🇺 XenCOM|[Bound by fate](https://github.com/xenkek/clover/blob/xenkek-patch-1/docs/ru/bound_by_fate.md)||
-|✕|🇷🇺 Ava_Clover|[DoubleClover](https://github.com/bessiaka/clover/blob/Ava_Clover/docs/ru/soosocta.md)||
-|✕|🇷🇺 TPU_1|[Совместная транспортировка груза](https://github.com/shamoleg/clover/blob/tpu_1/docs/ru/tpu_1.md)||
-|✕|🇷🇺 TPU_2|[Алгоритм полета сквозь лесную местность](https://github.com/shamoleg/clover/blob/tpu_2/docs/ru/tpu_2.md)|&nbsp;|
-
-See all points by criteria in the [full table](https://docs.google.com/spreadsheets/d/1qTpW8zFVdSEGnbtOvMgQD6DcYwu8URFt1RKOCeUaOe8).
-
-## CopterHack 2023 stages
-
-The qualifying and project development stages will be held in an online format, however, the final round will be in a hybrid mode (offline + online). The competition involves monthly updates from the teams with regular feedback from the jury. All teams are required to prepare a final video and presentation on the project's results to participate in the final stage.
-
-1. Qualifying stage. Applications are accepted on the deadline date until October 31, 2022.
-2. Projects development stage. This stage includes monthly updates and mentorship of participants. Starting date - November 1, 2022. Deadline date - February 28, 2023.
-3. All teams-participants are required to make the final video to proceed to the final round. Final videos are required to be uploaded until March 31, 2023.
-4. The final round. Projects presentation takes place April 23, 2023.
-
-## Conditions and criteria for evaluation the final result
-
-General project requirements:
-
-1. Open-source.
-2. Compatibility with the Clover platform.
-
-Judging criteria for the jury at the final:
-
-1. Readiness and the article (max. 10 points): the degree of readiness of the project; an accessible and understandable description of the project in the article; a link to the code with comments, diagrams, drawings. It should be possible to reproduce the project and get the result according to the article.
-2. Amount of work done (max. 6 points): the amount of work done by the team in the framework within of CopterHack, its complexity, and the technical level.
-3. Usefulness for Clover (max. 6 points): the relevance to the Clover and PX4 platform application in practice, the potential level of demand from other Clover users.
-4. Presentation at the final (max. 3 points): quality and entertainment points of the final presentation; completeness of the project coverage; demonstration; answers to the jury's questions.
-
-## Prize fund
-
-Basing on the results of the evaluation of projects at the final round, the jury will select the winners with the following prizes.
-
-* 1st place: $3000 (USD).
-* 2nd place: $2000 (USD).
-* 3rd place: $1000 (USD).
-* 4th place: $500 (USD).
-* 5th place: $500 (USD).
-
-The competition partners can reward the teams according to additional criteria identified during the evaluation of projects during the final round.
-
-## How to apply?
-
-> **Note** In order to be able to apply, you must have an account on [GitHub](https://github.com).
-
-Prepare your application and send it as a Draft Pull Request to [Clover repository](https://github.com/CopterExpress/clover)
-
-1. Fork the Clover repository:
-
-    <img src="../assets/github_application/github-fork.png" alt="GitHub Fork">
-
-2. On the web page of your fork, go to the `docs/en` section and create a new file in the [Markdown](http://en.wikipedia.org/wiki/Markdown) format:
-
-    <img src="../assets/github_application/create_new_file.png" alt="GitHub Create New File">
-
-3. Enter the title of your article. For example, `new-article.md`
-
-    <img src="../assets/github_application/new_article.png" alt="GitHub New Article">
-
-4. Fill in your application by the recommended template:
-
-   ```markdown
-   # Project name
-   
-   [CopterHack-2023](copterhack2023.md), team **Team name**.
-   
-   ## Team information
-   
-   The list of team members:
-   
-   (Describe the team: full name, contacts (Telegram username), role in the team).
-   
-   * Alexander Sokolov, @aleksandrsokolov111, engineer.
-   * Elena Smirnova, @elenasmirnova111, programmer.
-   
-   ## Project description
-   
-   ### Project idea
-   
-   Briefly describe the idea and stage of the project.
-   
-   ### The potential outcomes
-   
-   Describe how you see the project result.
-   
-   ### Using Clover platform
-   
-   Describe how the Clover platform will be used in your project.
-   
-   ### Additional information at the request of participants
-   
-   For example, information about the team's experience while working on projects, attach a link to articles, videos.
-   ```
-
-5. Go to the bottom of the page and create a new branch with the title of your article:
-
-    <img src="../assets/github_application/propose_new_file.png" alt="GitHub Propose New File">
-
-    > **Note** Don't commit changes directly to the `master` branch, create a new branch.
-
-6. If necessary, place additional visual assets in the `docs/assets` folder and add them to your article.
-
-7. Send a Draft Pull Request from your branch to Clover:
-
-    <img src="../assets/github_application/github-pull-request-create.png" alt="GitHub Create Pull">
-
-8. In the Pull Request comments, you will be given feedback on the application.
-
-9. Please note, in the *Checks* block the *Documentation* field should contain a tick, id cross appeared, click *Details* link to see the list of issues in you article found by markdownlint. If you need to change added files, edit them in you branch – changes will appear in the Pull Request automatically. **Do not open a new Pull Request for the same application**.
-
-10. During the contest, you will work on this document, bringing it closer to the finished state. By the end of the contest you are expected to publish your article which is supposed to be the result of your work in CopterHack.
-
-Teams-participants are supposed to be added to the special Telegram group, where one can send the project's updates and get feedback from the Jury. For all participating teams, COEX will provide a 40% discount on the Clover drone kit.
-
-> **Info** There are no restrictions on the age, education, and number of people in a team.
-
-## CopterHack 2023 projects’ papers contest
-
-Our participants have been engaged in advanced projects in the field of aerial robotics for already two years. This year we are planning to launch a new type of contest stimulating participants to present the research results running within the whole contest, at high -level international conferences as well as to publish them in Russian and international magazines in thematic areas.
-
-Original articles are accepted in following nominations:
-
-* $2000 (USD) for an article in a magazine of first quartile (Q1), indexed in Scopus, Web of Science.
-* $1000 (USD) for an article in a magazine, indexed in Scopus, Web of Science.
-* $500 (USD) for an article, published in Compendium (Conference Proceedings), indexed in Scopus, Web of Science.
-
-> **Note** [Easy way to find quartiles for journals in Web of Science and Scopus](https://www.texpedi.com/2021/07/how-to-find-journal-quartile.html).
-
-Requirements:
-
-1. The article is required to reflect the results of the project, developed within CopterHack 2023.
-2. The article is required to be accepted for publication by the moment of application for the Contest.
-3. It is required to indicate in the acknowledgement area that work is accomplished within the Contest.
-
-**Applications deadline**: December 10, 2023. The application for the contest should be submitted through the [Google Form](https://docs.google.com/forms/d/e/1FAIpQLSf52x0CTur-wUCG2URwY-p85gEUBUvgC0mPVNot0RHVjqcLZA/viewform).
-
-**Results announcement**: December 24, 2023.
-
----
-
-For all questions: [CopterHack in Telegram](https://t.me/CopterHack).
-
-> **Info** Please contact [Oleg Ponfilenok in Telegram](https://t.me/ponfilenok) if you are interested in becoming the contest's partner or jury member.

docs/en/djs_phoenix_chetak.md

@@ -1,55 +0,0 @@
-# Autonomous Racing Drone: CHETAK
-
-[CopterHack-2023](copterhack2023.md), team **DJS PHOENIX**.
-
-## Team Information
-
-![Without bg](https://user-images.githubusercontent.com/93365067/195974501-0acef6b7-e4ea-4c47-bd7a-615caf73a625.png)
-
-We are the DJS Phoenix, the official drone team of Dwarkadas. J. Sanghvi College of Engineering
-
-The list of team members:
-
-* Shubham Mehta, @Just_me_05, Mentor.
-* Harshal Warde, @kryptonisinert, Mechanical.
-* Parth Sawjiyani, @Non_Active, Mechanical.
-* Soham Dalvi, @devilsfootprint_1973, Mechanical.
-* Vedant Patel, @VedantMP, Mechanical.
-* Harsh Shah, @harssshhhhh, Mechanical.
-* Lisha Mehta, @lishamehta, Mechanical.
-* Shubh Pokarne, @Shubhpokarne, Electronics.
-* Tushar Nagda, @tushar_n11, Electronics.
-* Deep Tank, @Kraven, Electronics.
-* Khushi Sanghvi, @Cryptoknigghtt, Programmer.
-* Harshil Shah, @divine_fossil, Programmer.
-* Omkar Parab, @Omkar_parab21, Programmer.
-* Madhura Korgaonkar, @Madhura221, Programmer.
-* Shruti Shah, @Shrutishah22, Programmer.
-* Aditi Dubey, @aditi_0503, Marketing.
-* Krisha Lakhani, @krishalakhani, Marketing.
-
-## Project Description
-
-This year, our team DJS Phoenix, presents to you a fully Autonomous Racing Drone. The drone scans for ArUco tags on the gates and passes through them.
-
-### Project Idea
-
-This project proposes to develop an autonomous racing drone that can navigate through complex courses at high speeds while avoiding obstacles and detecting changes in the environment. In racing competitions, autonomous drones can compete in high-speed, precision races that challenge their agility, speed, and accuracy. These competitions could be held in indoor arenas or outdoor tracks, and they could attract enthusiasts and spectators from all over the world. With their advanced capabilities, autonomous racing drones could usher in a new era of racing events that are more exciting and challenging than ever before. From racing competitions to search and rescue operations, the autonomous racing drone can be used in a wide range of applications that benefit individuals, businesses, and society as a whole.
-
-## Potential Outcome
-
-### Problem
-
-In many industries and applications, there is a need for fast, efficient, and safe movement of goods and information. Drones have become an increasingly popular tool for a wide range of applications, from aerial photography to surveying and monitoring. However, operating a drone requires a certain level of skill and experience, which can be a barrier for individuals or businesses who want to take advantage of this technology. Additionally, traditional drones can be expensive and time-consuming to operate, limiting their accessibility and effectiveness. Therefore, there is a need for a more user-friendly and affordable solution that can expand the use of drones to new audiences and applications.
-
-### Solution
-
-The solution to the above problem statement is an autonomous racing drone. An autonomous racing drone is equipped with a camera that scans the ArUco tags for gate detection which is supported by software used in autonomous flights that enable it to navigate through a predetermined course while avoiding obstacles and achieving high speeds. Unlike traditional drones, an autonomous racing drone does not require manual control, making it an ideal solution for those who do not have the skills or experience to operate a drone.Its autonomous capabilities make it a more accessible and user-friendly solution than traditional drones, enabling individuals or businesses to take advantage of this technology without requiring extensive training or expertise.
-
-![image](https://user-images.githubusercontent.com/93365067/235303281-f63e379d-c156-45ad-b554-2c84bd82781d.png)
-
-### Additional Information
-
-In 2017, a student committee for DJS Phoenix was formed. In India, our team has participated in a number of contests, including IDRL-IIT GandhiNagar (sixth rank), IDRL-SVPCET Nagpur(second rank) and TECHNOXIAN (second place out of 50 national teams). In CopterHack-2021, our team participated, and we placed eighth internationally. We are back with improved concepts after learning from the previous season.
-
-For more information checkout gitbook: https://djs-phoenix.gitbook.io/chetak-faster-than-you-can-imagine/.

docs/en/educational_contests.md

@@ -20,7 +20,7 @@ The main goal of the contest is aerial robotics popularization  and community de
 * Third parties can provide technical support for recording a lecture.
 * The status of the participant is unlimited (student, representative of a general education institution, representative of the industry, amateur).
 
-Applications deadline: November 30, 2022.
+Applications deadline: September 1, 2022.
 
 ### How to apply?
 
@@ -64,7 +64,7 @@ The main goal of the contest is aerial robotics popularization  and community de
 
 The application to the contest is performed via the [Google Form](https://docs.google.com/forms/d/e/1FAIpQLSdelVy6yQ1iN6u88KeiEIKGj7gGaM0xccSt2tiYKB46ICmjkQ/viewform).
 
-Applications deadline: November 30, 2022.
+Applications deadline: September 1, 2022.
 
 ### Prizes
 
@@ -105,7 +105,7 @@ The course is evaluated according to a separate, publicly available lesson submi
 
 The application to the contest is performed via the [Google Form](https://docs.google.com/forms/d/e/1FAIpQLSdf2Q68X4hPnFE9f3EP95AxPNnzHKqIsFHtTRT6EBKiH93wzg/viewform) where the link to the video course should be attached.
 
-Applications deadline: November 30, 2022.
+Applications deadline: September 1, 2022.
 
 ### Prizes