Merge branch 'master' into roswww-static-target

.github/workflows/build-image.yaml

@@ -7,7 +7,6 @@ on:
     branches: [ master ]
   release:
     types: [ created ]
-  workflow_dispatch:
 
 jobs:
   build:
@@ -17,24 +16,14 @@ jobs:
       - 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 clover_*).zip clover_* && ls -lh . && unzip -l clover_*.zip
-      - name: Compress image using 7-Zip
+      - name: Compress image
         run: |
-          cd images && sudo chmod -R 777 . && 7z a -mx=9 $(echo *_*).7z *_* && ls -lh . && 7z l *_*.7z
+          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 }}
-      - name: Upload image to artifacts
-        if: ${{ github.event_name == 'workflow_dispatch' || github.ref != 'refs/heads/master' }}
-        uses: actions/upload-artifact@v4
-        with:
-          name: image
-          path: images/*_*.7z
-          retention-days: 1

.github/workflows/build.yml

@@ -5,7 +5,6 @@ on:
     branches: [ '*' ]
   pull_request:
     branches: [ master ]
-  workflow_dispatch:
 
 jobs:
   # melodic:

.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
@@ -72,9 +75,6 @@ jobs:
 
   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@v1

.github/workflows/editorconfig.yaml

@@ -5,7 +5,6 @@ on:
     branches: [ '*' ]
   pull_request:
     branches: [ master ]
-  workflow_dispatch:
 
 jobs:
   editorconfig:

.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

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

aruco_pose/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.24.0</version>
+  <version>0.23.0</version>
   <description>Positioning with ArUco markers</description>
 
   <maintainer email="okalachev@gmail.com">Oleg Kalachev</maintainer>
@@ -28,9 +28,6 @@
   <depend>sensor_msgs</depend>
   <depend>rostest</depend>
   <depend>dynamic_reconfigure</depend>
-  <depend>pluginlib</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)

builder/assets/clover.service

@@ -5,7 +5,7 @@ Requires=roscore.service
 [Service]
 User=pi
 ExecStart=/bin/bash -c ". /home/pi/catkin_ws/devel/setup.sh; \
-                      ROS_HOSTNAME=`hostname`.local ROS_OS_OVERRIDE=debian:bookworm exec stdbuf -o L roslaunch clover clover.launch --wait --screen --skip-log-check \
+                      ROS_HOSTNAME=`hostname`.local exec stdbuf -o L roslaunch clover clover.launch --wait --screen --skip-log-check \
                       2> >(tee /tmp/clover.err)"
 
 ExecStartPre=+rm /var/log/clover.log

builder/assets/hardware_setup.sh

@@ -13,38 +13,70 @@
 # copies or substantial portions of the Software.
 #
 
-# https://www.raspberrypi.com/documentation/computers/configuration.html
+set -e # Exit immidiately on non-zero result
+
+echo_stamp() {
+  # TEMPLATE: echo_stamp <TEXT> <TYPE>
+  # TYPE: SUCCESS, ERROR, INFO
+
+  # More info there https://www.shellhacks.com/ru/bash-colors/
+
+  TEXT="$(date '+[%Y-%m-%d %H:%M:%S]') $1"
+  TEXT="\e[1m$TEXT\e[0m" # BOLD
+
+  case "$2" in
+    SUCCESS)
+    TEXT="\e[32m${TEXT}\e[0m";; # GREEN
+    ERROR)
+    TEXT="\e[31m${TEXT}\e[0m";; # RED
+    *)
+    TEXT="\e[34m${TEXT}\e[0m";; # BLUE
+  esac
+  echo -e ${TEXT}
+}
 
 ##################################################
 # Configure hardware interfaces
 ##################################################
 
-echo "--- Enable sshd"
-/usr/bin/raspi-config nonint do_ssh 0
+# 1. Enable sshd
+echo_stamp "#1 Turn on sshd"
+touch /boot/ssh
+# /usr/bin/raspi-config nonint do_ssh 0
 
-echo "--- GPIO enabled by default"
+# 2. Enable GPIO
+echo_stamp "#2 GPIO enabled by default"
 
-echo "--- Enable I2C"
+# 3. Enable I2C
+echo_stamp "#3 Turn on I2C"
 /usr/bin/raspi-config nonint do_i2c 0
 
-echo "--- Enable SPI"
+# 4. Enable SPI
+echo_stamp "#4 Turn on SPI"
 /usr/bin/raspi-config nonint do_spi 0
 
-echo  "--- Enable raspicam"
+# 5. Enable raspicam
+echo_stamp "#5 Turn on raspicam"
 /usr/bin/raspi-config nonint do_camera 0
 
-echo "--- Setup UART"
+# 6. Enable hardware UART
+echo_stamp "#6 Turn on UART"
 # Temporary solution
 # https://github.com/RPi-Distro/raspi-config/pull/75
-/usr/bin/raspi-config nonint do_serial_hw 0
-/usr/bin/raspi-config nonint do_serial_cons 1
-
-echo dtoverlay=pi3-disable-bt >> /boot/firmware/config.txt
+/usr/bin/raspi-config nonint do_serial 1
+/usr/bin/raspi-config nonint set_config_var enable_uart 1 /boot/config.txt
+echo dtoverlay=pi3-disable-bt >> /boot/config.txt
 systemctl disable hciuart.service
 
-echo "--- Enable v4l2 driver"
-# http://robocraft.ru/blog/electronics/3158.html
+# After adding to Raspbian OS
+# https://github.com/RPi-Distro/raspi-config/commit/d6d9ecc0d9cbe4aaa9744ae733b9cb239e79c116
+#/usr/bin/raspi-config nonint do_serial 2
+
+# 7. Enable V4L driver http://robocraft.ru/blog/electronics/3158.html
 #echo "bcm2835-v4l2" >> /etc/modules
+echo_stamp "#7 Turn on v4l2 driver"
 if ! grep -q "^bcm2835-v4l2" /etc/modules;
 then printf "bcm2835-v4l2\n" >> /etc/modules
 fi
+
+echo_stamp "#8 End of configure hardware interfaces"

builder/assets/init_rpi.sh

@@ -13,31 +13,63 @@
 # copies or substantial portions of the Software.
 #
 
-echo "--- Fix home directory permissions"
-chmod +rx /home/pi
+set -e # Exit immidiately on non-zero result
+
+echo_stamp() {
+  # TEMPLATE: echo_stamp <TEXT> <TYPE>
+  # TYPE: SUCCESS, ERROR, INFO
+
+  # More info there https://www.shellhacks.com/ru/bash-colors/
+
+  TEXT="$(date '+[%Y-%m-%d %H:%M:%S]') $1"
+  TEXT="\e[1m$TEXT\e[0m" # BOLD
+
+  case "$2" in
+    SUCCESS)
+    TEXT="\e[32m${TEXT}\e[0m";; # GREEN
+    ERROR)
+    TEXT="\e[31m${TEXT}\e[0m";; # RED
+    *)
+    TEXT="\e[34m${TEXT}\e[0m";; # BLUE
+  esac
+  echo -e ${TEXT}
+}
 
 NEW_SSID='clover-'$(head -c 100 /dev/urandom | xxd -ps -c 100 | sed -e "s/[^0-9]//g" | cut -c 1-4)
-echo "--- Creating Wi-Fi AP with SSID=${NEW_SSID}"
-nmcli con add type wifi ifname wlan0 mode ap con-name clover ssid $NEW_SSID autoconnect true \
-&& nmcli con modify clover 802-11-wireless.band bg \
-&& nmcli con modify clover ipv4.method shared ipv4.address 192.168.11.1/24 \
-&& nmcli con modify clover ipv6.method disabled \
-&& nmcli con modify clover wifi-sec.key-mgmt wpa-psk \
-&& nmcli con modify clover wifi-sec.psk "cloverwifi" \
-&& systemctl disable dnsmasq # disable dnsmasq to avoid conflicts with NetworkManager's dnsmasq
+echo_stamp "Setting SSID to ${NEW_SSID}"
+# TODO: Use wpa_cli insted direct file edit
+# FIXME: We rely on raspberrypi-net-mods to copy our file to /etc/wpa_supplicant.
+# This is not very reliable, but seems to fix our rfkill problem.
+cat << EOF >> /boot/wpa_supplicant.conf
+ctrl_interface=DIR=/var/run/wpa_supplicant GROUP=netdev
+update_config=1
+country=GB
+network={
+    ssid="${NEW_SSID}"
+    psk="cloverwifi"
+    mode=2
+    proto=WPA RSN
+    key_mgmt=WPA-PSK
+    pairwise=CCMP
+    group=CCMP
+    auth_alg=OPEN
+}
+EOF
 
 NEW_HOSTNAME=$(echo ${NEW_SSID} | tr '[:upper:]' '[:lower:]')
-echo "--- Setting hostname to $NEW_HOSTNAME"
-hostnamectl set-hostname $NEW_HOSTNAME \
-&& sed -i 's/127\.0\.1\.1.*/127.0.1.1\t'${NEW_HOSTNAME}' '${NEW_HOSTNAME}'.local/g' /etc/hosts
+echo_stamp "Setting hostname to $NEW_HOSTNAME"
+hostnamectl set-hostname $NEW_HOSTNAME
+sed -i 's/127\.0\.1\.1.*/127.0.1.1\t'${NEW_HOSTNAME}' '${NEW_HOSTNAME}'.local/g' /etc/hosts
 # .local (mdns) hostname added to make it accesable when wlan and ethernet interfaces are down
 
-echo "--- Enable ROS services"
+echo_stamp "Enable ROS services"
 systemctl enable roscore
 systemctl enable clover
 
-echo "--- Harware setup"
+echo_stamp "Harware setup"
 /root/hardware_setup.sh
 
-echo "--- Remove init scripts"
+echo_stamp "Remove init scripts"
 rm /root/init_rpi.sh /root/hardware_setup.sh
+
+echo_stamp "End of initialization of the image"

builder/image-build-ros.sh

@@ -1,122 +0,0 @@
-#! /usr/bin/env bash
-
-#
-# Script for building ROS packages from scratch
-#
-# Copyright (C) 2022 Copter Express Technologies
-#
-# Author: Oleg Kalachev <okalachev@gmail.com>
-#
-# Distributed under MIT License (available at https://opensource.org/licenses/MIT).
-# The above copyright notice and this permission notice shall be included in all
-# copies or substantial portions of the Software.
-#
-
-set -ex # exit on error, echo commands
-
-# http://wiki.ros.org/Installation/Source
-
-export ROS_DISTRO=noetic
-. /etc/os-release # set $VERSION_CODENAME to Debian release code name
-export ROS_OS_OVERRIDE=debian:$VERSION_CODENAME
-export ROS_PYTHON_VERSION=3
-export CLOVER_DEPS="tf tf2 tf2_ros tf2_geometry_msgs geometry_msgs sensor_msgs visualization_msgs libgeographiclib-dev mavros mavros_extras cv_camera cv_bridge rosbridge_server web_video_server tf2_web_republisher libxml2 libxslt python3-lxml dynamic_reconfigure image_transport image_proc image_geometry python-pymavlink ros_pytest"
-export CLOVER_DEPS="$CLOVER_DEPS rostest python3-docopt image_publisher"
-
-echo "=== Building ROS from scratch"
-
-#echo "--- Adding sources"
-# echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list
-# curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | sudo apt-key add -
-
-#cp /etc/apt/trusted.gpg /etc/apt/trusted.gpg.d # https://askubuntu.com/a/1408456
-apt-get update
-apt-get install -y python3-distutils build-essential cmake git python3-pip python3-rosinstall-generator python3-vcstools python3-empy libpoco-dev
-
-# install vcstool using pip
-# curl https://bootstrap.pypa.io/get-pip.py -o get-pip.py && python3 get-pip.py && rm get-pip.py
-pip3 install -U --break-system-packages vcstool rosdep rosinstall-generator catkin-pkg future
-
-# sudo rosdep init
-# rm /etc/ros/rosdep/sources.list.d/20-default.list
-rosdep init
-rosdep update --os=debian:bullseye
-
-# rm /etc/ros/rosdep/sources.list.d/20-default.list && rosdep init
-# rosdep --os=debian:$VERSION_CODENAME update
-
-echo "--- Create Catkin workspace to build ROS package"
-mkdir ~/ros_catkin_ws
-cd ~/ros_catkin_ws
-
-echo "--- Download ROS sources"
-rosinstall_generator ros_base $CLOVER_DEPS --rosdistro $ROS_DISTRO --deps --tar > noetic.rosinstall
-mkdir ./src
-vcs import --input noetic.rosinstall ./src
-
-# https://answers.ros.org/question/343367/catkin-package-dependencies-issue-when-installing-ros-melodic-on-raspberry-pi-4/
-#sudo apt remove python-rospkg
-#sudo apt remove python-catkin-pkg
-##sudo apt --fix-broken install
-#sudo apt-get autoremove
-
-#echo "--- Install catkin_pkg"
-#cd
-#git clone https://github.com/ros-infrastructure/catkin_pkg.git
-#cd catkin_pkg
-#python3 setup.py install
-#cd ~/ros_catkin_ws
-
-echo "--- Resolve dependencies"
-rosdep install --from-paths ./src --ignore-packages-from-source --rosdistro $ROS_DISTRO -y --os=debian:bullseye --skip-keys="python3-catkin-pkg-modules libboost-thread python3-rosdep-modules" || true
-
-echo "--- Install missing dependencies"
-apt-get install -y liborocos-kdl1.5 geographiclib-tools libgeographiclib-dev
-
-echo "-- Install geographiclib datasets"
-wget https://raw.githubusercontent.com/mavlink/mavros/master/mavros/scripts/install_geographiclib_datasets.sh
-chmod +x install_geographiclib_datasets.sh
-./install_geographiclib_datasets.sh
-
-echo "--- Apply patches"
-wget https://github.com/ros/rosconsole/pull/58.patch
-patch -p1 -d src/rosconsole < 58.patch
-
-wget https://github.com/ros/ros_comm/pull/2353.patch
-patch -p2 -d src/ros_comm < 2353.patch
-
-wget https://github.com/AJahueyM/web_video_server/commit/5b722eb0822bcc3fe45fefe7b393b87bfe004417.patch
-patch -p1 -d src/web_video_server < 5b722eb0822bcc3fe45fefe7b393b87bfe004417.patch
-
-echo "--- Build ROS"
-# https://github.com/ros/catkin/issues/863#issuecomment-290392074
-./src/catkin/bin/catkin_make_isolated --install -DCMAKE_BUILD_TYPE=Release
-#	-DSETUPTOOLS_DEB_LAYOUT=OFF \
-#	--install-space=/opt/ros/$ROS_DISTRO
-
-# source ~/ros_catkin_ws/install_isolated/setup.bash
-#source /opt/ros/$ROS_DISTRO/setup.bash
-#
-#echo "--- List built ROS packages"
-#set +x
-#rospack list-names | while read line; do echo $line `rosversion $line`; done
-#set -x
-#
-#echo "--- Build Debian packages"
-#apt-get install -y python3-bloom debhelper dpkg-dev libtinyxml-dev
-#
-## add rosdep file to help bloom-generate resolve missing bookworm dependencies
-#echo "yaml file:///etc/ros/rosdep/noetic-rosdep-clover.yaml" >> /etc/ros/rosdep/sources.list.d/20-default.list
-#rosdep update
-#
-#pip3 install setuptools==45.2.0 # https://github.com/ros/catkin/issues/863#issuecomment-1000446018
-#
-#for file in `find . -name "package.xml" -not -path "*/debian/*"`; do
-#	cd $(dirname ${file})
-#	rm -rf debian
-#	bloom-generate rosdebian --os-name debian --os-version $VERSION_CODENAME --ros-distro $ROS_DISTRO --debug
-#	debian/rules binary # fakeroot is not needed as we are root
-#	cd -
-#done
-
-ls

builder/image-build.sh

@@ -13,22 +13,42 @@
 # copies or substantial portions of the Software.
 #
 
-set -ex # exit on error, echo commands
+set -e # Exit immidiately on non-zero result
 
 # https://www.raspberrypi.org/software/operating-systems/#raspberry-pi-os-32-bit
-SOURCE_IMAGE="https://downloads.raspberrypi.com/raspios_lite_armhf/images/raspios_lite_armhf-2024-03-15/2024-03-15-raspios-bookworm-armhf-lite.img.xz"
+SOURCE_IMAGE="https://downloads.raspberrypi.org/raspios_lite_armhf/images/raspios_lite_armhf-2021-05-28/2021-05-07-raspios-buster-armhf-lite.zip"
 
 export DEBIAN_FRONTEND=${DEBIAN_FRONTEND:='noninteractive'}
 export LANG=${LANG:='C.UTF-8'}
 export LC_ALL=${LC_ALL:='C.UTF-8'}
 
+echo_stamp() {
+  # TEMPLATE: echo_stamp <TEXT> <TYPE>
+  # TYPE: SUCCESS, ERROR, INFO
+
+  # More info there https://www.shellhacks.com/ru/bash-colors/
+
+  TEXT="$(date '+[%Y-%m-%d %H:%M:%S]') $1"
+  TEXT="\e[1m$TEXT\e[0m" # BOLD
+
+  case "$2" in
+    SUCCESS)
+    TEXT="\e[32m${TEXT}\e[0m";; # GREEN
+    ERROR)
+    TEXT="\e[31m${TEXT}\e[0m";; # RED
+    *)
+    TEXT="\e[34m${TEXT}\e[0m";; # BLUE
+  esac
+  echo -e ${TEXT}
+}
+
 BUILDER_DIR="/builder"
 REPO_DIR="${BUILDER_DIR}/repo"
 SCRIPTS_DIR="${REPO_DIR}/builder"
 IMAGES_DIR="${REPO_DIR}/images"
 
-[[ ! -d ${SCRIPTS_DIR} ]] && (echo "Error: directory ${SCRIPTS_DIR} doesn't exist"; exit 1)
-[[ ! -d ${IMAGES_DIR} ]] && mkdir ${IMAGES_DIR} && echo "Directory ${IMAGES_DIR} was created successful"
+[[ ! -d ${SCRIPTS_DIR} ]] && (echo_stamp "Directory ${SCRIPTS_DIR} doesn't exist" "ERROR"; exit 1)
+[[ ! -d ${IMAGES_DIR} ]] && mkdir ${IMAGES_DIR} && echo_stamp "Directory ${IMAGES_DIR} was created successful" "SUCCESS"
 
 if [[ -z ${TRAVIS_TAG} ]]; then IMAGE_VERSION="$(cd ${REPO_DIR}; git log --format=%h -1)"; else IMAGE_VERSION="${TRAVIS_TAG}"; fi
 # IMAGE_VERSION="${TRAVIS_TAG:=$(cd ${REPO_DIR}; git log --format=%h -1)}"
@@ -44,17 +64,15 @@ get_image() {
   local RPI_IMAGE_NAME=$(echo ${RPI_ZIP_NAME} | sed 's/zip/img/')
 
   if [ ! -e "${BUILD_DIR}/${RPI_ZIP_NAME}" ]; then
-    echo "--- Downloading original Linux distribution"
+    echo_stamp "Downloading original Linux distribution"
     wget --progress=dot:giga -O ${BUILD_DIR}/${RPI_ZIP_NAME} $2
-    echo "--- Downloading complete" "SUCCESS"
-  else
-    echo "Linux distribution already downloaded"
-  fi
+    echo_stamp "Downloading complete" "SUCCESS" \
+  else echo_stamp "Linux distribution already donwloaded"; fi
 
-  echo "--- Unzipping Linux distribution image"
-  apt-get update --allow-releaseinfo-change
-  apt-get install -y xz-utils
-  unxz --stdout ${BUILD_DIR}/${RPI_ZIP_NAME} > $1
+  echo_stamp "Unzipping Linux distribution image" \
+  && unzip -p ${BUILD_DIR}/${RPI_ZIP_NAME} ${RPI_IMAGE_NAME} > $1 \
+  && echo_stamp "Unzipping complete" "SUCCESS" \
+  || (echo_stamp "Unzipping was failed!" "ERROR"; exit 1)
 }
 
 get_image ${IMAGE_PATH} ${SOURCE_IMAGE}
@@ -102,7 +120,6 @@ ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/launch.
 # ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/kinetic-ros-clover.rosinstall' '/home/pi/ros_catkin_ws/'
 # Add rename script
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} exec ${SCRIPTS_DIR}'/image-ros.sh' ${REPO_URL} ${IMAGE_VERSION} false false ${NUMBER_THREADS}
-#${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} exec ${SCRIPTS_DIR}'/image-build-ros.sh'
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} exec ${SCRIPTS_DIR}'/image-validate.sh'
 
 ${BUILDER_DIR}/image-resize.sh ${IMAGE_PATH}

builder/image-init.sh

@@ -12,33 +12,50 @@
 # copies or substantial portions of the Software.
 #
 
-set -ex # exit on error, echo commands
+set -e # Exit immidiately on non-zero result
 
-echo "--- Move /etc/ld.so.preload out of the way"
-mv /etc/ld.so.preload /etc/ld.so.preload.disabled-for-build
+echo_stamp() {
+  # TEMPLATE: echo_stamp <TEXT> <TYPE>
+  # TYPE: SUCCESS, ERROR, INFO
 
-echo "--- Create pi user"
-echo 'pi:$6$c70VpvPsVNCG0YR5$l5vWWLsLko9Kj65gcQ8qvMkuOoRkEagI90qi3F/Y7rm8eNYZHW8CY6BOIKwMH7a3YYzZYL90zf304cAHLFaZE0' > /boot/userconf.txt
+  # More info there https://www.shellhacks.com/ru/bash-colors/
+
+  TEXT="$(date '+[%Y-%m-%d %H:%M:%S]') $1"
+  TEXT="\e[1m$TEXT\e[0m" # BOLD
+
+  case "$2" in
+    SUCCESS)
+    TEXT="\e[32m${TEXT}\e[0m";; # GREEN
+    ERROR)
+    TEXT="\e[31m${TEXT}\e[0m";; # RED
+    *)
+    TEXT="\e[34m${TEXT}\e[0m";; # BLUE
+  esac
+  echo -e ${TEXT}
+}
+
+echo_stamp "Write Clover information"
 
-echo "--- Write Clover information"
 # Clover image version
 echo "$1" >> /etc/clover_version
 # Origin image file name
 echo "${2%.*}" >> /etc/clover_origin
 
-echo "--- Write magic script to /etc/rc.local"
+echo_stamp "Write magic script to /etc/rc.local"
 MAGIC_SCRIPT="sudo /root/init_rpi.sh; sudo sed -i '/sudo \\\/root\\\/init_rpi.sh/d' /etc/rc.local && sudo reboot"
 sed -i "19a${MAGIC_SCRIPT}" /etc/rc.local
-# TODO: remake to oneshot systemd service
 
 # It needs for autosizer.sh & maybe that is correct
-echo "--- Change boot partition"
+echo_stamp "Change boot partition"
 sed -i 's/root=[^ ]*/root=\/dev\/mmcblk0p2/' /boot/cmdline.txt
-sed -i 's/.*  \/boot\/firmware  vfat    defaults          0       2$/\/dev\/mmcblk0p1  \/boot\/firmware  vfat  defaults          0       2/' /etc/fstab
+sed -i 's/.*  \/boot           vfat    defaults          0       2$/\/dev\/mmcblk0p1  \/boot           vfat    defaults          0       2/' /etc/fstab
 sed -i 's/.*  \/               ext4    defaults,noatime  0       1$/\/dev\/mmcblk0p2  \/               ext4    defaults,noatime  0       1/' /etc/fstab
-cat /boot/cmdline.txt
-cat /etc/fstab
 
-echo "--- Set max space for syslogs"
+echo_stamp "Set max space for syslogs"
 # https://unix.stackexchange.com/questions/139513/how-to-clear-journalctl
 sed -i 's/#SystemMaxUse=/SystemMaxUse=200M/' /etc/systemd/journald.conf
+
+echo_stamp "Move /etc/ld.so.preload out of the way"
+mv /etc/ld.so.preload /etc/ld.so.preload.disabled-for-build
+
+echo_stamp "End of init image"

builder/image-network.sh

@@ -12,20 +12,43 @@
 # copies or substantial portions of the Software.
 #
 
-set -ex # exit on error, echo commands
+set -e # Exit immidiately on non-zero result
+
+echo_stamp() {
+  # TEMPLATE: echo_stamp <TEXT> <TYPE>
+  # TYPE: SUCCESS, ERROR, INFO
+
+  # More info there https://www.shellhacks.com/ru/bash-colors/
+
+  TEXT="$(date '+[%Y-%m-%d %H:%M:%S]') $1"
+  TEXT="\e[1m$TEXT\e[0m" # BOLD
+
+  case "$2" in
+    SUCCESS)
+    TEXT="\e[32m${TEXT}\e[0m";; # GREEN
+    ERROR)
+    TEXT="\e[31m${TEXT}\e[0m";; # RED
+    *)
+    TEXT="\e[34m${TEXT}\e[0m";; # BLUE
+  esac
+  echo -e ${TEXT}
+}
+
+echo_stamp "#1 Write STATIC to /etc/dhcpcd.conf"
 
-echo "--- Write static to /etc/dhcpcd.conf"
 cat << EOF >> /etc/dhcpcd.conf
 interface wlan0
 static ip_address=192.168.11.1/24
 EOF
 
-echo "--- Set wpa_supplicant country"
+echo_stamp "#2 Set wpa_supplicant country"
+
 cat << EOF >> /etc/wpa_supplicant/wpa_supplicant.conf
 country=GB
 EOF
 
-echo "--- Write dhcp-config to /etc/dnsmasq.conf"
+echo_stamp "#3 Write dhcp-config to /etc/dnsmasq.conf"
+
 cat << EOF >> /etc/dnsmasq.conf
 interface=wlan0
 address=/clover/coex/192.168.11.1
@@ -36,3 +59,5 @@ bogus-priv
 domain-needed
 quiet-dhcp6
 EOF
+
+echo_stamp "#4 End of network installation"

builder/image-ros.sh

@@ -15,20 +15,44 @@
 
 set -ex # exit on error, echo commands
 
+REPO=$1
+REF=$2
+INSTALL_ROS_PACK_SOURCES=$3
+DISCOVER_ROS_PACK=$4
+NUMBER_THREADS=$5
+
 # Current ROS distribution
 ROS_DISTRO=noetic
-. /etc/os-release # set $VERSION_CODENAME to Debian release code name
-export ROS_OS_OVERRIDE=debian:$VERSION_CODENAME
+
+echo_stamp() {
+  # TEMPLATE: echo_stamp <TEXT> <TYPE>
+  # TYPE: SUCCESS, ERROR, INFO
+
+  # More info there https://www.shellhacks.com/ru/bash-colors/
+
+  TEXT="$(date '+[%Y-%m-%d %H:%M:%S]') $1"
+  TEXT="\e[1m$TEXT\e[0m" # BOLD
+
+  case "$2" in
+    SUCCESS)
+    TEXT="\e[32m${TEXT}\e[0m";; # GREEN
+    ERROR)
+    TEXT="\e[31m${TEXT}\e[0m";; # RED
+    *)
+    TEXT="\e[34m${TEXT}\e[0m";; # BLUE
+  esac
+  echo -e ${TEXT}
+}
 
 # https://gist.github.com/letmaik/caa0f6cc4375cbfcc1ff26bd4530c2a3
 # https://github.com/travis-ci/travis-build/blob/master/lib/travis/build/templates/header.sh
 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
+      echo -e "\nThe command \"$@\" failed. Retrying, $count of $max_count.\n" >&2
     }
     # ! { } ignores set -e, see https://stackoverflow.com/a/4073372
     ! { "$@"; result=$?; }
@@ -38,136 +62,124 @@ my_travis_retry() {
   done
 
   [ $count -gt $max_count ] && {
-    echo -e "\nThe command \"$*\" failed $max_count times.\n" >&2
+    echo -e "\nThe command \"$@\" failed $max_count times.\n" >&2
   }
 
   return $result
 }
 
-echo "--- Install rosdep"
-my_travis_retry pip3 install -U rosdep
-
 # TODO: 'kinetic-rosdep-clover.yaml' should add only if we use our repo?
-echo "--- Init rosdep"
+echo_stamp "Init rosdep"
 my_travis_retry rosdep init
-
-echo "--- Update rosdep"
-echo "yaml file:///etc/ros/rosdep/${ROS_DISTRO}-rosdep-clover.yaml" >> /etc/ros/rosdep/sources.list.d/10-clover.list
+# 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/20-default.list
 my_travis_retry rosdep update
 
-echo "--- Populate rosdep for ROS user"
-my_travis_retry sudo -u pi ROS_OS_OVERRIDE=debian:$VERSION_CODENAME rosdep update
+echo_stamp "Populate rosdep for ROS user"
+my_travis_retry sudo -u pi rosdep update
 
-# echo "Reconfiguring Clover repository for simplier unshallowing"
+export ROS_IP='127.0.0.1' # needed for running tests
+
+# echo_stamp "Reconfiguring Clover repository for simplier unshallowing"
 cd /home/pi/catkin_ws/src/clover
 git config remote.origin.fetch "+refs/heads/*:refs/remotes/origin/*"
 
 # This is sort of a hack to force "custom" packages to be installed - the ones built by COEX, linked against OpenCV 4.2
 # I **wish** OpenCV would not be such a mess, but, well, here we are.
-# echo "--- Installing OpenCV 4.2-compatible ROS packages"
-# apt install -y --no-install-recommends \
-# ros-${ROS_DISTRO}-compressed-image-transport=1.14.0-0buster \
-# ros-${ROS_DISTRO}-cv-bridge=1.15.0-0buster \
-# ros-${ROS_DISTRO}-cv-camera=0.5.1-0buster \
-# ros-${ROS_DISTRO}-image-publisher=1.15.3-0buster \
-# ros-${ROS_DISTRO}-web-video-server=0.2.1-0buster
-# apt-mark hold \
-# ros-${ROS_DISTRO}-compressed-image-transport \
-# ros-${ROS_DISTRO}-cv-bridge \
-# ros-${ROS_DISTRO}-cv-camera \
-# ros-${ROS_DISTRO}-image-publisher \
-# ros-${ROS_DISTRO}-web-video-server
+echo_stamp "Installing OpenCV 4.2-compatible ROS packages"
+apt install -y --no-install-recommends \
+ros-${ROS_DISTRO}-compressed-image-transport=1.14.0-0buster \
+ros-${ROS_DISTRO}-cv-bridge=1.15.0-0buster \
+ros-${ROS_DISTRO}-cv-camera=0.5.1-0buster \
+ros-${ROS_DISTRO}-image-publisher=1.15.3-0buster \
+ros-${ROS_DISTRO}-web-video-server=0.2.1-0buster
+apt-mark hold \
+ros-${ROS_DISTRO}-compressed-image-transport \
+ros-${ROS_DISTRO}-cv-bridge \
+ros-${ROS_DISTRO}-cv-camera \
+ros-${ROS_DISTRO}-image-publisher \
+ros-${ROS_DISTRO}-web-video-server
 
-#echo "--- Installing libboost-dev" # https://travis-ci.org/github/CopterExpress/clover/jobs/766318908#L6536
-#my_travis_retry apt-get install -y --no-install-recommends libboost-dev libboost-all-dev
+echo_stamp "Installing libboost-dev" # https://travis-ci.org/github/CopterExpress/clover/jobs/766318908#L6536
+my_travis_retry apt-get install -y --no-install-recommends libboost-dev libboost-all-dev
 
-echo "--- Build and install Clover"
+echo_stamp "Build and install Clover"
 cd /home/pi/catkin_ws
 # Don't try to install gazebo_ros
-my_travis_retry rosdep install -y --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} --os=debian:$VERSION_CODENAME \
-  --skip-keys="gazebo_ros gazebo_plugins"
+my_travis_retry rosdep install -y --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} --os=debian:buster \
+  --skip-keys=gazebo_ros --skip-keys=gazebo_plugins
 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
-catkin_make -j2 -DCMAKE_BUILD_TYPE=RelWithDebInfo
+# Don't build simulation plugins for actual drone
+sudo -E -u pi sh -c '. /opt/ros/${ROS_DISTRO}/setup.sh && catkin_make -j2 -DCMAKE_BUILD_TYPE=RelWithDebInfo'
 source devel/setup.bash
 
-echo "--- Install clever package (for backwards compatibility)"
+echo_stamp "Install clever package (for backwards compatibility)"
 cd /home/pi/catkin_ws/src/clover/builder/assets/clever
 ./setup.py install
 rm -rf build  # remove build artifacts
 
-echo "--- Build Clover documentation"
+echo_stamp "Build Clover documentation"
 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
-npm cache clean --force
 
-echo "--- Installing additional ROS packages"
+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 \
     ros-${ROS_DISTRO}-vl53l1x \
     ros-${ROS_DISTRO}-ws281x \
-    ros-${ROS_DISTRO}-libcamera-ros \
     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}-image-view
 
 # TODO move GeographicLib datasets to Mavros debian package
-echo "--- Install GeographicLib datasets (needed for mavros)" \
+echo_stamp "Install GeographicLib datasets (needed for mavros)" \
 && wget -qO- https://raw.githubusercontent.com/mavlink/mavros/master/mavros/scripts/install_geographiclib_datasets.sh | bash
 
-echo "--- Running tests"
-export ROS_IP='127.0.0.1' # needed for running tests
+echo_stamp "Running tests"
 cd /home/pi/catkin_ws
 # FIXME: Investigate failing tests
 catkin_make run_tests #&& catkin_test_results
 
-echo "--- Change permissions for catkin_ws"
+echo_stamp "Change permissions for catkin_ws"
 chown -Rf pi:pi /home/pi/catkin_ws
 
-echo "--- Update www"
-sudo -u pi sh -c ". devel/setup.sh && rosrun clover www"
-
-echo "--- Make \$HOME/examples symlink"
+echo_stamp "Make \$HOME/examples symlink"
 ln -s "$(catkin_find clover examples --first-only)" /home/pi
 chown -Rf pi:pi /home/pi/examples
 
-echo "--- Make systemd services symlinks"
+echo_stamp "Make systemd services symlinks"
 ln -s /home/pi/catkin_ws/src/clover/builder/assets/clover.service /lib/systemd/system/
 ln -s /home/pi/catkin_ws/src/clover/builder/assets/roscore.service /lib/systemd/system/
 # validate
 [ -f /lib/systemd/system/clover.service ]
 [ -f /lib/systemd/system/roscore.service ]
 
-echo "--- Make udev rules symlink"
+echo_stamp "Make udev rules symlink"
 ln -s "$(catkin_find clover udev --first-only)"/* /lib/udev/rules.d/
 
-echo "--- Setup ROS environment"
+echo_stamp "Setup ROS environment"
 cat << EOF >> /home/pi/.bashrc
 LANG='C.UTF-8'
 LC_ALL='C.UTF-8'
 export ROS_HOSTNAME=\`hostname\`.local
-export ROS_OS_OVERRIDE=debian:bookworm
 source /opt/ros/${ROS_DISTRO}/setup.bash
 source /home/pi/catkin_ws/devel/setup.bash
 EOF
 
-echo "--- Cleanup apt"
-apt-get autoremove --purge -y
-apt-get clean
+#echo_stamp "Removing local apt mirror"
+# Restore original sources.list
+#mv /var/sources.list.bak /etc/apt/sources.list
+# Clean apt cache
+apt-get clean -qq > /dev/null
+# Remove local mirror repository key
+#apt-key del COEX-MIRROR
 
-echo "--- Cleanup pip"
-pip3 cache purge
-
-echo "--- Cleanup /tmp"
-rm -rf /tmp/*
+echo_stamp "END of ROS INSTALLATION"

builder/image-software.sh

@@ -12,9 +12,27 @@
 # copies or substantial portions of the Software.
 #
 
-set -ex # exit on error, echo commands
+set -e # Exit immidiately on non-zero result
 
-. /etc/os-release # set $VERSION_CODENAME to Debian release code name
+echo_stamp() {
+  # TEMPLATE: echo_stamp <TEXT> <TYPE>
+  # TYPE: SUCCESS, ERROR, INFO
+
+  # More info there https://www.shellhacks.com/ru/bash-colors/
+
+  TEXT="$(date '+[%Y-%m-%d %H:%M:%S]') $1"
+  TEXT="\e[1m${TEXT}\e[0m" # BOLD
+
+  case "$2" in
+    SUCCESS)
+    TEXT="\e[32m${TEXT}\e[0m";; # GREEN
+    ERROR)
+    TEXT="\e[31m${TEXT}\e[0m";; # RED
+    *)
+    TEXT="\e[34m${TEXT}\e[0m";; # BLUE
+  esac
+  echo -e ${TEXT}
+}
 
 # https://gist.github.com/letmaik/caa0f6cc4375cbfcc1ff26bd4530c2a3
 # https://github.com/travis-ci/travis-build/blob/master/lib/travis/build/templates/header.sh
@@ -23,7 +41,7 @@ my_travis_retry() {
   local count=1
   while [ $count -le 3 ]; do
     [ $result -ne 0 ] && {
-      echo -e "\n${ANSI_RED}The command \"$*\" failed. Retrying, $count of 3.${ANSI_RESET}\n" >&2
+      echo -e "\n${ANSI_RED}The command \"$@\" failed. Retrying, $count of 3.${ANSI_RESET}\n" >&2
     }
     # ! { } ignores set -e, see https://stackoverflow.com/a/4073372
     ! { "$@"; result=$?; }
@@ -33,39 +51,41 @@ my_travis_retry() {
   done
 
   [ $count -gt 3 ] && {
-    echo -e "\n${ANSI_RED}The command \"$*\" failed 3 times.${ANSI_RESET}\n" >&2
+    echo -e "\n${ANSI_RED}The command \"$@\" failed 3 times.${ANSI_RESET}\n" >&2
   }
 
   return $result
 }
 
-echo "--- Increase apt retries"
+echo_stamp "Increase apt retries"
+
 echo "APT::Acquire::Retries \"3\";" > /etc/apt/apt.conf.d/80-retries
 
-echo "--- Install apt keys & repos"
+echo_stamp "Install apt keys & repos"
 
 # TODO: This STDOUT consist 'OK'
 apt-get update \
 && apt-get install --no-install-recommends -y dirmngr > /dev/null \
 && apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654
 
-# echo "deb http://packages.ros.org/ros/ubuntu buster main" > /etc/apt/sources.list.d/ros-latest.list
+echo "deb http://packages.ros.org/ros/ubuntu buster main" > /etc/apt/sources.list.d/ros-latest.list
 
-wget -O - 'http://packages.coex.tech/key.asc' | apt-key add -
-echo "deb http://packages.coex.tech $VERSION_CODENAME main" >> /etc/apt/sources.list
+wget -O - 'http://packages.coex.tech/key.asc' | apt-key add - 
+echo 'deb http://packages.coex.tech buster main' >> /etc/apt/sources.list
 
-echo "--- Update apt cache"
+echo_stamp "Update apt cache"
 
 # TODO: FIX ERROR: /usr/bin/apt-key: 596: /usr/bin/apt-key: cannot create /dev/null: Permission denied
 apt-get update
 # && apt upgrade -y
 
 # Let's retry fetching those packages several times, just in case
-echo "--- Install software"
-my_travis_retry apt-get install --no-install-recommends -y cmake-data cmake
+echo_stamp "Software installing"
+my_travis_retry apt-get install --no-install-recommends -y cmake-data=3.13.4-1 cmake=3.13.4-1 # FIXME: using older CMake due to https://travis-ci.org/github/CopterExpress/clover/jobs/764367665#L6984
 my_travis_retry apt-get install --no-install-recommends -y \
 unzip \
 zip \
+ipython \
 ipython3 \
 screen \
 byobu  \
@@ -76,57 +96,64 @@ dnsmasq  \
 tmux \
 tree \
 vim \
+libjpeg8 \
 tcpdump \
 libpoco-dev \
 libzbar0 \
+python3-rosdep \
 python3-rosinstall-generator \
 python3-wstool \
 python3-rosinstall \
 build-essential \
 libffi-dev \
 monkey \
-pigpio python3-pigpio \
+pigpio python-pigpio python3-pigpio \
 i2c-tools \
-espeak espeak-data python3-espeak \
+espeak espeak-data python-espeak python3-espeak \
 ntpdate \
-mjpg-streamer \
-xxd \
+python-dev \
 python3-dev \
-python3-systemd \
-python3-opencv \
-python3-pip
-#libjpeg8 \
+python-systemd \
+mjpg-streamer \
+python3-opencv
 
 # Deny byobu to check available updates
 sed -i "s/updates_available//" /usr/share/byobu/status/status
 # sed -i "s/updates_available//" /home/pi/.byobu/status
 
-echo "--- Make sure pip is installed"
+echo_stamp "Installing pip"
+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
+rm get-pip.py get-pip2.py
+#my_travis_retry pip install --upgrade pip
+#my_travis_retry pip3 install --upgrade pip
+
+echo_stamp "Make sure both pip and pip3 are installed"
 pip --version
 pip3 --version
 
-echo "--- Enable installing packages with pip"
-mv /usr/lib/python3.11/EXTERNALLY-MANAGED /usr/lib/python3.11/EXTERNALLY-MANAGED.old
-
-echo "--- Install and enable Butterfly (web terminal)"
+echo_stamp "Install and enable Butterfly (web terminal)"
+echo_stamp "Workaround for tornado >= 6.0 breaking butterfly"
 export CRYPTOGRAPHY_DONT_BUILD_RUST=1
 my_travis_retry pip3 install cryptography==3.4.6 # https://stackoverflow.com/a/68472128/6850197
-#my_travis_retry pip3 install pyOpenSSL==20.0.1
-#my_travis_retry pip3 install tornado==5.1.1
+my_travis_retry pip3 install pyOpenSSL==20.0.1
+my_travis_retry pip3 install tornado==5.1.1
 my_travis_retry pip3 install butterfly
 my_travis_retry pip3 install butterfly[systemd]
 systemctl enable butterfly.socket
 
-echo "--- Install ws281x library"
+echo_stamp "Install ws281x library"
 my_travis_retry pip3 install --prefer-binary rpi_ws281x
 
-echo "--- Setup Monkey"
+echo_stamp "Setup Monkey"
 mv /etc/monkey/sites/default /etc/monkey/sites/default.orig
 mv /root/monkey /etc/monkey/sites/default
 sed -i 's/SymLink Off/SymLink On/' /etc/monkey/monkey.conf
 systemctl enable monkey.service
 
-echo "--- Install Node.js"
+echo_stamp "Install Node.js"
 cd /home/pi
 wget --no-verbose https://nodejs.org/dist/v10.15.0/node-v10.15.0-linux-armv6l.tar.gz
 tar -xzf node-v10.15.0-linux-armv6l.tar.gz
@@ -134,24 +161,28 @@ cp -R node-v10.15.0-linux-armv6l/* /usr/local/
 rm -rf node-v10.15.0-linux-armv6l/
 rm node-v10.15.0-linux-armv6l.tar.gz
 
-echo "--- Installing debugpy"
-my_travis_retry pip3 install debugpy
+echo_stamp "Installing ptvsd"
+my_travis_retry pip install ptvsd
+my_travis_retry pip3 install ptvsd
 
-echo "--- Installing pyzbar"
+echo_stamp "Installing pyzbar"
+my_travis_retry pip install pyzbar
 my_travis_retry pip3 install pyzbar
 
-echo "--- Add .vimrc"
+echo_stamp "Add .vimrc"
 cat << EOF > /home/pi/.vimrc
 set mouse-=a
 syntax on
 autocmd BufNewFile,BufRead *.launch set syntax=xml
 EOF
 
-echo "--- Change default keyboard layout to US"
+echo_stamp "Change default keyboard layout to US"
 sed -i 's/XKBLAYOUT="gb"/XKBLAYOUT="us"/g' /etc/default/keyboard
 
-echo "--- Attempting to kill dirmngr"
+echo_stamp "Attempting to kill dirmngr"
 gpgconf --kill dirmngr
 # dirmngr is only used by apt-key, so we can safely kill it.
 # We ignore pkill's exit value as well.
 pkill -9 -f dirmngr || true
+
+echo_stamp "End of software installation"

builder/image-validate.sh

@@ -12,33 +12,28 @@
 # copies or substantial portions of the Software.
 #
 
-set -ex # exit on error, echo commands
+set -ex
 
-
-echo "--- Run image tests"
+echo "Run image tests"
 
 export ROS_DISTRO='noetic'
 export ROS_IP='127.0.0.1'
 source /opt/ros/${ROS_DISTRO}/setup.bash
 source /home/pi/catkin_ws/devel/setup.bash
+systemctl start roscore
 
 cd /home/pi/catkin_ws/src/clover/builder/test/
 ./tests.sh
 ./tests.py
+./tests_py3.py
 [[ $(./test_qr.py) == "Found QRCODE with data Проверка Unicode with center at x=66.0, y=66.0" ]] 
 [[ $(./tests_clever.py) == "Warning: clever package is renamed to clover" ]]  # test backwards compatibility
 
+systemctl stop roscore
+
 # check documented packages available
 apt-cache show gst-rtsp-launch
 apt-cache show openvpn
 
-echo "Largest packages installed"
-sudo -E sh -c 'apt-get install -y debian-goodies'
-dpigs -H -n 100
-
-echo "Cleanup apt"
-apt-get autoremove --purge -y
-apt-get clean
-
 echo "Move /etc/ld.so.preload back to its original position"
 mv /etc/ld.so.preload.disabled-for-build /etc/ld.so.preload

builder/test/tests.py

@@ -23,10 +23,6 @@ from clover.srv import GetTelemetry, Navigate, NavigateGlobal, SetPosition, SetV
 from led_msgs.srv import SetLEDs
 from led_msgs.msg import LEDStateArray, LEDState
 from aruco_pose.msg import Marker, MarkerArray, Point2D
-from vision_msgs.msg import BoundingBox2D, BoundingBox2DArray, BoundingBox3D, BoundingBox3DArray, \
-    Classification2D, Classification3D, Detection2D, Detection2DArray, Detection3D, Detection3DArray, \
-    ObjectHypothesis, ObjectHypothesisWithPose, VisionInfo
-
 from clover import long_callback
 
 import dynamic_reconfigure.client
@@ -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

@@ -1,11 +1,12 @@
 #!/usr/bin/env bash
 
-set -ex # exit on error, echo commands
+set -ex
 
 # TODO: validate versions
 
 # validate required software is installed
 
+python2 --version
 python3 --version
 ipython3 --version
 
@@ -21,28 +22,30 @@ pip --version
 pip3 --version
 tcpdump --version
 monkey --version
-espeak --version
-xxd --version
+# espeak --version
 systemctl --version
 
 if [ -z $VM ]; then
 	# rpi only software
 	python --version
-	[[ $(python -c 'import sys;print(sys.version_info.major)') == "3" ]]
+	ipython --version
+	pip2 --version
+	# `python` is python2 for now
+	[[ $(python -c 'import sys;print(sys.version_info.major)') == "2" ]]
 
-	python -m debugpy --version
-	python3 -m debugpy --version
+	# ptvsd does not have a stand-alone binary
+	python -m ptvsd --version
+	python3 -m ptvsd --version
 
 	pigpiod -v
 	i2cdetect -V
-	/usr/local/bin/butterfly.server.py --help
+	butterfly -h
 	mjpg_streamer --version
 fi
 
 # ros stuff
 
 roscore -h
-catkin_find
 rosversion clover
 rosversion aruco_pose
 rosversion mavros
@@ -58,31 +61,18 @@ rosversion web_video_server
 rosversion nodelet
 rosversion image_view
 
-[[ $(rosversion ws281x) == "0.0.15" ]]
+[[ $(rosversion ws281x) == "0.0.13" ]]
 
 if [ -z $VM ]; then
-	# rosversion compressed_image_transport
+	rosversion compressed_image_transport
 	rosversion rosshow
 	rosversion vl53l1x
 	rosversion rosserial
-	[[ $(rosversion cv_camera) == "0.6.1" ]] # patched version with init fix
+	[[ $(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/*) ]]
 

builder/test/tests_py3.py

@@ -0,0 +1,8 @@
+#!/usr/bin/env python3
+
+# Make sure our Python 3 software is installed
+
+import cv2
+from pyzbar import pyzbar
+
+print(cv2.getBuildInformation())

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/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/camera_info/fisheye_cam_1280x720.yaml

@@ -1,65 +0,0 @@
-# Generated from fisheye_cam.yaml by rescale_camera_info.py
-camera_matrix:
-  cols: 3
-  data:
-  - 415.5985593268293
-  - 0.0
-  - 400.0
-  - 0.0
-  - 312.35267324512984
-  - 225.0
-  - 0.0
-  - 0.0
-  - 1.0
-  rows: 3
-camera_name: main_camera_optical
-distortion_coefficients:
-  cols: 8
-  data:
-  - 0.215356885
-  - -0.117472846
-  - -0.000306197672
-  - -0.000109444025
-  - -0.00453657258
-  - 0.573090623
-  - -0.127574577
-  - -0.0286125589
-  - 0.0
-  - 0.0
-  - 0.0
-  - 0.0
-  - 0.0
-  - 0.0
-  rows: 1
-distortion_model: plumb_bob
-image_height: 720
-image_width: 1280
-projection_matrix:
-  cols: 4
-  data:
-  - 415.5985593268293
-  - 0.0
-  - 405.4752811707317
-  - 0.0
-  - 0.0
-  - 312.35267324512984
-  - 205.91677004464282
-  - 0.0
-  - 0.0
-  - 0.0
-  - 1.0
-  - 0.0
-  rows: 3
-rectification_matrix:
-  cols: 3
-  data:
-  - 1
-  - 0
-  - 0
-  - 0
-  - 1
-  - 0
-  - 0
-  - 0
-  - 1
-  rows: 3

clover/camera_info/fisheye_cam_1920x1080.yaml

@@ -1,65 +0,0 @@
-# Generated from fisheye_cam.yaml by rescale_camera_info.py
-camera_matrix:
-  cols: 3
-  data:
-  - 1246.7956779804879
-  - 0.0
-  - 1200.0
-  - 0.0
-  - 702.7935148015422
-  - 506.25
-  - 0.0
-  - 0.0
-  - 1.0
-  rows: 3
-camera_name: main_camera_optical
-distortion_coefficients:
-  cols: 8
-  data:
-  - 0.215356885
-  - -0.117472846
-  - -0.000306197672
-  - -0.000109444025
-  - -0.00453657258
-  - 0.573090623
-  - -0.127574577
-  - -0.0286125589
-  - 0.0
-  - 0.0
-  - 0.0
-  - 0.0
-  - 0.0
-  - 0.0
-  rows: 1
-distortion_model: plumb_bob
-image_height: 1080
-image_width: 1920
-projection_matrix:
-  cols: 4
-  data:
-  - 1246.7956779804879
-  - 0.0
-  - 1216.4258435121951
-  - 0.0
-  - 0.0
-  - 702.7935148015422
-  - 463.31273260044634
-  - 0.0
-  - 0.0
-  - 0.0
-  - 1.0
-  - 0.0
-  rows: 3
-rectification_matrix:
-  cols: 3
-  data:
-  - 1
-  - 0
-  - 0
-  - 0
-  - 1
-  - 0
-  - 0
-  - 0
-  - 1
-  rows: 3

clover/camera_info/fisheye_cam_2592x1944.yaml

@@ -1,65 +0,0 @@
-# Generated from fisheye_cam.yaml by rescale_camera_info.py
-camera_matrix:
-  cols: 3
-  data:
-  - 5049.522495820976
-  - 0.0
-  - 4860.0
-  - 0.0
-  - 2846.313734946246
-  - 2050.3125
-  - 0.0
-  - 0.0
-  - 1.0
-  rows: 3
-camera_name: main_camera_optical
-distortion_coefficients:
-  cols: 8
-  data:
-  - 0.215356885
-  - -0.117472846
-  - -0.000306197672
-  - -0.000109444025
-  - -0.00453657258
-  - 0.573090623
-  - -0.127574577
-  - -0.0286125589
-  - 0.0
-  - 0.0
-  - 0.0
-  - 0.0
-  - 0.0
-  - 0.0
-  rows: 1
-distortion_model: plumb_bob
-image_height: 1944
-image_width: 2592
-projection_matrix:
-  cols: 4
-  data:
-  - 5049.522495820976
-  - 0.0
-  - 4926.52466622439
-  - 0.0
-  - 0.0
-  - 2846.313734946246
-  - 1876.4165670318075
-  - 0.0
-  - 0.0
-  - 0.0
-  - 1.0
-  - 0.0
-  rows: 3
-rectification_matrix:
-  cols: 3
-  data:
-  - 1
-  - 0
-  - 0
-  - 0
-  - 1
-  - 0
-  - 0
-  - 0
-  - 1
-  rows: 3

clover/camera_info/fisheye_cam_320x240.yaml

@@ -1,65 +0,0 @@
-# Generated from fisheye_cam.yaml by rescale_camera_info.py
-camera_matrix:
-  cols: 3
-  data:
-  - 166.23942373073172
-  - 0.0
-  - 160.0
-  - 0.0
-  - 166.5880923974026
-  - 120.0
-  - 0.0
-  - 0.0
-  - 1.0
-  rows: 3
-camera_name: main_camera_optical
-distortion_coefficients:
-  cols: 8
-  data:
-  - 0.215356885
-  - -0.117472846
-  - -0.000306197672
-  - -0.000109444025
-  - -0.00453657258
-  - 0.573090623
-  - -0.127574577
-  - -0.0286125589
-  - 0.0
-  - 0.0
-  - 0.0
-  - 0.0
-  - 0.0
-  - 0.0
-  rows: 1
-distortion_model: plumb_bob
-image_height: 240
-image_width: 320
-projection_matrix:
-  cols: 4
-  data:
-  - 166.23942373073172
-  - 0.0
-  - 162.19011246829268
-  - 0.0
-  - 0.0
-  - 166.5880923974026
-  - 109.82227735714285
-  - 0.0
-  - 0.0
-  - 0.0
-  - 1.0
-  - 0.0
-  rows: 3
-rectification_matrix:
-  cols: 3
-  data:
-  - 1
-  - 0
-  - 0
-  - 0
-  - 1
-  - 0
-  - 0
-  - 0
-  - 1
-  rows: 3

clover/camera_info/fisheye_cam_3840x2160.yaml

@@ -1,65 +0,0 @@
-# Generated from fisheye_cam.yaml by rescale_camera_info.py
-camera_matrix:
-  cols: 3
-  data:
-  - 30297.13497492585
-  - 0.0
-  - 29160.0
-  - 0.0
-  - 12808.411807258106
-  - 9226.40625
-  - 0.0
-  - 0.0
-  - 1.0
-  rows: 3
-camera_name: main_camera_optical
-distortion_coefficients:
-  cols: 8
-  data:
-  - 0.215356885
-  - -0.117472846
-  - -0.000306197672
-  - -0.000109444025
-  - -0.00453657258
-  - 0.573090623
-  - -0.127574577
-  - -0.0286125589
-  - 0.0
-  - 0.0
-  - 0.0
-  - 0.0
-  - 0.0
-  - 0.0
-  rows: 1
-distortion_model: plumb_bob
-image_height: 2160
-image_width: 3840
-projection_matrix:
-  cols: 4
-  data:
-  - 30297.13497492585
-  - 0.0
-  - 29559.14799734634
-  - 0.0
-  - 0.0
-  - 12808.411807258106
-  - 8443.874551643134
-  - 0.0
-  - 0.0
-  - 0.0
-  - 1.0
-  - 0.0
-  rows: 3
-rectification_matrix:
-  cols: 3
-  data:
-  - 1
-  - 0
-  - 0
-  - 0
-  - 1
-  - 0
-  - 0
-  - 0
-  - 1
-  rows: 3

clover/camera_info/fisheye_cam_4056x3040.yaml

@@ -1,65 +0,0 @@
-# Generated from fisheye_cam.yaml by rescale_camera_info.py
-camera_matrix:
-  cols: 3
-  data:
-  - 192008.0929035926
-  - 0.0
-  - 184801.5
-  - 0.0
-  - 81119.941445968
-  - 58433.90625
-  - 0.0
-  - 0.0
-  - 1.0
-  rows: 3
-camera_name: main_camera_optical
-distortion_coefficients:
-  cols: 8
-  data:
-  - 0.215356885
-  - -0.117472846
-  - -0.000306197672
-  - -0.000109444025
-  - -0.00453657258
-  - 0.573090623
-  - -0.127574577
-  - -0.0286125589
-  - 0.0
-  - 0.0
-  - 0.0
-  - 0.0
-  - 0.0
-  - 0.0
-  rows: 1
-distortion_model: plumb_bob
-image_height: 3040
-image_width: 4056
-projection_matrix:
-  cols: 4
-  data:
-  - 192008.0929035926
-  - 0.0
-  - 187331.10043318244
-  - 0.0
-  - 0.0
-  - 81119.941445968
-  - 53477.87216040651
-  - 0.0
-  - 0.0
-  - 0.0
-  - 1.0
-  - 0.0
-  rows: 3
-rectification_matrix:
-  cols: 3
-  data:
-  - 1
-  - 0
-  - 0
-  - 0
-  - 1
-  - 0
-  - 0
-  - 0
-  - 1
-  rows: 3

clover/camera_info/fisheye_cam_640x480.yaml

@@ -1,65 +0,0 @@
-# Generated from fisheye_cam.yaml by rescale_camera_info.py
-camera_matrix:
-  cols: 3
-  data:
-  - 166.23942373073172
-  - 0.0
-  - 160.0
-  - 0.0
-  - 166.5880923974026
-  - 120.0
-  - 0.0
-  - 0.0
-  - 1.0
-  rows: 3
-camera_name: main_camera_optical
-distortion_coefficients:
-  cols: 8
-  data:
-  - 0.215356885
-  - -0.117472846
-  - -0.000306197672
-  - -0.000109444025
-  - -0.00453657258
-  - 0.573090623
-  - -0.127574577
-  - -0.0286125589
-  - 0.0
-  - 0.0
-  - 0.0
-  - 0.0
-  - 0.0
-  - 0.0
-  rows: 1
-distortion_model: plumb_bob
-image_height: 480
-image_width: 640
-projection_matrix:
-  cols: 4
-  data:
-  - 166.23942373073172
-  - 0.0
-  - 162.19011246829268
-  - 0.0
-  - 0.0
-  - 166.5880923974026
-  - 109.82227735714285
-  - 0.0
-  - 0.0
-  - 0.0
-  - 1.0
-  - 0.0
-  rows: 3
-rectification_matrix:
-  cols: 3
-  data:
-  - 1
-  - 0
-  - 0
-  - 0
-  - 1
-  - 0
-  - 0
-  - 0
-  - 1
-  rows: 3

clover/camera_info/fisheye_cam_800x600.yaml

@@ -1,65 +0,0 @@
-# Generated from fisheye_cam.yaml by rescale_camera_info.py
-camera_matrix:
-  cols: 3
-  data:
-  - 207.79927966341464
-  - 0.0
-  - 200.0
-  - 0.0
-  - 208.23511549675322
-  - 150.0
-  - 0.0
-  - 0.0
-  - 1.0
-  rows: 3
-camera_name: main_camera_optical
-distortion_coefficients:
-  cols: 8
-  data:
-  - 0.215356885
-  - -0.117472846
-  - -0.000306197672
-  - -0.000109444025
-  - -0.00453657258
-  - 0.573090623
-  - -0.127574577
-  - -0.0286125589
-  - 0.0
-  - 0.0
-  - 0.0
-  - 0.0
-  - 0.0
-  - 0.0
-  rows: 1
-distortion_model: plumb_bob
-image_height: 600
-image_width: 800
-projection_matrix:
-  cols: 4
-  data:
-  - 207.79927966341464
-  - 0.0
-  - 202.73764058536585
-  - 0.0
-  - 0.0
-  - 208.23511549675322
-  - 137.27784669642855
-  - 0.0
-  - 0.0
-  - 0.0
-  - 1.0
-  - 0.0
-  rows: 3
-rectification_matrix:
-  cols: 3
-  data:
-  - 1
-  - 0
-  - 0
-  - 0
-  - 1
-  - 0
-  - 0
-  - 0
-  - 1
-  rows: 3

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

@@ -5,7 +5,7 @@
 # - 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/navigate_wait.py

@@ -16,8 +16,11 @@ set_attitude = rospy.ServiceProxy('set_attitude', srv.SetAttitude)
 set_rates = rospy.ServiceProxy('set_rates', srv.SetRates)
 land = rospy.ServiceProxy('land', Trigger)
 
-def navigate_wait(x=0, y=0, z=0, yaw=math.nan, speed=0.5, frame_id='body', tolerance=0.2, auto_arm=False):
-    res = navigate(x=x, y=y, z=z, yaw=yaw, speed=speed, frame_id=frame_id, auto_arm=auto_arm)
+def navigate_wait(x=0, y=0, z=0, yaw=float('nan'), yaw_rate=0, speed=0.5, \
+        frame_id='body', tolerance=0.2, auto_arm=False):
+
+    res = navigate(x=x, y=y, z=z, yaw=yaw, yaw_rate=yaw_rate, speed=speed, \
+        frame_id=frame_id, auto_arm=auto_arm)
 
     if not res.success:
         return res

clover/examples/red_circle.py

@@ -1,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://clover.coex.tech/red_circle
-
-# Input topic: main_camera/image_raw (camera image)
-# Output topics:
-#   cv/mask (red color mask)
-#   cv/red_circle (position of the center of the red circle in 3D space)
-
-import rospy
-import cv2
-import numpy as np
-from math import nan
-from sensor_msgs.msg import Image, CameraInfo
-from geometry_msgs.msg import PointStamped, Point
-from cv_bridge import CvBridge
-from clover import long_callback, srv
-import tf2_ros
-import tf2_geometry_msgs
-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/launch/aruco.launch

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

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

@@ -3,15 +3,9 @@
 
     <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="type" default="libcamera"/> <!-- camera interface: libcamera, v4l2 -->
-    <arg name="camera_id" default="0"/> <!-- libcamera camera id -->
-    <arg name="device" default="/dev/video0"/> <!-- v4l2 device path -->
-    <arg name="width" default="320"/>
-    <arg name="height" default="240"/>
-    <arg name="fps" default="40"/>
+    <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"/>
@@ -31,34 +25,20 @@
         <param name="num_worker_threads" value="2"/>
     </node>
 
-    <!-- camera node using libcamera -->
-    <node pkg="nodelet" type="nodelet" name="main_camera" args="load libcamera_ros/LibcameraRos main_camera_nodelet_manager" output="screen" clear_params="true" if="$(eval not simulator and type == 'libcamera')" respawn="true">
-        <param name="camera_name" value=""/>
-        <param name="camera_id" value="$(arg camera_id)"/>
-        <param name="frame_id" value="main_camera_optical"/>
-        <param name="calib_url" type="string" value="file://$(find clover)/camera_info/fisheye_cam_$(arg width)x$(arg height).yaml"/>
-        <param name="stream_role" value="still"/>
-        <param name="pixel_format" value="RGB888"/>
-        <param name="use_ros_time" value="true"/>
-        <param name="resolution/width" value="$(arg width)"/>
-        <param name="resolution/height" value="$(arg height)"/>
-
-        <!-- see: https://github.com/ctu-mrs/libcamera_ros/blob/b3645/config/param.yaml#L19 -->
-        <param name="control/fps" value="$(arg fps)"/>
-    </node>
-
-    <!-- old camera node for v4l2 (cv_camera) -->
-    <node pkg="nodelet" type="nodelet" name="main_camera" args="load cv_camera/CvCameraNodelet main_camera_nodelet_manager" launch-prefix="rosrun clover waitfile $(arg device)" clear_params="true" if="$(eval not simulator and type == 'v4l2')" respawn="true">
+    <!-- camera node -->
+    <node pkg="nodelet" type="nodelet" name="main_camera" args="load cv_camera/CvCameraNodelet main_camera_nodelet_manager" launch-prefix="rosrun clover waitfile $(arg device)" clear_params="true" unless="$(arg simulator)" respawn="true">
         <param name="device_path" value="$(arg device)"/>
         <param name="frame_id" value="main_camera_optical"/>
         <param name="camera_info_url" value="file://$(find clover)/camera_info/fisheye_cam.yaml"/>
 
         <param name="rate" value="100"/> <!-- poll rate -->
-        <param name="cv_cap_prop_fps" value="$(arg fps)"/> <!-- camera FPS -->
+        <param name="cv_cap_prop_fps" value="40"/> <!-- camera FPS -->
         <param name="capture_delay" value="0.02"/> <!-- approximate delay on frame retrieving -->
         <param name="rescale_camera_info" value="true"/> <!-- automatically rescale camera calibration info -->
-        <param name="image_width" value="$(arg width)"/>
-        <param name="image_height" value="$(arg height)"/>
+
+        <!-- camera resolution -->
+        <param name="image_width" value="320"/>
+        <param name="image_height" value="240"/>
     </node>
 
     <!-- camera visualization markers -->
@@ -69,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,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>clover</name>
-  <version>0.24.0</version>
+  <version>0.23.0</version>
   <description>The Clover package</description>
 
   <maintainer email="okalachev@gmail.com">Oleg Kalachev</maintainer>
@@ -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
@@ -264,7 +252,7 @@ def check_fcu():
         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:
@@ -303,14 +291,6 @@ def check_fcu():
                     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://clover.coex.tech/power', cell)
                     elif cell < 3.7:
@@ -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')

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);
 	}
 }
@@ -649,59 +528,10 @@ inline void checkState()
 		throw std::runtime_error("No connection to FCU, https://clover.coex.tech/connection");
 }
 
-void publishState()
-{
-	clover::State msg;
-	msg.mode = setpoint_type;
-	msg.yaw_mode = setpoint_yaw_type;
-
-	if (setpoint_position.header.frame_id.empty()) {
-		msg.x = NAN;
-		msg.y = NAN;
-		msg.z = NAN;
-	} else {
-		msg.x = setpoint_position.point.x;
-		msg.y = setpoint_position.point.y;
-		msg.z = setpoint_altitude.point.z;
-	}
-
-	msg.speed = nav_speed;
-	msg.lat = setpoint_lat;
-	msg.lon = setpoint_lon;
-	msg.vx = setpoint_velocity.vector.x;
-	msg.vy = setpoint_velocity.vector.y;
-	msg.vz = setpoint_velocity.vector.z;
-	msg.roll = setpoint_roll;
-	msg.pitch = setpoint_pitch;
-	msg.yaw = !yaw_frame_id.empty() ? setpoint_yaw : NAN;
-
-	msg.roll_rate = setpoint_rates.x;
-	msg.pitch_rate = setpoint_rates.y;
-	msg.yaw_rate = setpoint_rates.z;
-	msg.thrust = setpoint_thrust;
-
-	if (setpoint_type == VELOCITY) {
-		msg.xy_frame_id = setpoint_velocity.header.frame_id;
-		msg.z_frame_id = setpoint_velocity.header.frame_id;
-	} else {
-		msg.xy_frame_id = setpoint_position.header.frame_id;
-		msg.z_frame_id = setpoint_altitude.header.frame_id;
-	}
-	msg.yaw_frame_id = yaw_frame_id;
-
-	state_pub.publish(msg);
-}
-
-inline float safe(float value) {
-	return std::isfinite(value) ? value : 0;
-}
-
 #define ENSURE_FINITE(var) { if (!std::isfinite(var)) throw std::runtime_error(#var " argument cannot be NaN or Inf"); }
 
-#define ENSURE_NON_INF(var) { if (std::isinf(var)) throw std::runtime_error(#var " argument cannot be Inf"); }
-
 bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, float vy, float vz,
-           float roll, float pitch, float yaw, float roll_rate, float pitch_rate, float yaw_rate, // editorconfig-checker-disable-line
+           float pitch, float roll, float yaw, float pitch_rate, float roll_rate, float yaw_rate, // editorconfig-checker-disable-line
            float lat, float lon, float thrust, float speed, string frame_id, bool auto_arm, // editorconfig-checker-disable-line
            uint8_t& success, string& message) // editorconfig-checker-disable-line
 {
@@ -728,40 +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/tools/rescale_camera_info.py

@@ -1,68 +0,0 @@
-#!/usr/bin/env python3
-
-# Copyright (C) 2024 Copter Express Technologies
-#
-# Author: Oleg Kalachev <okalachev@gmail.com>
-#
-# Distributed under MIT License (available at https://opensource.org/licenses/MIT).
-# The above copyright notice and this permission notice shall be included in all
-# copies or substantial portions of the Software.
-
-"""Rescale camera info
-
-Rescale camera info files for different resolutions.
-
-Usage:
-  rescale_camera_info.py <camera_info_file>
-  rescale_camera_info.py (-h | --help)
-
-Options:
-  <camera_info_file>  Path to the source camera info file
-
-Example:
-  rescale_camera_info.py camera_info.yaml
-"""
-
-from docopt import docopt
-import yaml
-
-arguments = docopt(__doc__)
-
-camera_info = yaml.safe_load(open(arguments['<camera_info_file>']))
-RESOLUTIONS = (
-    (320, 240), # QVGA
-    (640, 480), # VGA
-    (800, 600), # SVGA
-    (1280, 720), # HD
-    (1920, 1080), # FullHD
-    (2592, 1944), # 5MP
-    (3840, 2160), # 4K
-    (4056, 3040),
-)
-# TODO: retrieve resolutions list (v4l2-ctl --list-formats-ext)
-
-for resolution in RESOLUTIONS:
-    width_k = resolution[0] / camera_info['image_width']
-    height_k = resolution[1] / camera_info['image_height']
-
-    camera_info_rescaled = camera_info.copy()
-    camera_info_rescaled['image_width'] = resolution[0]
-    camera_info_rescaled['image_height'] = resolution[1]
-
-    # See http://docs.ros.org/api/sensor_msgs/html/msg/CameraInfo.html for clarification
-    camera_info_rescaled['camera_matrix']['data'][0] *= width_k
-    camera_info_rescaled['camera_matrix']['data'][2] *= width_k
-    camera_info_rescaled['camera_matrix']['data'][4] *= height_k
-    camera_info_rescaled['camera_matrix']['data'][5] *= height_k
-
-    camera_info_rescaled['projection_matrix']['data'][0] *= width_k
-    camera_info_rescaled['projection_matrix']['data'][2] *= width_k
-    camera_info_rescaled['projection_matrix']['data'][5] *= height_k
-    camera_info_rescaled['projection_matrix']['data'][6] *= height_k
-
-    output_file = arguments['<camera_info_file>'].replace('.yaml', '_{}x{}.yaml'.format(resolution[0], resolution[1]))
-    with open(output_file, 'w') as f:
-        f.write('# Generated from {} by rescale_camera_info.py\n'.format(arguments['<camera_info_file>']))
-        yaml.dump(camera_info_rescaled, f)
-
-    print('Saved {}'.format(output_file))

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

@@ -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_blocks/README.md

@@ -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.24.0</version>
+  <version>0.23.0</version>
   <description>Blockly programming support for Clover</description>
   <maintainer email="okalachev@gmail.com">Oleg Kalachev</maintainer>
   <license>MIT</license>

clover_blocks/www/blocks.js

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

clover_blocks/www/index.html

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

clover_blocks/www/python.js

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

clover_description/package.xml

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

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

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)