Merge branch 'master' into roswww-static-target

.github/workflows/build-image.yaml

@@ -7,24 +7,23 @@ on:
     branches: [ master ]
   release:
     types: [ created ]
-  workflow_dispatch:
 
 jobs:
   build:
     runs-on: ubuntu-latest
     steps:
-      - uses: actions/checkout@v4
+      - uses: actions/checkout@v2
       - name: Build image
         run: |
           docker run --privileged --rm -v /dev:/dev -v $(pwd):/builder/repo -e TRAVIS_TAG="${{ github.event.release.tag_name }}" sfalexrog/img-tool:qemu-update
       - name: Compress image
         run: |
-          cd images && sudo chmod -R 777 . && zip -9 $(echo *_*).zip *_* && ls -l . && unzip -l *_*.zip
+          cd images && sudo chmod -R 777 . && zip -9 $(echo clover_*).zip clover_* && ls -l . && unzip -l clover_*.zip
       - name: Upload image
         uses: softprops/action-gh-release@v1
         if: ${{ github.event_name == 'release' }}
         with:
-          files: images/*_*.zip
+          files: images/clover_*.zip
           prerelease: true
         env:
           GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }}

.github/workflows/build.yml

@@ -5,13 +5,12 @@ on:
     branches: [ '*' ]
   pull_request:
     branches: [ master ]
-  workflow_dispatch:
 
 jobs:
   # melodic:
   #   runs-on: ubuntu-latest
   #   steps:
-  #     - uses: actions/checkout@v4
+  #     - uses: actions/checkout@v2
   #     - name: Native Melodic build
   #       run: |
   #         docker run --rm -v $(pwd):/root/catkin_ws/src/clover ros:melodic-ros-base /root/catkin_ws/src/clover/builder/standalone-install.sh
@@ -23,7 +22,7 @@ jobs:
         working-directory: catkin_ws
         shell: bash
     steps:
-    - uses: actions/checkout@v4
+    - uses: actions/checkout@v2
       with:
         path: catkin_ws/src/clover
     - name: Install requirements
@@ -45,7 +44,7 @@ jobs:
           fakeroot debian/rules binary
           cd -
         done
-    - uses: actions/upload-artifact@v4
+    - uses: actions/upload-artifact@v3
       with:
         name: debian-packages
         path: catkin_ws/src/clover/*.deb

.github/workflows/docs.yml

@@ -5,13 +5,16 @@ on:
     branches: [ '*' ]
   pull_request:
     branches: [ '*' ]
-  workflow_dispatch:
 
 permissions:
   contents: read
   pages: write
   id-token: write
 
+concurrency:
+  group: "pages"
+  cancel-in-progress: true
+
 defaults:
   run:
     shell: bash
@@ -20,9 +23,9 @@ jobs:
   docs:
     runs-on: ubuntu-22.04
     steps:
-      - uses: actions/checkout@v4
+      - uses: actions/checkout@v2
       - name: Use Node.js
-        uses: actions/setup-node@v4
+        uses: actions/setup-node@v1
         with: { node-version: '10' }
       - name: Setup tools
         run: |
@@ -57,24 +60,21 @@ jobs:
           rm _book/clover_ru.pdf && mv _book/clover_ru_compressed.pdf _book/clover_ru.pdf
           rm _book/clover_en.pdf && mv _book/clover_en_compressed.pdf _book/clover_en.pdf
           ls -lah _book/clover*.pdf
-          echo 'GITBOOK_PDF_OK=1' >> "$GITHUB_OUTPUT"
+          echo '::set-output name=GITBOOK_PDF_OK::1'
       - name: Download older PDFs
         if: ${{ !steps.generate-pdf.outputs.GITBOOK_PDF_OK }}
         run: |
           rm -f _book/clover*.pdf
-          wget --no-verbose https://clovercoex.tech/clover_ru.pdf -P _book/
-          wget --no-verbose https://clovercoex.tech/clover_en.pdf -P _book/
+          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@v3
+        uses: actions/upload-pages-artifact@v1
         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 }}
@@ -82,8 +82,5 @@ jobs:
     needs: docs
     steps:
       - name: Deploy to GitHub Pages
-        env:
-          FREEZE_DOCS: ${{ secrets.FREEZE_DOCS }}
-        if: ${{ !env.FREEZE_DOCS }}
         id: deployment
-        uses: actions/deploy-pages@v4
+        uses: actions/deploy-pages@v1

.github/workflows/editorconfig.yaml

@@ -5,15 +5,14 @@ on:
     branches: [ '*' ]
   pull_request:
     branches: [ master ]
-  workflow_dispatch:
 
 jobs:
   editorconfig:
     runs-on: ubuntu-latest
     steps:
-      - uses: actions/checkout@v4
+      - uses: actions/checkout@v2
       - name: .editorconfig Linter
         run: |
           wget --no-verbose https://github.com/okalachev/editorconfig-checker/releases/download/1.2.1-disable-spaces-amount/ec-linux-amd64
           chmod +x ec-linux-amd64
-          ./ec-linux-amd64 -spaces-after-tabs -e "roslib.js|ros3d.js|eventemitter2.js|yaml.js|draw.cpp|BinUtils.swift|\.idea|apps/android/app|blockly/|clover_blocks/programs/|highlight/|python.js|Assets.xcassets|test_parser_pass.txt|test_node_failure.txt|aruco_pose/vendor|\.stl|\.dxf|\.dae|\.material"
+          ./ec-linux-amd64 -spaces-after-tabs -e "roslib.js|ros3d.js|eventemitter2.js|json-to-pretty-yaml.js|draw.cpp|BinUtils.swift|\.idea|apps/android/app|blockly/|clover_blocks/programs/|highlight/|python.js|Assets.xcassets|test_parser_pass.txt|test_node_failure.txt|aruco_pose/vendor|\.stl|\.dxf|\.dae|\.material"

.markdownlint.json

@@ -113,9 +113,7 @@
             "VMware",
             "DuoCam"
         ],
-        "code_blocks": false,
-        "html_elements": false
+        "code_blocks": false
     },
-    "MD045": false,
-    "MD051": false
+    "MD045": false
 }

README.md

@@ -6,7 +6,7 @@ Clover is an open source [ROS](https://www.ros.org)-based framework, providing u
 
 COEX Clover Drone is an educational programmable drone kit, suited perfectly for running clover software. The kit is shipped unassembled and includes Pixracer-compatible autopilot running PX4 firmware, Raspberry Pi 4 as a companion computer, a camera for computer vision navigation as well as additional sensors and peripheral devices. Batteries included.
 
-The main documentation is available at [https://clovercoex.tech](https://clovercoex.tech/).
+The main documentation is available at [https://clover.coex.tech](https://clover.coex.tech/). Official website: [coex.tech/clover](https://coex.tech/clover).
 
 [__Support us on Kickstarter!__](https://www.kickstarter.com/projects/copterexpress/cloverdrone)
 
@@ -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:
@@ -30,11 +30,11 @@ Image features:
 * Configured networking
 * OpenCV
 * [`mavros`](http://wiki.ros.org/mavros)
-* Periphery drivers for ROS ([GPIO](https://clovercoex.tech/en/gpio.html), [LED strip](https://clovercoex.tech/en/leds.html), etc)
+* Periphery drivers for ROS ([GPIO](https://clover.coex.tech/en/gpio.html), [LED strip](https://clover.coex.tech/en/leds.html), etc)
 * `aruco_pose` package for marker-assisted navigation
 * `clover` package for autonomous drone control
 
-API description for autonomous flights is available [on GitBook](https://clovercoex.tech/en/simple_offboard.html).
+API description for autonomous flights is available [on GitBook](https://clover.coex.tech/en/simple_offboard.html).
 
 For manual package installation and running see [`clover` package documentation](clover/README.md).
 

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,8 +71,7 @@ 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)

aruco_pose/cfg/Detector.cfg

@@ -4,10 +4,7 @@ PACKAGE = "aruco_pose"
 from dynamic_reconfigure.parameter_generator_catkin import *
 import cv2.aruco
 
-try:
-        p = cv2.aruco.DetectorParameters_create()
-except AttributeError:
-        p = cv2.aruco.DetectorParameters()
+p = cv2.aruco.DetectorParameters_create()
 
 gen = ParameterGenerator()
 

aruco_pose/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
-<package format="3">
+<package format="2">
   <name>aruco_pose</name>
-  <version>0.25.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,12 @@ 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 estimate_poses_, send_tf_, auto_flip_, use_map_markers_;
 	bool waiting_for_map_;
 	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_;
@@ -106,8 +105,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_");
@@ -140,13 +138,13 @@ private:
 		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,12 +179,12 @@ 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());
 					}
 				}
 			}
@@ -207,9 +205,9 @@ 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_);
 					}
 
 					if (send_tf_) {
@@ -265,7 +263,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;

aruco_pose/src/aruco_map.cpp

@@ -81,9 +81,9 @@ private:
 	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()
@@ -104,14 +104,12 @@ public:
 
 		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", "");
 
@@ -179,21 +177,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 +191,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 +203,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;

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)

book.json

@@ -29,7 +29,7 @@
             "blank": true
         },
         "sitemap": {
-            "hostname": "https://clovercoex.tech"
+            "hostname": "https://clover.coex.tech"
         },
         "toolbar": {
             "buttons":

builder/assets/noetic-rosdep-clover.yaml

@@ -16,726 +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]
-geographiclib:
-  debian:
-    buster: [libgeographic-dev]

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
@@ -72,7 +72,7 @@ my_travis_retry() {
 echo_stamp "Init rosdep"
 my_travis_retry rosdep init
 # FIXME: Re-add this after missing packages are built
-echo "yaml file:///etc/ros/rosdep/${ROS_DISTRO}-rosdep-clover.yaml" >> /etc/ros/rosdep/sources.list.d/10-clover.list
+echo "yaml file:///etc/ros/rosdep/${ROS_DISTRO}-rosdep-clover.yaml" >> /etc/ros/rosdep/sources.list.d/20-default.list
 my_travis_retry rosdep update
 
 echo_stamp "Populate rosdep for ROS user"
@@ -112,7 +112,7 @@ my_travis_retry pip3 install wheel
 my_travis_retry pip3 install -r /home/pi/catkin_ws/src/clover/clover/requirements.txt
 source /opt/ros/${ROS_DISTRO}/setup.bash
 # Don't build simulation plugins for actual drone
-catkin_make -j2 -DCMAKE_BUILD_TYPE=RelWithDebInfo
+sudo -E -u pi sh -c '. /opt/ros/${ROS_DISTRO}/setup.sh && catkin_make -j2 -DCMAKE_BUILD_TYPE=RelWithDebInfo'
 source devel/setup.bash
 
 echo_stamp "Install clever package (for backwards compatibility)"
@@ -125,12 +125,11 @@ cd /home/pi/catkin_ws/src/clover
 builder/assets/install_gitbook.sh
 gitbook install
 gitbook build
-# replace assets copy to assets symlink to save space
-rm -rf _book/assets && ln -s ../docs/assets _book/assets
 touch node_modules/CATKIN_IGNORE docs/CATKIN_IGNORE _book/CATKIN_IGNORE clover/www/CATKIN_IGNORE apps/CATKIN_IGNORE # ignore documentation files by catkin
 
 echo_stamp "Installing additional ROS packages"
 my_travis_retry apt-get install -y --no-install-recommends \
+    ros-${ROS_DISTRO}-dynamic-reconfigure \
     ros-${ROS_DISTRO}-rosbridge-suite \
     ros-${ROS_DISTRO}-rosserial \
     ros-${ROS_DISTRO}-usb-cam \
@@ -138,11 +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}-nodelet-topic-tools \
-    ros-${ROS_DISTRO}-stereo-msgs \
-    ros-${ROS_DISTRO}-vision-msgs \
-    ros-${ROS_DISTRO}-angles
+    ros-${ROS_DISTRO}-image-view
 
 # TODO move GeographicLib datasets to Mavros debian package
 echo_stamp "Install GeographicLib datasets (needed for mavros)" \
@@ -156,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-software.sh

@@ -122,7 +122,7 @@ sed -i "s/updates_available//" /usr/share/byobu/status/status
 # sed -i "s/updates_available//" /home/pi/.byobu/status
 
 echo_stamp "Installing pip"
-curl https://bootstrap.pypa.io/pip/3.7/get-pip.py -o get-pip.py
+curl https://bootstrap.pypa.io/get-pip.py -o get-pip.py
 curl https://bootstrap.pypa.io/pip/2.7/get-pip.py -o get-pip2.py
 python3 get-pip.py
 python get-pip2.py

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

@@ -6,10 +6,6 @@ import os
 import rospy
 from geometry_msgs.msg import PoseStamped
 from sensor_msgs.msg import Range, BatteryState
-from vision_msgs.msg import BoundingBox2D, BoundingBox2DArray, BoundingBox3D, BoundingBox3DArray, \
-    Classification2D, Classification3D, Detection2D, Detection2DArray, Detection3D, Detection3DArray, \
-    ObjectHypothesis, ObjectHypothesisWithPose, VisionInfo
-import angles
 
 import cv2
 import cv2.aruco
@@ -37,12 +33,8 @@ import tf2_geometry_msgs
 import VL53L1X
 import pymavlink
 from pymavlink import mavutil
-from image_geometry import PinholeCameraModel, StereoCameraModel
 # from espeak import espeak
 from pyzbar import pyzbar
-import docopt
-import geopy
-import flask
 
 print(cv2.getBuildInformation())
 

builder/test/tests.sh

@@ -60,9 +60,6 @@ rosversion cv_camera
 rosversion web_video_server
 rosversion nodelet
 rosversion image_view
-rosversion stereo_msgs
-rosversion vision_msgs
-rosversion angles
 
 [[ $(rosversion ws281x) == "0.0.13" ]]
 
@@ -74,21 +71,8 @@ if [ -z $VM ]; then
 	[[ $(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 examples are present
 [[ $(ls $H/examples/*) ]]
 

check_assets_size.py

@@ -18,7 +18,7 @@ EXCLUDE = 'rviz.png', 'ssid.png', 'sitl_docker_demo.png', 'qgc-params.png', 'but
     'qgc-battery.png', 'qgc-radio.png', 'qgc-cal-acc.png', 'qgc-esc.png', 'qgc-cal-compass.png', \
     'qgc.png', 'qgc-parameters.png', 'clever4-front-white-large.png', 'qgc-modes.png', \
     'qgc-requires-setup.png', 'clever4-front-white.png', 'clever4-kit-white.png', '26_1.png', 'battery_holder.stl', \
-    'camera_case.stl', 'camera_mount.stl', 'grip_right.stl', 'grip_left.stl'
+    'camera_case.stl', 'camera_mount.stl'
 
 code = 0
 

clover/CHANGELOG.rst

@@ -2,52 +2,6 @@
 Changelog for package clover
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
-0.25 (2024-07-28)
------------------
-* Optimize displaying newlines in the topic viewer, add width and indent parameters.
-* Link assets instead of copying in documentation to save space.
-* Install image_geometry and dynamic_reconfigure as clover dependencies.
-* Add dictionary parameter to aruco.launch.
-* Solve the issue with aruco_detect not running when aruco_map is not enabled.
-* Documentation improvements.
-* Rest changes.
-
-0.24 (2023-10-11)
------------------
-* Significant update to autonomous flights API.
-* Updates to selfcheck.py.
-* Support PX4 v1.14 parameters.
-* Added scripts for automatic testing of autonomous flights.
-* Added new examples for working with the camera, including a red circle model and its recognition and following.
-* Implemented long_callback Python decorator to address the issue #218.
-* Implemented optical_flow/enabled dynamic parameter.
-* Updated LED strip native library to support RPi 4 rev. 1.5.
-* Show number of messages received in web topic viewer.
-* Run main_camera/image_raw_throttled topic by default.
-* Added rectify argument to main_camera.launch
-* Added udev rules for all supported autopilots by PX4.
-* Various changes.
-
-0.23 (2022-02-10)
------------------
-* Web tool for topics monitoring.
-* Publish optical flow when local position is not available.
-* Force estimator init.
-* Web viewer for Clover logs.
-* selfcheck.py improvements.
-* Various changes.
-
-0.22 (2021-06-07)
------------------
-* Move to ROS Noetic and Python 3.
-* aruco.launch: add placement, length and map arguments.
-* Web: add link for viewing the error log.
-* LED: add error/ignore parameter to not flash on some errors.
-* Wait for FC and camera devices before launching mavros and camera driver.
-* clover.launch: disable rc node by default.
-* optical_flow: publish debug image even when calc_flow_gyro failed.
-* Various changes.
-
 0.21.1 (2020-11-17)
 -------------------
 * First release of clover package to ROS

clover/CMakeLists.txt

@@ -30,8 +30,6 @@ find_package(catkin REQUIRED COMPONENTS
 
 list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/cmake")
 
-# https://github.com/mavlink/mavros/blob/7f1a8/mavros/CMakeLists.txt#L42
-set(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH};/usr/share/cmake/geographiclib")
 find_package(GeographicLib REQUIRED)
 
 # Workaround for OpenCV 3/4 support
@@ -82,10 +80,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(
@@ -93,9 +92,6 @@ add_service_files(
   GetTelemetry.srv
   Navigate.srv
   NavigateGlobal.srv
-  SetAltitude.srv
-  SetYaw.srv
-  SetYawRate.srv
   SetPosition.srv
   SetVelocity.srv
   SetAttitude.srv
@@ -234,6 +230,9 @@ target_link_libraries(${PROJECT_NAME}
   ${OpenCV_LIBRARIES}
 )
 
+# Set Clover to default www page
+set(ROSWWW_STATIC_DEFAULT ${PROJECT_NAME})
+
 #############
 ## Install ##
 #############
@@ -310,5 +309,4 @@ endif()
 if (CATKIN_ENABLE_TESTING)
   find_package(rostest REQUIRED)
   add_rostest(test/basic.test)
-  add_rostest(test/offboard.test)
 endif()

clover/README.md

@@ -50,6 +50,6 @@ To start connection to the flight controller, use:
 roslaunch clover clover.launch
 ```
 
-For the simulation information see the [corresponding article](https://clovercoex.tech/en/simulation.html).
+For the simulation information see the [corresponding article](https://clover.coex.tech/en/simulation.html).
 
 > Note that the package is configured to connect to `/dev/px4fmu` by default (see [previous section](#manual-installation)). Install udev rules or specify path to your FCU device in `mavros.launch`.

clover/cmake/FindGeographicLib.cmake

@@ -0,0 +1,18 @@
+# taken from: https://github.com/mavlink/mavros/blob/master/libmavconn/cmake/Modules/FindGeographicLib.cmake
+
+# Look for GeographicLib
+#
+# Set
+#  GEOGRAPHICLIB_FOUND = TRUE
+#  GeographicLib_INCLUDE_DIRS = /usr/local/include
+#  GeographicLib_LIBRARIES = /usr/local/lib/libGeographic.so
+#  GeographicLib_LIBRARY_DIRS = /usr/local/lib
+
+find_path (GeographicLib_INCLUDE_DIRS NAMES GeographicLib/Config.h)
+
+find_library (GeographicLib_LIBRARIES NAMES Geographic)
+
+include (FindPackageHandleStandardArgs)
+find_package_handle_standard_args (GeographicLib DEFAULT_MSG
+  GeographicLib_LIBRARIES GeographicLib_INCLUDE_DIRS)
+mark_as_advanced (GeographicLib_LIBRARIES GeographicLib_INCLUDE_DIRS)

clover/examples/camera.py

@@ -1,11 +1,11 @@
-# Information: https://clovercoex.tech/camera
+# 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.
+# - prints its name to the console. 
 
 import rospy
 import cv2
@@ -21,7 +21,7 @@ center_pub = rospy.Publisher('~center', Image, queue_size=1)
 
 def get_color_name(h):
     if h < 15: return 'red'
-    elif h < 30: return 'orange'
+    if h < 30: return 'orange'
     elif h < 60: return 'yellow'
     elif h < 90: return 'green'
     elif h < 120: return 'cyan'

clover/examples/flight.py

@@ -1,4 +1,4 @@
-# Information: https://clovercoex.tech/programming
+# Information: https://clover.coex.tech/programming
 
 import rospy
 from clover import srv

clover/examples/flight_marker.py

@@ -1,4 +1,4 @@
-# Information: https://clovercoex.tech/aruco
+# Information: https://clover.coex.tech/aruco
 
 import rospy
 from clover import srv

clover/examples/get_telemetry.py

@@ -1,4 +1,4 @@
-# Information: https://clovercoex.tech/en/simple_offboard.html#gettelemetry
+# Information: https://clover.coex.tech/en/simple_offboard.html#gettelemetry
 
 import rospy
 from clover import srv

clover/examples/gps.py

@@ -1,4 +1,4 @@
-# Information: https://clovercoex.tech/en/simple_offboard.html#navigateglobal
+# Information: https://clover.coex.tech/en/simple_offboard.html#navigateglobal
 
 import rospy
 from clover import srv
@@ -16,7 +16,7 @@ set_attitude = rospy.ServiceProxy('set_attitude', srv.SetAttitude)
 set_rates = rospy.ServiceProxy('set_rates', srv.SetRates)
 land = rospy.ServiceProxy('land', Trigger)
 
-# https://clovercoex.tech/en/snippets.html#wait_arrival
+# https://clover.coex.tech/en/snippets.html#wait_arrival
 def wait_arrival(tolerance=0.2):
     while not rospy.is_shutdown():
         telem = get_telemetry(frame_id='navigate_target')
@@ -27,7 +27,7 @@ def wait_arrival(tolerance=0.2):
 start = get_telemetry()
 
 if math.isnan(start.lat):
-    raise Exception('No global position, install and configure GPS sensor: https://clovercoex.tech/gps')
+    raise Exception('No global position, install and configure GPS sensor: https://clover.coex.tech/gps')
 
 print('Start point global position: lat={}, lon={}'.format(start.lat, start.lon))
 

clover/examples/leds.py

@@ -1,4 +1,4 @@
-# Information: https://clovercoex.tech/led
+# Information: https://clover.coex.tech/led
 
 import rospy
 from clover.srv import SetLEDEffect

clover/examples/navigate_wait.py

@@ -1,4 +1,4 @@
-# Information: https://clovercoex.tech/en/snippets.html#navigate_wait
+# Information: https://clover.coex.tech/en/snippets.html#navigate_wait
 
 import math
 import rospy
@@ -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,91 +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://clovercoex.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
-import image_geometry
-
-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_model = image_geometry.PinholeCameraModel()
-camera_model.fromCameraInfo(rospy.wait_for_message('main_camera/camera_info', CameraInfo))
-
-
-def img_xy_to_point(xy, dist):
-    xy_rect = camera_model.rectifyPoint(xy)
-    ray = camera_model.projectPixelTo3dRay(xy_rect)
-    return Point(x=ray[0] * dist, y=ray[1] * dist, 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/examples/subscriber.py

@@ -1,4 +1,4 @@
-# Information: https://clovercoex.tech/en/laser.html
+# Information: https://clover.coex.tech/en/laser.html
 
 import rospy
 from sensor_msgs.msg import Range

clover/launch/aruco.launch

@@ -6,7 +6,7 @@
     <arg name="length" default="0.22"/> <!-- not-in-map markers length, m -->
     <arg name="map" default="map.txt"/> <!-- markers map file name -->
 
-    <!-- For additional help go to https://clovercoex.tech/aruco -->
+    <!-- For additional help go to https://clover.coex.tech/aruco -->
 
     <arg name="force_init" default="false"/>
     <arg name="disable" default="false"/> <!-- only force init -->
@@ -16,12 +16,11 @@
         <remap from="image_raw" to="main_camera/image_raw"/>
         <remap from="camera_info" to="main_camera/camera_info"/>
         <remap from="map_markers" to="aruco_map/map"/>
-        <param name="dictionary" value="2"/> <!-- DICT_4X4_250 -->
         <param name="estimate_poses" value="true"/>
         <param name="send_tf" value="true"/>
-        <param name="use_map_markers" value="$(arg aruco_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="use_map_markers" value="true"/>
+        <param name="known_tilt" value="map" if="$(eval placement == 'floor')"/>
+        <param name="known_tilt" value="map_flipped" if="$(eval placement == 'ceiling')"/>
         <param name="length" value="$(arg length)"/>
         <param name="transform_timeout" value="0.1"/>
         <!-- aruco detector parameters -->
@@ -37,8 +36,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)"/>
@@ -54,4 +53,8 @@
         <param name="force_init" value="$(arg force_init)"/>
         <param name="offset_frame_id" value="aruco_map"/>
     </node>
+
+    <!-- run map_flipped frame if placement is ceiling -->
+    <node pkg="tf2_ros" type="static_transform_publisher" name="map_flipped_frame"
+        args="0 0 0 3.1415926 3.1415926 0 map map_flipped" if="$(eval placement == 'ceiling')"/>
 </launch>

clover/launch/clover.launch

@@ -45,13 +45,12 @@
         <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"/>
+        <param name="disable_on_vpe" value="false"/>
     </node>
 
     <!-- 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 +71,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)"/>
@@ -88,5 +84,4 @@
         <!-- Send fake GCS heartbeats. Set to "true" for upstream PX4 -->
         <param name="use_fake_gcs" value="false"/>
     </node>
-
 </launch>

clover/launch/led.launch

@@ -7,7 +7,7 @@
 
     <arg name="simulator" default="false"/>
 
-    <!-- For additional help go to https://clovercoex.tech/led -->
+    <!-- For additional help go to https://clover.coex.tech/led -->
 
     <!-- ws281x led strip driver -->
     <node pkg="ws281x" name="led" type="ws281x_node" clear_params="true" output="screen" if="$(eval ws281x and not simulator)">
@@ -21,8 +21,7 @@
     </node>
 
     <!-- high level led effects control, events notification with leds -->
-    <node pkg="clover" name="led_effect" type="led" clear_params="true" output="screen" if="$(arg led_effect)">
-        <param name="led" value="led"/>
+    <node pkg="clover" name="led_effect" type="led" ns="led" clear_params="true" output="screen" if="$(arg led_effect)">
         <param name="blink_rate" value="2"/>
         <param name="fade_period" value="0.5"/>
         <param name="rainbow_period" value="5"/>

clover/launch/main_camera.launch

@@ -1,12 +1,11 @@
 <launch>
-    <!-- article about camera setup: https://clovercoex.tech/camera_setup -->
+    <!-- article about camera setup: https://clover.coex.tech/camera_setup -->
 
     <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="rectify" default="false"/> <!-- enable rectification -->
     <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"/>
@@ -50,11 +49,4 @@
     <!-- 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)"/>
-
-    <!-- rectified image topic -->
-    <node pkg="nodelet" type="nodelet" name="rectify" args="load image_proc/rectify main_camera_nodelet_manager" if="$(arg rectify)">
-        <remap from="image_mono" to="main_camera/image_raw"/>
-        <remap from="camera_info" to="main_camera/camera_info"/>
-        <remap from="image_rect" to="main_camera/image_rect"/>
-    </node>
 </launch>

clover/launch/mavros.launch

@@ -77,6 +77,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 +1,4 @@
 <launch>
-    <!-- shortcut for running the simulation (`roslaunch clover simulator.launch`) -->
+    <!-- shurtcut 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,13 +1,13 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>clover</name>
-  <version>0.25.0</version>
+  <version>0.23.0</version>
   <description>The Clover package</description>
 
   <maintainer email="okalachev@gmail.com">Oleg Kalachev</maintainer>
   <license>MIT</license>
 
-  <url type="website">https://clovercoex.tech/</url>
+  <url type="website">https://clover.coex.tech/</url>
   <author email="okalachev@gmail.com">Oleg Kalachev</author>
   <author email="urpylka@gmail.com">Artem Smirnov</author>
 
@@ -42,10 +42,9 @@
   <depend condition="$ROS_PYTHON_VERSION == 2">python-lxml</depend>
   <depend condition="$ROS_PYTHON_VERSION == 3">python3-lxml</depend>
   <depend>dynamic_reconfigure</depend>
-  <depend>image_proc</depend>
-  <depend>image_geometry</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/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/led.cpp

@@ -309,19 +309,15 @@ int main(int argc, char **argv)
 	nh_priv.param("notify/low_battery/threshold", low_battery_threshold, 3.7);
 	nh_priv.param("notify/error/ignore", error_ignore, {});
 
-	std::string led; // led namespace
-	nh_priv.param("led", led, std::string("led"));
-	if (!led.empty()) led += "/";
-
-	ros::service::waitForService(led + "set_leds"); // cannot work without set_leds service
-	set_leds_srv = nh.serviceClient<led_msgs::SetLEDs>(led + "set_leds", true);
+	ros::service::waitForService("set_leds"); // cannot work without set_leds service
+	set_leds_srv = nh.serviceClient<led_msgs::SetLEDs>("set_leds", true);
 
 	// wait for leds count info
-	handleState(*ros::topic::waitForMessage<led_msgs::LEDStateArray>(led + "state", nh));
+	handleState(*ros::topic::waitForMessage<led_msgs::LEDStateArray>("state", nh));
 
-	auto state_sub = nh.subscribe(led + "state", 1, &handleState);
+	auto state_sub = nh.subscribe("state", 1, &handleState);
 
-	auto set_effect = nh.advertiseService(led + "set_effect", &setEffect);
+	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);

clover/src/selfcheck.py

@@ -9,7 +9,7 @@
 # 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
@@ -50,16 +50,6 @@ 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 = ''
-
-
 def failure(text, *args):
     msg = text % args
     thread_local.reports += [{'failure': msg}]
@@ -85,9 +75,9 @@ def check(name):
                     if 'failure' in report:
                         rospy.logerr('%s: %s', name, report['failure'])
                     elif 'info' in report:
-                        rospy.loginfo(GREY + name + END + ': ' + report['info'])
+                        rospy.loginfo('\033[90m%s\033[0m: %s', name, report['info'])
                 if not thread_local.reports:
-                    rospy.loginfo(GREY + name + END + ': ' + GREEN + 'OK' + END)
+                    rospy.loginfo('\033[90m%s\033[0m: \033[92mOK\033[0m', name)
                 if rospy.get_param('~time', False):
                     rospy.loginfo('%s: %.1f sec', name, rospy.get_time() - start)
         return wrapper
@@ -97,7 +87,7 @@ def check(name):
 def ff(value, precision=2):
     # safely format float or int
     if value is None:
-        return RED + '???' + END
+        return '\033[31m???\033[0m'
     if isinstance(value, float):
         return ('{:.' + str(precision + 1) + '}').format(value)
     elif isinstance(value, int):
@@ -107,7 +97,7 @@ def ff(value, precision=2):
 param_get = rospy.ServiceProxy('mavros/param/get', ParamGet)
 
 
-def get_param(name, default=None, strict=True):
+def get_param(name, default=None):
     try:
         res = param_get(param_id=name)
     except rospy.ServiceException as e:
@@ -115,8 +105,7 @@ def get_param(name, default=None, strict=True):
         return None
 
     if not res.success:
-        if strict:
-            failure('unable to retrieve PX4 parameter %s', name)
+        failure('unable to retrieve PX4 parameter %s', name)
         return default
     else:
         if res.value.integer != 0:
@@ -235,7 +224,6 @@ 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
@@ -259,12 +247,12 @@ def check_fcu():
                     info(line[len('HW arch: '):])
 
             if not clover_fw:
-                info('not Clover PX4 firmware, check https://clovercoex.tech/firmware')
+                info('not Clover PX4 firmware, check https://clover.coex.tech/firmware')
 
         est = get_param('SYS_MC_EST_GROUP')
         if est == 1:
             info('selected estimator: LPE')
-            fuse = int(get_param('LPE_FUSION'))
+            fuse = get_param('LPE_FUSION')
             if fuse & (1 << 4):
                 info('LPE_FUSION: land detector fusion is enabled')
             else:
@@ -300,19 +288,11 @@ def check_fcu():
             try:
                 battery = rospy.wait_for_message('mavros/battery', BatteryState, timeout=3)
                 if not battery.cell_voltage:
-                    failure('cell voltage is not available, https://clovercoex.tech/power')
+                    failure('cell voltage is not available, https://clover.coex.tech/power')
                 else:
                     cell = battery.cell_voltage[0]
-                    # number of cells 1 means this is overall voltage
-                    if len(battery.cell_voltage) == 1:
-                        n_cells = get_param('BAT1_N_CELLS', strict=False)
-                        if n_cells is None:
-                            # older PX4
-                            n_cells = get_param('BAT_N_CELLS', strict=True)
-                        cell /= n_cells
-
                     if cell > 4.3 or cell < 3.0:
-                        failure('incorrect cell voltage: %.2f V, https://clovercoex.tech/power', cell)
+                        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:
@@ -325,14 +305,7 @@ def check_fcu():
             failure('cannot read time sync offset')
 
     except rospy.ROSException:
-        failure('no MAVROS state')
-        fcu_url = rospy.get_param('mavros/fcu_url', '?')
-        if fcu_url == '/dev/px4fmu':
-            if not os.path.exists('/lib/udev/rules.d/99-px4fmu.rules'):
-                info('udev rules are not installed, install udev rules or change usb_device to /dev/ttyACM0 in mavros.launch')
-            else:
-                info('udev did\'t recognize px4fmu device, check wiring or change usb_device to /dev/ttyACM0 in mavros.launch')
-        info('fcu_url = %s', rospy.get_param('mavros/fcu_url', '?'))
+        failure('no MAVROS state (check wiring)')
 
 
 def describe_direction(v):
@@ -413,18 +386,15 @@ def check_aruco():
 
     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_detect/known_tilt = %s', known_tilt)
         try:
             markers = rospy.wait_for_message('aruco_detect/markers', MarkerArray, timeout=0.8)
         except rospy.ROSException:
@@ -435,15 +405,12 @@ 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)
@@ -483,8 +450,7 @@ def check_vpe():
         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):
+            elif rospy.get_param('aruco_map/known_tilt') == 'map_flipped':
                 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')
@@ -502,7 +468,7 @@ def check_vpe():
             failure('vision yaw weight is zero, change ATT_W_EXT_HDG parameter')
         else:
             info('vision yaw weight: %s', ff(vision_yaw_w))
-        fuse = int(get_param('LPE_FUSION'))
+        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')
@@ -510,22 +476,11 @@ def check_vpe():
             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'))
     elif est == 2:
-        ev_ctrl = get_param('EKF2_EV_CTRL', strict=False)
-        if ev_ctrl is not None: # PX4 after v1.14
-            ev_ctrl = int(ev_ctrl)
-            if not ev_ctrl & (1 << 0):
-                failure('vision horizontal position fusion is disabled, change EKF2_EV_CTRL parameter')
-            if not ev_ctrl & (1 << 1):
-                failure('vision vertical position fusion is disabled, change EKF2_EV_CTRL parameter')
-            if not ev_ctrl & (1 << 3):
-                failure('vision yaw fusion is disabled, change EKF2_EV_CTRL parameter')
-        else: # PX4 before v1.14
-            fuse = int(get_param('EKF2_AID_MASK'))
-            if not fuse & (1 << 3):
-                failure('vision position fusion is disabled, change EKF2_AID_MASK parameter')
-            if not fuse & (1 << 4):
-                failure('vision yaw fusion is disabled, change EKF2_AID_MASK parameter')
-
+        fuse = get_param('EKF2_AID_MASK')
+        if not fuse & (1 << 3):
+            failure('vision position fusion is disabled, change EKF2_AID_MASK parameter')
+        if not fuse & (1 << 4):
+            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)
@@ -632,22 +587,12 @@ def check_global_position():
         rospy.wait_for_message('mavros/global_position/global', NavSatFix, timeout=0.8)
     except rospy.ROSException:
         info('no global position')
-        if get_param('SYS_MC_EST_GROUP') == 2:
-            gps_ctrl = get_param('EKF2_GPS_CTRL', strict=False)
-            if gps_ctrl is not None: # PX4 after v1.14
-                if int(gps_ctrl) & (1 << 0):
-                    failure('GPS fusion enabled may suppress vision position aiding, change EKF2_GPS_CTRL')
-            else: # PX4 before v1.14
-                if int(get_param('EKF2_AID_MASK', 0)) & (1 << 0):
-                    failure('GPS fusion enabled may suppress vision position aiding, change EKF2_AID_MASK')
+        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)
@@ -658,7 +603,7 @@ def check_optical_flow():
             failure('SENS_FLOW_ROT = %s, but it should be zero', rot)
         est = get_param('SYS_MC_EST_GROUP')
         if est == 1:
-            fuse = int(get_param('LPE_FUSION'))
+            fuse = get_param('LPE_FUSION')
             if not fuse & (1 << 1):
                 failure('optical flow fusion is disabled, change LPE_FUSION parameter')
             if not fuse & (1 << 1):
@@ -672,14 +617,9 @@ def check_optical_flow():
                           get_paramf('LPE_FLW_R', 4),
                           get_paramf('LPE_FLW_RR', 4))
         elif est == 2:
-            of_ctrl = get_param('EKF2_OF_CTRL', strict=False)
-            if of_ctrl is not None: # PX4 after v1.14
-                if of_ctrl == 0:
-                    failure('optical flow fusion is disabled, change EKF2_OF_CTRL')
-            else: # PX4 before v1.14
-                fuse = int(get_param('EKF2_AID_MASK', 0))
-                if not fuse & (1 << 1):
-                    failure('optical flow fusion is disabled, change EKF2_AID_MASK parameter')
+            fuse = get_param('EKF2_AID_MASK', 0)
+            if not fuse & (1 << 1):
+                failure('optical flow fusion is disabled, change EKF2_AID_MASK parameter')
             delay = get_param('EKF2_OF_DELAY', 0)
             if delay != 0:
                 failure('EKF2_OF_DELAY = %.2f, but it should be zero', delay)
@@ -721,26 +661,23 @@ def check_rangefinder():
 
     est = get_param('SYS_MC_EST_GROUP')
     if est == 1:
-        fuse = int(get_param('LPE_FUSION', 0))
+        fuse = get_param('LPE_FUSION', 0)
         if not fuse & (1 << 5):
             info('"pub agl as lpos down" in LPE_FUSION is disabled, NOT operating over flat surface')
         else:
             info('"pub agl as lpos down" in LPE_FUSION is enabled, operating over flat surface')
 
     elif est == 2:
-        hgt = get_param('EKF2_HGT_REF', strict=False)
-        if hgt is None: # PX4 before v1.14
-            hgt = get_param('EKF2_HGT_MODE')
+        hgt = get_param('EKF2_HGT_MODE')
         if hgt != 2:
             info('EKF2_HGT_MODE != Range sensor, NOT operating over flat surface')
         else:
             info('EKF2_HGT_MODE = Range sensor, operating over flat surface')
-        aid = get_param('EKF2_RNG_AID', strict=False)
-        if aid is not None: # PX4 before v1.14
-            if aid != 1:
-                info('EKF2_RNG_AID != 1, range sensor aiding disabled')
-            else:
-                info('EKF2_RNG_AID = 1, range sensor aiding enabled')
+        aid = get_param('EKF2_RNG_AID')
+        if aid != 1:
+            info('EKF2_RNG_AID != 1, range sensor aiding disabled')
+        else:
+            info('EKF2_RNG_AID = 1, range sensor aiding enabled')
 
 
 @check('Boot duration')
@@ -881,7 +818,7 @@ def check_network():
                 if ros_hostname in parts:
                     break
         else:
-            failure('not found %s in /etc/hosts, ROS will malfunction if network interfaces are down, https://clovercoex.tech/hostname', ros_hostname)
+            failure('not found %s in /etc/hosts, ROS will malfunction if network interfaces are down, https://clover.coex.tech/hostname', ros_hostname)
 
 
 @check('RPi health')

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;
@@ -189,7 +170,7 @@ void handleLocalPosition(const PoseStamped& pose)
 {
 	local_position = pose;
 	publishBodyFrame();
-	// TODO: home?
+	// TODO: terrain?, home?
 }
 
 // wait for transform without interrupting publishing setpoints
@@ -207,29 +188,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 +207,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 +341,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 +385,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 +439,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 +458,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 +468,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 +484,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);
 	}
 }
@@ -646,62 +525,13 @@ inline void checkState()
 		throw std::runtime_error("State timeout, check mavros settings");
 
 	if (!state.connected)
-		throw std::runtime_error("No connection to FCU, https://clovercoex.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;
+		throw std::runtime_error("No connection to FCU, https://clover.coex.tech/connection");
 }
 
 #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 +558,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 +634,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 +664,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 +681,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,39 +788,27 @@ 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)
@@ -1048,7 +840,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 +851,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());
@@ -1081,11 +863,6 @@ bool land(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)
 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;
 }
@@ -1111,8 +888,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 +923,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,16 +931,10 @@ 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);
@@ -1193,7 +948,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;
@@ -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");
@@ -150,9 +122,8 @@ 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_priv.param<string>("frame_id", frame_id, "");
+	nh_priv.param<string>("offset_frame_id", offset_frame_id, "");
 	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");
 	offset_timeout = ros::Duration(nh_priv.param("offset_timeout", 3.0));

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.test

@@ -40,16 +40,6 @@
     <node pkg="topic_tools" name="main_camera_throttle" type="throttle" ns="main_camera"
         args="messages image_raw 5.0 image_raw_throttled" required="true"/>
 
-    <node pkg="nodelet" type="nodelet" name="main_camera_nodelet_manager" args="manager" output="screen" required="true">
-        <param name="num_worker_threads" value="2"/>
-    </node>
-
-    <node pkg="nodelet" type="nodelet" name="rectify" args="load image_proc/rectify main_camera_nodelet_manager" required="true">
-        <remap from="image_mono" to="main_camera/image_raw"/>
-        <remap from="camera_info" to="main_camera/camera_info"/>
-        <remap from="image_rect" to="main_camera/image_rect"/>
-    </node>
-
     <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/udev/99-px4fmu.rules

@@ -1,54 +1,17 @@
-# Source files: PX4-Autopilot/boards/**/nuttx-config/nsh/defconfig
-
-# Additional info:
-# https://docs.px4.io/main/en/flight_controller/
-# https://github.com/mavlink/qgroundcontrol/blob/master/src/comm/USBBoardInfo.json
-
-SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0001", ATTRS{product}=="PX4 GNF405", SYMLINK+="px4fmu"
-SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0001", ATTRS{product}=="PX4 OmnibusF4SD", SYMLINK+="px4fmu"
-SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0016", ATTRS{product}=="PX4 Crazyflie v2.0", SYMLINK+="px4fmu"
-SUBSYSTEM=="tty", ATTRS{idVendor}=="1FC9", ATTRS{idProduct}=="001c", ATTRS{product}=="PX4 FMUK66 v3.x", SYMLINK+="px4fmu"
-SUBSYSTEM=="tty", ATTRS{idVendor}=="1FC9", ATTRS{idProduct}=="001c", ATTRS{product}=="PX4 FMUK66 E", SYMLINK+="px4fmu"
-SUBSYSTEM=="tty", ATTRS{idVendor}=="1FC9", ATTRS{idProduct}=="001d", ATTRS{product}=="PX4 FMURT1062 v1.x", SYMLINK+="px4fmu"
-SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0001", ATTRS{product}=="DiatoneMambaF405 MK2", SYMLINK+="px4fmu"
-SUBSYSTEM=="tty", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="a32f", ATTRS{product}=="PX4 FMU ModalAI FCv1", SYMLINK+="px4fmu"
-SUBSYSTEM=="tty", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="a330", ATTRS{product}=="PX4 FMU ModalAI FCv2", SYMLINK+="px4fmu"
-SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0012", ATTRS{product}=="PX4 FMU UVify Core", SYMLINK+="px4fmu"
-SUBSYSTEM=="tty", ATTRS{idVendor}=="3162", ATTRS{idProduct}=="0050", ATTRS{product}=="PX4 KakuteH7", SYMLINK+="px4fmu"
-SUBSYSTEM=="tty", ATTRS{idVendor}=="3162", ATTRS{idProduct}=="0050", ATTRS{product}=="PX4 KakuteH7v2", SYMLINK+="px4fmu"
-SUBSYSTEM=="tty", ATTRS{idVendor}=="3162", ATTRS{idProduct}=="004b", ATTRS{product}=="PX4 DurandalV1", SYMLINK+="px4fmu"
-SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0050", ATTRS{product}=="PX4 KakuteF7", SYMLINK+="px4fmu"
-SUBSYSTEM=="tty", ATTRS{idVendor}=="3162", ATTRS{idProduct}=="0050", ATTRS{product}=="PX4 KakuteH7Mini-nand", SYMLINK+="px4fmu"
-SUBSYSTEM=="tty", ATTRS{idVendor}=="3162", ATTRS{idProduct}=="004E", ATTRS{product}=="PX4 PIX32V5", SYMLINK+="px4fmu"
-SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0061", ATTRS{product}=="PX4 ATL Mantis-EDU", SYMLINK+="px4fmu"
-SUBSYSTEM=="tty", ATTRS{idVendor}=="3163", ATTRS{idProduct}=="004c", ATTRS{product}=="PX4 CUAV Nora", SYMLINK+="px4fmu"
-SUBSYSTEM=="tty", ATTRS{idVendor}=="3163", ATTRS{idProduct}=="004c", ATTRS{product}=="PX4 CUAV X7Pro", SYMLINK+="px4fmu"
-SUBSYSTEM=="tty", ATTRS{idVendor}=="1B8C", ATTRS{idProduct}=="0036", ATTRS{product}=="MatekH743-mini", SYMLINK+="px4fmu"
-SUBSYSTEM=="tty", ATTRS{idVendor}=="1B8C", ATTRS{idProduct}=="0036", ATTRS{product}=="MatekH743", SYMLINK+="px4fmu"
-SUBSYSTEM=="tty", ATTRS{idVendor}=="120A", ATTRS{idProduct}=="1004", ATTRS{product}=="Matekgnssm9nf4", SYMLINK+="px4fmu"
-SUBSYSTEM=="tty", ATTRS{idVendor}=="1209", ATTRS{idProduct}=="1013", ATTRS{product}=="MatekH743", SYMLINK+="px4fmu"
-SUBSYSTEM=="tty", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="0037", ATTRS{product}=="PX4 FMU SmartAP AIRLink", SYMLINK+="px4fmu"
-SUBSYSTEM=="tty", ATTRS{idVendor}=="2DAE", ATTRS{idProduct}=="1058", ATTRS{product}=="CubeOrange+", SYMLINK+="px4fmu"
-SUBSYSTEM=="tty", ATTRS{idVendor}=="2DAE", ATTRS{idProduct}=="1012", ATTRS{product}=="CubeYellow", SYMLINK+="px4fmu"
-SUBSYSTEM=="tty", ATTRS{idVendor}=="2DAE", ATTRS{idProduct}=="1016", ATTRS{product}=="CubeOrange", SYMLINK+="px4fmu"
-SUBSYSTEM=="tty", ATTRS{idVendor}=="3185", ATTRS{idProduct}=="0035", ATTRS{product}=="PX4 FMU v6X.x", SYMLINK+="px4fmu"
-SUBSYSTEM=="tty", ATTRS{idVendor}=="3185", ATTRS{idProduct}=="0038", ATTRS{product}=="PX4 FMU v6C.x", SYMLINK+="px4fmu"
-SUBSYSTEM=="tty", ATTRS{idVendor}=="3185", ATTRS{idProduct}=="0033", ATTRS{product}=="PX4 FMU v5X.x", SYMLINK+="px4fmu"
-SUBSYSTEM=="tty", ATTRS{idVendor}=="1B8C", ATTRS{idProduct}=="0036", ATTRS{product}=="PX4 FMU v6U.x", SYMLINK+="px4fmu"
-SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0013", ATTRS{product}=="PX4 FMU v4.x PRO", SYMLINK+="px4fmu"
+# PixHawk (px4fmu-v2), px4fmu-v3
 SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0011", ATTRS{product}=="PX4 FMU v2.x", SYMLINK+="px4fmu"
+# PixRacer (px4fmu-v4)
 SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0012", ATTRS{product}=="PX4 FMU v4.x", SYMLINK+="px4fmu"
+# px4fmu-v5
 SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0032", ATTRS{product}=="PX4 FMU v5.x", SYMLINK+="px4fmu"
-SUBSYSTEM=="tty", ATTRS{idVendor}=="3162", ATTRS{idProduct}=="004b", ATTRS{product}=="PX4 SP RACING H7 EXTREME", SYMLINK+="px4fmu"
-SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0030", ATTRS{product}=="MindPX FMU v2.x", SYMLINK+="px4fmu"
-SUBSYSTEM=="tty", ATTRS{idVendor}=="3185", ATTRS{idProduct}=="0039", ATTRS{product}=="ARK FMU v6X.x", SYMLINK+="px4fmu"
-SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0016", ATTRS{product}=="PX4 FreeFly RTK GPS", SYMLINK+="px4fmu"
-SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="1024", ATTRS{product}=="mRoControlZeroH7 OEM", SYMLINK+="px4fmu"
-SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="1017", ATTRS{product}=="mRoPixracerPro", SYMLINK+="px4fmu"
-SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="1023", ATTRS{product}=="mRoControlZeroH7", SYMLINK+="px4fmu"
-SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="008D", ATTRS{product}=="mRoControlZeroF7", SYMLINK+="px4fmu"
-SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0021", ATTRS{product}=="PX4 AUAV X2.1", SYMLINK+="px4fmu"
-SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="1022", ATTRS{product}=="mRoControlZero Classic", SYMLINK+="px4fmu"
-SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0088", ATTRS{product}=="mRo x2.1-777", SYMLINK+="px4fmu"
-SUBSYSTEM=="tty", ATTRS{idVendor}=="35a7", ATTRS{idProduct}=="0002", ATTRS{product}=="FCC-R1", SYMLINK+="px4fmu"
-SUBSYSTEM=="tty", ATTRS{idVendor}=="35a7", ATTRS{idProduct}=="0001", ATTRS{product}=="FCC-K1", SYMLINK+="px4fmu"
+# auav
+SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0021", ATTRS{product}=="PX4 AUAV x2.1", SYMLINK+="px4fmu"
+# crazyflie
+SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0016", ATTRS{product}=="PX4 Crazyflie v2.0", SYMLINK+="px4fmu"
+# px4fmu-v4pro
+SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0013", ATTRS{product}=="PX4 FMU v4.x PRO", SYMLINK+="px4fmu"
+# Omnibus
+SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0001", ATTRS{product}=="PX4 OmnibusF4SD", SYMLINK+="px4fmu"
+# CUAV X7 Pro
+SUBSYSTEM=="tty", ATTRS{idVendor}=="3163", ATTRS{idProduct}=="004c", ATTRS{product}=="PX4 CUAV X7Pro", SYMLINK+="px4fmu"
+

clover/www/index.html

@@ -3,7 +3,7 @@
 <h1>Clover Drone Kit Tools</h1>
 
 <ul>
-	<li><a href="docs">View documentation</a> (snapshot of <a href="https://clovercoex.tech">clovercoex.tech</a>)</li>
+	<li><a href="docs">View documentation</a> (snapshot of <a href="https://clover.coex.tech">clover.coex.tech</a>)</li>
 	<li><a href="topics.html">View topics</a></li>
 	<li><a href="" id="wvs">View image topics</a> (<code>web_video_server</code>)</li>
 	<li><a href="" id="butterfly">Open web terminal</a> (<code>Butterfly</code>)</li>
@@ -12,6 +12,10 @@
 	<li><a href="console.html">Clover console</a> (<code>/var/log/clover.log</code>)</li>
 </ul>
 
+<p>
+	Update www using <code>rosrun roswww_static update</code>.
+</p>
+
 <div class="version"></div>
 
 <script type="text/javascript">

clover/www/js/json-to-pretty-yaml.js

@@ -0,0 +1,236 @@
+// Browserified https://www.npmjs.com/package/json-to-pretty-yaml module
+
+(function(){function r(e,n,t){function o(i,f){if(!n[i]){if(!e[i]){var c="function"==typeof require&&require;if(!f&&c)return c(i,!0);if(u)return u(i,!0);var a=new Error("Cannot find module '"+i+"'");throw a.code="MODULE_NOT_FOUND",a}var p=n[i]={exports:{}};e[i][0].call(p.exports,function(r){var n=e[i][1][r];return o(n||r)},p,p.exports,r,e,n,t)}return n[i].exports}for(var u="function"==typeof require&&require,i=0;i<t.length;i++)o(t[i]);return o}return r})()({1:[function(require,module,exports){
+(function() {
+    "use strict";
+
+    var typeOf = require('remedial').typeOf;
+    var trimWhitespace = require('remove-trailing-spaces');
+
+    function stringify(data) {
+        var handlers, indentLevel = '';
+
+        handlers = {
+            "undefined": function() {
+                // objects will not have `undefined` converted to `null`
+                // as this may have unintended consequences
+                // For arrays, however, this behavior seems appropriate
+                return 'null';
+            },
+            "null": function() {
+                return 'null';
+            },
+            "number": function(x) {
+                return x;
+            },
+            "boolean": function(x) {
+                return x ? 'true' : 'false';
+            },
+            "string": function(x) {
+                // to avoid the string "true" being confused with the
+                // the literal `true`, we always wrap strings in quotes
+                return JSON.stringify(x);
+            },
+            "array": function(x) {
+                var output = '';
+
+                if (0 === x.length) {
+                    output += '[]';
+                    return output;
+                }
+
+                indentLevel = indentLevel.replace(/$/, '  ');
+                x.forEach(function(y, i) {
+                    // TODO how should `undefined` be handled?
+                    var handler = handlers[typeOf(y)];
+
+                    if (!handler) {
+                        throw new Error('what the crap: ' + typeOf(y));
+                    }
+
+                    output += '\n' + indentLevel + '- ' + handler(y, true);
+
+                });
+                indentLevel = indentLevel.replace(/  /, '');
+
+                return output;
+            },
+            "object": function(x, inArray, rootNode) {
+                var output = '';
+
+                if (0 === Object.keys(x).length) {
+                    output += '{}';
+                    return output;
+                }
+
+                if (!rootNode) {
+                    indentLevel = indentLevel.replace(/$/, '  ');
+                }
+
+                Object.keys(x).forEach(function(k, i) {
+                    var val = x[k],
+                        handler = handlers[typeOf(val)];
+
+                    if ('undefined' === typeof val) {
+                        // the user should do
+                        // delete obj.key
+                        // and not
+                        // obj.key = undefined
+                        // but we'll error on the side of caution
+                        return;
+                    }
+
+                    if (!handler) {
+                        throw new Error('what the crap: ' + typeOf(val));
+                    }
+
+                    if (!(inArray && i === 0)) {
+                        output += '\n' + indentLevel;
+                    }
+
+                    output += k + ': ' + handler(val);
+                });
+                indentLevel = indentLevel.replace(/  /, '');
+
+                return output;
+            },
+            "function": function() {
+                // TODO this should throw or otherwise be ignored
+                return '[object Function]';
+            }
+        };
+        return trimWhitespace(handlers[typeOf(data)](data, true, true) + '\n');
+
+    }
+
+    window.yamlStringify = stringify;
+    module.exports.stringify = stringify;
+}());
+
+},{"remedial":2,"remove-trailing-spaces":3}],2:[function(require,module,exports){
+/*jslint onevar: true, undef: true, nomen: true, eqeqeq: true, plusplus: true, bitwise: true, regexp: true, newcap: true, immed: true */
+(function () {
+    "use strict";
+
+    var global = Function('return this')()
+      , classes = "Boolean Number String Function Array Date RegExp Object".split(" ")
+      , i
+      , name
+      , class2type = {}
+      ;
+
+    for (i in classes) {
+      if (classes.hasOwnProperty(i)) {
+        name = classes[i];
+        class2type["[object " + name + "]"] = name.toLowerCase();
+      }
+    }
+
+    function typeOf(obj) {
+      return (null === obj || undefined === obj) ? String(obj) : class2type[Object.prototype.toString.call(obj)] || "object";
+    }
+
+    function isEmpty(o) {
+        var i, v;
+        if (typeOf(o) === 'object') {
+            for (i in o) { // fails jslint
+                v = o[i];
+                if (v !== undefined && typeOf(v) !== 'function') {
+                    return false;
+                }
+            }
+        }
+        return true;
+    }
+
+    if (!String.prototype.entityify) {
+        String.prototype.entityify = function () {
+            return this.replace(/&/g, "&amp;").replace(/</g,
+                "&lt;").replace(/>/g, "&gt;");
+        };
+    }
+
+    if (!String.prototype.quote) {
+        String.prototype.quote = function () {
+            var c, i, l = this.length, o = '"';
+            for (i = 0; i < l; i += 1) {
+                c = this.charAt(i);
+                if (c >= ' ') {
+                    if (c === '\\' || c === '"') {
+                        o += '\\';
+                    }
+                    o += c;
+                } else {
+                    switch (c) {
+                    case '\b':
+                        o += '\\b';
+                        break;
+                    case '\f':
+                        o += '\\f';
+                        break;
+                    case '\n':
+                        o += '\\n';
+                        break;
+                    case '\r':
+                        o += '\\r';
+                        break;
+                    case '\t':
+                        o += '\\t';
+                        break;
+                    default:
+                        c = c.charCodeAt();
+                        o += '\\u00' + Math.floor(c / 16).toString(16) +
+                            (c % 16).toString(16);
+                    }
+                }
+            }
+            return o + '"';
+        };
+    } 
+
+    if (!String.prototype.supplant) {
+        String.prototype.supplant = function (o) {
+            return this.replace(/{([^{}]*)}/g,
+                function (a, b) {
+                    var r = o[b];
+                    return typeof r === 'string' || typeof r === 'number' ? r : a;
+                }
+            );
+        };
+    }
+
+    if (!String.prototype.trim) {
+        String.prototype.trim = function () {
+            return this.replace(/^\s*(\S*(?:\s+\S+)*)\s*$/, "$1");
+        };
+    }
+
+    // CommonJS / npm / Ender.JS
+    module.exports = {
+        typeOf: typeOf,
+        isEmpty: isEmpty
+    };
+    global.typeOf = global.typeOf || typeOf;
+    global.isEmpty = global.isEmpty || isEmpty;
+}());
+
+},{}],3:[function(require,module,exports){
+"use strict";
+
+/**
+ * removeTrailingSpaces
+ * Remove the trailing spaces from a string.
+ *
+ * @name removeTrailingSpaces
+ * @function
+ * @param {String} input The input string.
+ * @returns {String} The output string.
+ */
+
+module.exports = function removeTrailingSpaces(input) {
+  // TODO If possible, use a regex
+  return input.split("\n").map(function (x) {
+    return x.trimRight();
+  }).join("\n");
+};
+},{}]},{},[1]);

clover/www/js/topics.js

@@ -64,11 +64,8 @@ function viewTopic(topic) {
 			}
 		}
 
-		let width = Number(params.width) || 100;
-		let indent = Number(params.indent) || 2;
-		let txt = YAML.stringify(msg, { lineWidth: width, indent: indent });
-		let html = `<div class=counter>${counter} received</div>${txt}`; // JSON.stringify(msg, null, 4);
-		topicMessage.innerHTML = html;
+		let txt = `<div class=counter>${counter} received</div>${yamlStringify(msg)}`; // JSON.stringify(msg, null, 4);
+		topicMessage.innerHTML = txt;
 	});
 }
 

clover/www/topics.html

@@ -4,7 +4,7 @@
 		<script src="js/roslib.js"></script>
 		<link rel="icon" href="data:,"> <!-- make chrome don't request icon -->
 		<script type="module" src="js/topics.js"></script>
-		<script src="js/yaml.js"></script>
+		<script src="js/json-to-pretty-yaml.js"></script>
 		<style>
 			#topics { line-height: 1.2em; }
 			#topic-view {

clover_blocks/README.md

@@ -4,7 +4,7 @@ Blockly programming support for Clover.
 
 <img src="screenshot.png" width=700>
 
-See user documentation at the [main Clover documentation site](https://clovercoex.tech/en/blocks.html).
+See user documentation at the [main Clover documentation site](https://clover.coex.tech/en/blocks.html).
 
 Internal package documentation is given below.
 
@@ -47,7 +47,6 @@ http://<hostname>/clover_blocks/?navigate_tolerance=0.5&sleep_time=0.1
 
 * `~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.
 * `~prompt` ([*clover_blocks/Prompt*](msg/Prompt.msg)) – user input request (includes random request ID string).
 

clover_blocks/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>clover_blocks</name>
-  <version>0.25.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/src/clover_blocks

@@ -166,7 +166,7 @@ def load(req):
         return {'names': [], 'programs': [], 'message': str(e)}
 
 
-name_regexp = re.compile(r'^[a-zA-Z1-9-_.]{0,30}$')
+name_regexp = re.compile(r'^[a-zA-Z-_.]{0,20}$')
 
 def store(req):
     if not name_regexp.match(req.name):

clover_blocks/www/blocks.js

@@ -12,10 +12,9 @@ const COLOR_FLIGHT = 293;
 const COLOR_STATE = 36;
 const COLOR_LED = 143;
 const COLOR_GPIO = 200;
-const DOCS_URL = 'https://clovercoex.tech/en/blocks.html';
+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.25.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_simulation/airframes/4500_clover

@@ -31,7 +31,7 @@ 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_HGT_MODE 2 # 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

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.25.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}"/>

docs/en/README.md

@@ -12,4 +12,4 @@ If you have studied the documentation but have not found an answer to your quest
 
 We also have a chat for programmers coding for PX4, autonomous navigation indoors, and drone swarms: [@DroneCode](tg://resolve?domain=DroneCode).
 
-You can download [PDF-version](https://clovercoex.tech/clover_en.pdf) of this documentation.
+You can download [PDF-version](https://clover.coex.tech/clover_en.pdf) of this documentation.

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)
@@ -57,7 +57,6 @@
   * [COEX Pix](coex_pix.md)
   * [COEX PDB](coex_pdb.md)
   * [COEX GPS](coex_gps.md)
-  * [Using SSH keys](ssh_keys.md)
   * [Guide on autonomous flight](auto_setup.md)
   * [Hostname](hostname.md)
   * [PX4 Simulation](sitl.md)
@@ -106,12 +105,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_map.md

@@ -136,7 +136,7 @@ navigate(x=2, y=2, z=2, speed=1, frame_id='aruco_map')
 
 ### Using a specific marker frame
 
-Starting with the [image](image.md) version 0.18, the drone also can fly relative to a marker in the map, even if it is not currently visible. Like with [single-marker navigation](aruco_marker.md#working-with-detected-markers), this works by setting the frame_id parameter to `aruco_ID`, where ID is the desired marker number.
+Starting with the [image](image.md) version 0.18, the drone also can fly relative to a marker in the map, even if it is not currently visible. Like with [single-marker navigation](aruco_marker.md#working-with-detected-markers), this works by setting the frame_id parameter to aruco_ID, where ID is the desired marker number.
 
 The following code will move the drone to the point 1 meter above the center of marker 5: