Merge branch 'v0.24-release' into simple-offboard-update

* aruco_pose: rename parameter known_tilt to known_vertical

* More clean variable names

* aruco_pose: add flip_vertical parameter and get rid of map_flipped

* selfcheck.py: support flip_vertical parameter

* aruco_pose: document flip_vertical parameter

* selfcheck.py: fix known_vertical description

* Fix editorconfig

.github/workflows/build.yml

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

.github/workflows/docs.yml

@@ -11,10 +11,6 @@ permissions:
   pages: write
   id-token: write
 
-concurrency:
-  group: "pages"
-  cancel-in-progress: true
-
 defaults:
   run:
     shell: bash
@@ -75,6 +71,9 @@ 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 }}

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/workflow/status/CopterExpress/clover/CI)
+![GitHub Workflow Status](https://img.shields.io/github/actions/workflow/status/CopterExpress/clover/build-image.yaml?branch=master)
 ![GitHub all releases](https://img.shields.io/github/downloads/CopterExpress/clover/total)
 
 Image features:

aruco_pose/CMakeLists.txt

@@ -251,4 +251,5 @@ if (CATKIN_ENABLE_TESTING)
   add_rostest(test/test_node_failure.test)
   add_rostest(test/largemap.test)
   add_rostest(test/crash_opencv.test)
+  add_rostest(test/duplicate.test)
 endif()

aruco_pose/README.md

@@ -43,7 +43,8 @@ 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_tilt` (*string*) – known tilt (pitch and roll) of all the markers as a frame
+* `~known_vertical` (*string*) – known vertical (Z axis) of all the markers as a frame
+* `~flip_vertical` – flip vertical vector
 
 ### Topics
 
@@ -71,7 +72,8 @@ 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_tilt` – known tilt (pitch and roll) of markers map as a frame
+* `~known_vertical` – known vertical (Z axis) of markers map as a frame
+* `~flip_vertical` – flip vertical vector
 * `~image_width` – debug image width (default: 2000)
 * `~image_height` – debug image height (default: 2000)
 * `~image_margin` – debug image margin (default: 200)

aruco_pose/package.xml

@@ -1,5 +1,5 @@
 <?xml version="1.0"?>
-<package format="2">
+<package format="3">
   <name>aruco_pose</name>
   <version>0.23.0</version>
   <description>Positioning with ArUco markers</description>
@@ -28,6 +28,8 @@
   <depend>sensor_msgs</depend>
   <depend>rostest</depend>
   <depend>dynamic_reconfigure</depend>
+  <depend condition="$ROS_PYTHON_VERSION == 2">python-docopt</depend>
+  <depend condition="$ROS_PYTHON_VERSION == 3">python3-docopt</depend>
 
   <test_depend>image_publisher</test_depend>
   <test_depend>ros_pytest</test_depend>

aruco_pose/src/aruco_detect.cpp

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

aruco_pose/src/aruco_map.cpp

@@ -81,9 +81,9 @@ private:
 	bool enabled_ = true;
 	std::string type_;
 	visualization_msgs::MarkerArray vis_array_;
-	std::string known_tilt_, map_, markers_frame_, markers_parent_frame_;
+	std::string known_vertical_, map_, markers_frame_, markers_parent_frame_;
 	int image_width_, image_height_, image_margin_;
-	bool auto_flip_, image_axis_;
+	bool flip_vertical_, auto_flip_, image_axis_;
 
 public:
 	virtual void onInit()
@@ -104,7 +104,8 @@ public:
 
 		type_ = nh_priv_.param<std::string>("type", "map");
 		transform_.child_frame_id = nh_priv_.param<std::string>("frame_id", "aruco_map");
-		known_tilt_ = nh_priv_.param<std::string>("known_tilt", "");
+		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);
 		auto_flip_ = nh_priv_.param("auto_flip", false);
 		image_width_ = nh_priv_.param("image_width" , 2000);
 		image_height_ = nh_priv_.param("image_height", 2000);
@@ -177,7 +178,7 @@ public:
 			corners.push_back(marker_corners);
 		}
 
-		if (known_tilt_.empty()) {
+		if (known_vertical_.empty()) {
 			// simple estimation
 			valid = cv::aruco::estimatePoseBoard(corners, ids, board_, camera_matrix_, dist_coeffs_,
 			                                     rvec, tvec, false);
@@ -191,7 +192,7 @@ public:
 
 		} else {
 			Mat obj_points, img_points;
-			// estimation with "snapping"
+			// estimation with known vertical
 			cv::aruco::getBoardObjectAndImagePoints(board_, corners, ids, obj_points, img_points);
 			if (obj_points.empty()) goto publish_debug;
 
@@ -203,11 +204,11 @@ public:
 
 			fillTransform(transform_.transform, rvec, tvec);
 			try {
-				geometry_msgs::TransformStamped snap_to = tf_buffer_.lookupTransform(markers->header.frame_id,
+				geometry_msgs::TransformStamped vertical = tf_buffer_.lookupTransform(markers->header.frame_id,
-				                                          known_tilt_, markers->header.stamp, ros::Duration(0.02));
+				                                           known_vertical_, markers->header.stamp, ros::Duration(0.02));
-				snapOrientation(transform_.transform.rotation, snap_to.transform.rotation, auto_flip_);
+				applyVertical(transform_.transform.rotation, vertical.transform.rotation, flip_vertical_, auto_flip_);
 			} catch (const tf2::TransformException& e) {
-				NODELET_WARN_THROTTLE(1, "can't snap: %s", e.what());
+				NODELET_WARN_THROTTLE(1, "can't retrieve known vertical: %s", e.what());
 			}
 
 			geometry_msgs::TransformStamped shift;
@@ -503,7 +504,7 @@ publish_debug:
 		vis_marker.pose.position.x = x;
 		vis_marker.pose.position.y = y;
 		vis_marker.pose.position.z = z;
-		tf::quaternionTFToMsg(q, marker.pose.orientation);
+		tf::quaternionTFToMsg(q, vis_marker.pose.orientation);
 		vis_marker.frame_locked = true;
 		vis_array_.markers.push_back(vis_marker);
 

aruco_pose/src/utils.h

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

aruco_pose/test/basic.py

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

aruco_pose/test/duplicate.py

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

aruco_pose/test/duplicate.test

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

builder/image-ros.sh

@@ -151,6 +151,9 @@ catkin_make run_tests #&& catkin_test_results
 echo_stamp "Change permissions for catkin_ws"
 chown -Rf pi:pi /home/pi/catkin_ws
 
+echo_stamp "Update www"
+sudo -u pi sh -c ". devel/setup.sh && rosrun clover www"
+
 echo_stamp "Make \$HOME/examples symlink"
 ln -s "$(catkin_find clover examples --first-only)" /home/pi
 chown -Rf pi:pi /home/pi/examples

builder/test/tests.py

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

builder/test/tests.sh

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

clover/CMakeLists.txt

@@ -53,7 +53,7 @@ find_package(OpenCV ${_opencv_version} REQUIRED
 ## Uncomment this if the package has a setup.py. This macro ensures
 ## modules and global scripts declared therein get installed
 ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
-# catkin_python_setup()
+catkin_python_setup()
 
 ################################################
 ## Declare ROS messages, services and actions ##
@@ -80,11 +80,10 @@ find_package(OpenCV ${_opencv_version} REQUIRED
 ##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
 
 ## Generate messages in the 'msg' folder
-# add_message_files(
+add_message_files(
-#   FILES
+  FILES
-#   Message1.msg
+  State.msg
-#   Message2.msg
+)
-# )
 
 ## Generate services in the 'srv' folder
 add_service_files(
@@ -92,6 +91,9 @@ add_service_files(
   GetTelemetry.srv
   Navigate.srv
   NavigateGlobal.srv
+  SetAltitude.srv
+  SetYaw.srv
+  SetYawRate.srv
   SetPosition.srv
   SetVelocity.srv
   SetAttitude.srv
@@ -306,4 +308,5 @@ endif()
 if (CATKIN_ENABLE_TESTING)
   find_package(rostest REQUIRED)
   add_rostest(test/basic.test)
+  add_rostest(test/offboard.test)
 endif()

clover/examples/camera.py

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

clover/examples/navigate_wait.py

@@ -16,11 +16,8 @@ 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=float('nan'), yaw_rate=0, speed=0.5, \
+def navigate_wait(x=0, y=0, z=0, yaw=math.nan, speed=0.5, frame_id='body', tolerance=0.2, auto_arm=False):
-        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)
-
-    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/launch/aruco.launch

@@ -18,8 +18,9 @@
         <remap from="map_markers" to="aruco_map/map"/>
         <param name="estimate_poses" value="true"/>
         <param name="send_tf" value="true"/>
-        <param name="known_tilt" value="map" if="$(eval placement == 'floor')"/>
+        <param name="use_map_markers" value="true"/>
-        <param name="known_tilt" value="map_flipped" if="$(eval placement == 'ceiling')"/>
+        <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="length" value="$(arg length)"/>
         <param name="transform_timeout" value="0.1"/>
         <!-- aruco detector parameters -->
@@ -35,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_tilt" value="map" if="$(eval placement == 'floor')"/>
+        <param name="known_vertical" value="map" if="$(eval placement == 'floor' or placement == 'ceiling')"/>
-        <param name="known_tilt" value="map_flipped" if="$(eval placement == 'ceiling')"/>
+        <param name="flip_vertical" value="true" if="$(eval placement == 'ceiling')"/>
         <param name="image_axis" value="true"/>
         <param name="frame_id" value="aruco_map_detected" if="$(arg aruco_vpe)"/>
         <param name="frame_id" value="aruco_map" unless="$(arg aruco_vpe)"/>

clover/launch/clover.launch

@@ -12,7 +12,7 @@
     <arg name="led" default="true"/>
     <arg name="blocks" default="false"/>
     <arg name="rc" default="false"/>
-    <arg name="force_init" value="true"/> <!-- force estimator to init by publishing zero pose -->
+    <arg name="force_init" default="true"/> <!-- force estimator to init by publishing zero pose -->
 
     <arg name="simulator" default="false"/> <!-- flag that we are operating on a simulated drone -->
 
@@ -45,10 +45,9 @@
         <remap from="camera_info" to="main_camera/camera_info"/>
         <param name="calc_flow_gyro" value="true"/>
         <param name="roi_rad" value="0.8"/>
+        <param name="disable_on_vpe" value="true"/>
     </node>
 
-    <node pkg="tf2_ros" type="static_transform_publisher" name="map_flipped_frame" args="0 0 0 3.1415926 3.1415926 0 map map_flipped"/>
-
     <!-- simplified offboard control -->
     <node name="simple_offboard" pkg="clover" type="simple_offboard" output="screen" clear_params="true">
         <param name="reference_frames/main_camera_optical" value="map"/>
@@ -86,8 +85,4 @@
         <param name="use_fake_gcs" value="false"/>
     </node>
 
-    <!-- Update static directory -->
-    <node pkg="roswww_static" name="roswww_static" type="main.py" clear_params="true">
-        <param name="default_package" value="clover"/>
-    </node>
 </launch>

clover/launch/main_camera.launch

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

clover/launch/mavros.launch

@@ -1,5 +1,5 @@
 <launch>
-    <arg name="fcu_conn" default="usb"/> <!-- options: usb, uart, tcp, udp, sitl -->
+    <arg name="fcu_conn" default="usb"/> <!-- options: usb, uart, tcp, udp, sitl, hitl -->
     <arg name="fcu_ip" default="127.0.0.1"/>
     <arg name="fcu_sys_id" default="1"/>
     <arg name="gcs_bridge" default="tcp"/>
@@ -23,6 +23,9 @@
         <!-- sitl since PX4 1.9.0 -->
         <param name="fcu_url" value="udp://@$(arg fcu_ip):14580" if="$(eval fcu_conn == 'sitl')"/>
 
+        <!-- hitl connection (to gazebo_mavlink_interface plugin) -->
+        <param name="fcu_url" value="udp://$(arg fcu_ip):14540@" if="$(eval fcu_conn == 'hitl')"/>
+
         <!-- set target_system_id -->
         <param name="target_system_id" value="$(arg fcu_sys_id)" />
 

clover/launch/simulator.launch

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

clover/msg/State.msg

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

@@ -37,10 +37,13 @@
   <depend>rosbridge_server</depend>
   <depend>web_video_server</depend>
   <depend>tf2_web_republisher</depend>
+  <depend>libxml2</depend>
+  <depend>libxslt</depend>
+  <depend condition="$ROS_PYTHON_VERSION == 2">python-lxml</depend>
+  <depend condition="$ROS_PYTHON_VERSION == 3">python3-lxml</depend>
   <depend>dynamic_reconfigure</depend>
   <exec_depend>python-pymavlink</exec_depend>
-  <!-- Use test_depend for packages you need only for testing: -->
+  <test_depend>ros_pytest</test_depend>
-  <!--   <test_depend>gtest</test_depend> -->
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>

clover/requirements.txt

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

clover/setup.py

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

clover/src/autotest/autotest_aruco.py

@@ -13,7 +13,12 @@ from util import handle_response
 
 rospy.init_node('autotest_aruco', disable_signals=True) # disable signals to allow interrupting with ctrl+c
 
-flow_client = dynamic_reconfigure.client.Client('optical_flow')
+try:
+    flow_client = dynamic_reconfigure.client.Client('optical_flow', timeout=2)
+except rospy.ROSException:
+    flow_client = None
+    print('Cannot configure optical flow, skip')
+
 get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry)
 navigate = handle_response(rospy.ServiceProxy('navigate', srv.Navigate))
 land = handle_response(rospy.ServiceProxy('land', Trigger))
@@ -30,11 +35,8 @@ 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=float('nan'), yaw_rate=0, speed=0.5, \
+def navigate_wait(x=0, y=0, z=0, yaw=math.nan, speed=0.5, frame_id='body', tolerance=0.2, auto_arm=False):
-        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)
-
-    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
@@ -67,12 +69,13 @@ input('Go to center %g %g 1.5 speed 5 [enter] ' % (center_x, center_y))
 navigate_wait(x=center_x, y=center_y, z=1.5, speed=5, frame_id='aruco_map')
 print_current_map_position()
 
-input('Disable optical flow and keep hovering [enter] ')
+if flow_client:
-flow_client.update_configuration({'enabled': False})
+    input('Disable optical flow and keep hovering [enter] ')
-rospy.sleep(5)
+    flow_client.update_configuration({'enabled': False})
+    rospy.sleep(5)
 
-input('Enable optical flow back [enter] ')
+    input('Enable optical flow back [enter] ')
-flow_client.update_configuration({'enabled': True})
+    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
+from math import nan, inf
 import signal
 import sys
 from clover import srv
@@ -15,6 +15,8 @@ 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))
@@ -28,11 +30,8 @@ def interrupt(sig, frame):
 
 signal.signal(signal.SIGINT, interrupt)
 
-def navigate_wait(x=0, y=0, z=0, yaw=nan, yaw_rate=0, speed=0.5, \
+def navigate_wait(x=0, y=0, z=0, yaw=nan, speed=0.5, frame_id='body', tolerance=0.2, auto_arm=False):
-        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)
-
-    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,17 +68,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° [enter] ')
+input('Rotate right 90° using set_yaw [enter] ')
-navigate(yaw=-math.pi / 2, frame_id='navigate_target')
+set_yaw(yaw=-math.pi / 2, frame_id='navigate_target')
 rospy.sleep(3)
 
 input('Use set_attitude to fly backwards [enter]')
-set_attitude(pitch=-0.3, roll=0, yaw=0, thrust=0.5, frame_id='body')
+set_attitude(roll=0, pitch=-0.3, yaw=0, thrust=0.5, frame_id='body')
 rospy.sleep(0.3)
 set_position(frame_id='body')
 
 input('Use set_attitude to fly right [enter]')
-set_attitude(pitch=0, roll=0.3, yaw=0, thrust=0.5, frame_id='body')
+set_attitude(roll=0.3, pitch=0, yaw=0, thrust=0.5, frame_id='body')
 rospy.sleep(0.5)
 set_position(frame_id='body')
 
@@ -88,13 +87,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 yaw_rate [enter]')
+input('Rotate 360° to the right using set_yaw_rate [enter]')
-set_position(x=nan, y=nan, z=nan, frame_id='body', yaw=nan, yaw_rate=-1)
+set_yaw_rate(yaw_rate=-1)
 rospy.sleep(2 * math.pi)
 set_position(frame_id='body')
 
-input('Return to start point [enter]')
+input('Return to start point heading forward [enter]')
-navigate_wait(x=start.x, y=start.y, z=start.z, yaw=start.yaw, speed=1, frame_id='map')
+navigate_wait(x=start.x, y=start.y, z=start.z, yaw=inf, speed=1, frame_id='map')
 
 input('Land [enter]')
 land()

clover/src/clover/__init__.py

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

clover/src/led.cpp

@@ -319,8 +319,8 @@ int main(int argc, char **argv)
 
 	auto set_effect = nh.advertiseService("set_effect", &setEffect);
 
-	auto mavros_state_sub = nh.subscribe("/mavros/state", 1, &handleMavrosState);
+	auto mavros_state_sub = nh.subscribe("mavros/state", 1, &handleMavrosState);
-	auto battery_sub = nh.subscribe("/mavros/battery", 1, &handleBattery);
+	auto battery_sub = nh.subscribe("mavros/battery", 1, &handleBattery);
 	auto rosout_sub = nh.subscribe("/rosout_agg", 1, &handleLog);
 
 	timer = nh.createTimer(ros::Duration(0), &proceed, false, false);

clover/src/optical_flow.cpp

@@ -25,6 +25,7 @@
 #include <dynamic_reconfigure/server.h>
 #include <mavros_msgs/OpticalFlowRad.h>
 #include <sensor_msgs/Imu.h>
+#include <geometry_msgs/PoseStamped.h>
 #include <geometry_msgs/Vector3Stamped.h>
 #include <geometry_msgs/PointStamped.h>
 #include <geometry_msgs/TwistStamped.h>
@@ -57,6 +58,9 @@ private:
 	std::unique_ptr<tf2_ros::TransformListener> tf_listener_;
 	bool calc_flow_gyro_;
 	float flow_gyro_default_;
+	bool disable_on_vpe_;
+	ros::Subscriber vpe_sub_;
+	ros::Time last_vpe_time_;
 	std::shared_ptr<dynamic_reconfigure::Server<clover::FlowConfig>> dyn_srv_;
 
 	void onInit()
@@ -87,6 +91,11 @@ private:
 
 		img_sub_ = it.subscribeCamera("image_raw", 1, &OpticalFlow::flow, this);
 
+		disable_on_vpe_ = nh_priv.param("disable_on_vpe", false);
+		if (disable_on_vpe_) {
+			vpe_sub_ = nh.subscribe("mavros/vision_pose/pose", 1, &OpticalFlow::vpeCallback, this);
+		}
+
 		dyn_srv_ = std::make_shared<dynamic_reconfigure::Server<clover::FlowConfig>>(nh_priv);
 		dynamic_reconfigure::Server<clover::FlowConfig>::CallbackType cb;
 
@@ -121,6 +130,12 @@ private:
 	{
 		if (!enabled_) return;
 
+		if (disable_on_vpe_ &&
+			!last_vpe_time_.isZero() &&
+			(msg->header.stamp - last_vpe_time_).toSec() < 0.1) {
+			return;
+		}
+
 		parseCameraInfo(cinfo);
 
 		auto img = cv_bridge::toCvShare(msg, "mono8")->image;
@@ -154,7 +169,7 @@ private:
 
 		img.convertTo(curr_, CV_32F);
 
-		if (prev_.empty()) {
+		if (prev_.empty() || (msg->header.stamp - prev_stamp_).toSec() > 0.1) { // outdated previous frame
 			prev_ = curr_.clone();
 			prev_stamp_ = msg->header.stamp;
 			cv::createHanningWindow(hann_, curr_.size(), CV_32F);
@@ -236,6 +251,14 @@ private:
 			prev_ = curr_.clone();
 			prev_stamp_ = msg->header.stamp;
 
+			// Publish estimated angular velocity
+			geometry_msgs::TwistStamped velo;
+			velo.header.stamp = msg->header.stamp;
+			velo.header.frame_id = fcu_frame_id_;
+			velo.twist.angular.x = flow_fcu.vector.x / integration_time.toSec();
+			velo.twist.angular.y = flow_fcu.vector.y / integration_time.toSec();
+			velo_pub_.publish(velo);
+
 publish_debug:
 			// Publish debug image
 			if (img_pub_.getNumSubscribers() > 0) {
@@ -248,14 +271,6 @@ publish_debug:
 				out_msg.image = img;
 				img_pub_.publish(out_msg.toImageMsg());
 			}
-
-			// Publish estimated angular velocity
-			geometry_msgs::TwistStamped velo;
-			velo.header.stamp = msg->header.stamp;
-			velo.header.frame_id = fcu_frame_id_;
-			velo.twist.angular.x = flow_fcu.vector.x / integration_time.toSec();
-			velo.twist.angular.y = flow_fcu.vector.y / integration_time.toSec();
-			velo_pub_.publish(velo);
 		}
 	}
 
@@ -284,6 +299,10 @@ publish_debug:
 			prev_ = Mat(); // clear previous frame
 		}
 	}
+
+	void vpeCallback(const geometry_msgs::PoseStamped& vpe) {
+		last_vpe_time_ = vpe.header.stamp;
+	}
 };
 
 PLUGINLIB_EXPORT_CLASS(OpticalFlow, nodelet::Nodelet)

clover/src/selfcheck.py

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

clover/src/simple_offboard.cpp

@@ -23,6 +23,7 @@
 #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>
@@ -37,14 +38,19 @@
 #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;
@@ -54,6 +60,7 @@ 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;
@@ -81,33 +88,40 @@ bool land_only_in_offboard, nav_from_sp, check_kill_switch;
 std::map<string, string> reference_frames;
 
 // Publishers
-ros::Publisher attitude_pub, attitude_raw_pub, position_pub, position_raw_pub, rates_pub, thrust_pub;
+ros::Publisher attitude_pub, attitude_raw_pub, position_pub, position_raw_pub, rates_pub, thrust_pub, state_pub;
 
 // Service clients
 ros::ServiceClient arming, set_mode;
 
 // Containers
 ros::Timer setpoint_timer;
-tf::Quaternion tq;
 PoseStamped position_msg;
 PositionTarget position_raw_msg;
-AttitudeTarget att_raw_msg;
+//TwistStamped rates_msg;
-Thrust thrust_msg;
-TwistStamped rates_msg;
 TransformStamped target, setpoint;
 geometry_msgs::TransformStamped body;
+geometry_msgs::TransformStamped terrain;
 
 // State
 PoseStamped nav_start;
-PoseStamped setpoint_position, setpoint_position_transformed;
+PointStamped setpoint_position;
-Vector3Stamped setpoint_velocity, setpoint_velocity_transformed;
+PointStamped setpoint_altitude;
-QuaternionStamped setpoint_attitude, setpoint_attitude_transformed;
+Vector3Stamped setpoint_velocity;
-float setpoint_yaw_rate;
+float setpoint_yaw, setpoint_roll, setpoint_pitch;
+Vector3 setpoint_rates;
+string yaw_frame_id;
+float setpoint_thrust;
 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,
@@ -115,7 +129,10 @@ enum setpoint_type_t {
 	POSITION,
 	VELOCITY,
 	ATTITUDE,
-	RATES
+	RATES,
+	_ALTITUDE,
+	_YAW,
+	_YAW_RATE,
 };
 
 enum setpoint_type_t setpoint_type = NONE;
@@ -170,7 +187,7 @@ void handleLocalPosition(const PoseStamped& pose)
 {
 	local_position = pose;
 	publishBodyFrame();
-	// TODO: terrain?, home?
+	// TODO: home?
 }
 
 // wait for transform without interrupting publishing setpoints
@@ -188,6 +205,20 @@ inline bool waitTransform(const string& target, const string& source,
 	return false;
 }
 
+void handleAltitude(const Altitude& alt)
+{
+	// publish terrain frame
+	if (!std::isfinite(alt.bottom_clearance)) return;
+	// terrain.header.stamp = alt.header.stamp;
+
+	if (!waitTransform(local_frame, body.child_frame_id, alt.header.stamp, ros::Duration(0.1))) return;
+
+	auto t = tf_buffer.lookupTransform(local_frame, body.child_frame_id, alt.header.stamp);
+	t.child_frame_id = terrain.child_frame_id;
+	t.transform.translation.z -= alt.bottom_clearance;
+	static_transform_broadcaster->sendTransform(t);
+}
+
 #define TIMEOUT(msg, timeout) (msg.header.stamp.isZero() || (ros::Time::now() - msg.header.stamp > timeout))
 
 bool getTelemetry(GetTelemetry::Request& req, GetTelemetry::Response& res)
@@ -207,11 +238,11 @@ bool getTelemetry(GetTelemetry::Request& req, GetTelemetry::Response& res)
 	res.vx = NAN;
 	res.vy = NAN;
 	res.vz = NAN;
-	res.pitch = NAN;
 	res.roll = NAN;
+	res.pitch = NAN;
 	res.yaw = NAN;
-	res.pitch_rate = NAN;
 	res.roll_rate = NAN;
+	res.pitch_rate = NAN;
 	res.yaw_rate = NAN;
 	res.voltage = NAN;
 	res.cell_voltage = NAN;
@@ -341,20 +372,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, float speed, Point& nav_setpoint)
+void getNavigateSetpoint(const ros::Time& stamp, const 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_position_transformed.pose.position);
+	float distance = getDistance(nav_start.pose.position, setpoint_pose_local.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_position_transformed.pose.position.x - nav_start.pose.position.x) * passed;
+	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_position_transformed.pose.position.y - nav_start.pose.position.y) * 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_position_transformed.pose.position.z - nav_start.pose.position.z) * passed;
+	nav_setpoint.z = nav_start.pose.position.z + (setpoint_pose_local.pose.position.z - nav_start.pose.position.z) * passed;
 }
 
 PoseStamped globalToLocal(double lat, double lon)
@@ -385,44 +416,101 @@ 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;
 
-	try {
+	// transform position
-		// transform position and/or yaw
+	if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == POSITION) {
-		if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == POSITION || setpoint_type == VELOCITY || setpoint_type == ATTITUDE) {
+		setpoint_position.header.stamp = stamp;
-			setpoint_position.header.stamp = stamp;
+		setpoint_altitude.header.stamp = stamp;
-			tf_buffer.transform(setpoint_position, setpoint_position_transformed, local_frame, ros::Duration(0.05));
+		// 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());
 		}
-
+		// transform altitude
-		// transform velocity
+		try {
-		if (setpoint_type == VELOCITY) {
+			setpoint_pose_local.pose.position.z = tf_buffer.transform(setpoint_altitude, local_frame, ros::Duration(0.05)).point.z;
-			setpoint_velocity.header.stamp = stamp;
+		} catch (tf2::TransformException& ex) {
-			tf_buffer.transform(setpoint_velocity, setpoint_velocity_transformed, local_frame, ros::Duration(0.05));
+			// can't transform altitude, use last known
+			ROS_WARN_THROTTLE(10, "can't transform: %s", ex.what());
 		}
-
-	} 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) {
-			double yaw_towards = atan2(position_msg.pose.position.y - nav_start.pose.position.y,
+			yaw_local = atan2(position_msg.pose.position.y - nav_start.pose.position.y,
-			                           position_msg.pose.position.x - nav_start.pose.position.x);
+			                  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_position_transformed;
+		position_msg = setpoint_pose_local;
+		position_msg.pose.orientation = tf::createQuaternionMsgFromYaw(yaw_local);
 	}
 
 	if (setpoint_type == POSITION || setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL) {
@@ -439,14 +527,14 @@ void publish(const ros::Time stamp)
 			                             PositionTarget::IGNORE_AFY +
 			                             PositionTarget::IGNORE_AFZ +
 			                             PositionTarget::IGNORE_YAW;
-			position_raw_msg.yaw_rate = setpoint_yaw_rate;
+			position_raw_msg.yaw_rate = setpoint_rates.z;
 			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
 			}
 
@@ -458,9 +546,22 @@ 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 +
@@ -468,14 +569,22 @@ 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_transformed.vector;
+		position_raw_msg.velocity = setpoint_velocity_local.vector;
-		position_raw_msg.yaw = tf2::getYaw(setpoint_position_transformed.pose.orientation);
+		position_raw_msg.yaw = yaw_local;
-		position_raw_msg.yaw_rate = setpoint_yaw_rate;
+		position_raw_msg.yaw_rate = setpoint_rates.z;
 		position_raw_pub.publish(position_raw_msg);
 	}
 
 	if (setpoint_type == ATTITUDE) {
-		attitude_pub.publish(setpoint_position_transformed);
+		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;
 		thrust_pub.publish(thrust_msg);
 	}
 
@@ -484,11 +593,12 @@ 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 = rates_msg.twist.angular;
+		att_raw_msg.body_rate = setpoint_rates;
-		att_raw_msg.thrust = thrust_msg.thrust;
+		att_raw_msg.thrust = setpoint_thrust;
 		attitude_raw_pub.publish(att_raw_msg);
 	}
 }
@@ -528,10 +638,59 @@ 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 pitch, float roll, float yaw, float pitch_rate, float roll_rate, float yaw_rate, // editorconfig-checker-disable-line
+           float roll, float pitch, float yaw, float roll_rate, float pitch_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
 {
@@ -558,69 +717,40 @@ 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;
 
-		// Serve "partial" commands
+		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);
 
-		if (!auto_arm && std::isfinite(yaw) &&
+		if (sp_type == NAVIGATE_GLOBAL) {
-		    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);
-			ENSURE_FINITE(z);
+		}
-		} else if (sp_type == VELOCITY) {
+
-			ENSURE_FINITE(vx);
+		if (isfinite(x) != isfinite(y)) {
-			ENSURE_FINITE(vy);
+			throw std::runtime_error("x and y can be set only together");
-			ENSURE_FINITE(vz);
+		}
-		} else if (sp_type == ATTITUDE) {
+
-			ENSURE_FINITE(pitch);
+		if (isfinite(yaw_rate)) {
-			ENSURE_FINITE(roll);
+			if (sp_type > RATES && setpoint_type == ATTITUDE) {
-			ENSURE_FINITE(thrust);
+				throw std::runtime_error("Yaw rate cannot be set in attitude mode.");
-		} else if (sp_type == RATES) {
+			}
-			ENSURE_FINITE(pitch_rate);
+		}
-			ENSURE_FINITE(roll_rate);
+
-			ENSURE_FINITE(thrust);
+		// 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.");
+			}
 		}
 
 		if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL) {
@@ -634,20 +764,13 @@ 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 (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == POSITION || sp_type == VELOCITY || sp_type == ATTITUDE) {
+		// 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)) {
 			// 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);
@@ -664,15 +787,26 @@ 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
-		setpoint_type = sp_type;
+		if (sp_type <= RATES) {
+			setpoint_type = sp_type;
+		}
 
-		if (sp_type != NAVIGATE && sp_type != NAVIGATE_GLOBAL) {
+		if (setpoint_type != NAVIGATE && setpoint_type != NAVIGATE_GLOBAL) {
 			nav_from_sp_flag = false;
 		}
 
+		if (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) {
@@ -681,89 +815,139 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl
 			} else {
 				nav_start = local_position;
 			}
-			nav_speed = speed;
+
+			if (!isnan(speed)) {
+				nav_speed = speed;
+			}
+
 			nav_from_sp_flag = true;
 		}
 
-		// if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == POSITION || sp_type == VELOCITY) {
+		// handle position
-		// 	if (std::isnan(yaw) && yaw_rate == 0) {
+		if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == POSITION) {
-		// 		// keep yaw unchanged
-		//		// TODO: this is incorrect, because we need yaw in desired frame
-		// 		yaw = tf2::getYaw(local_position.pose.orientation);
-		// 	}
-		// }
 
-		if (sp_type == POSITION || sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == VELOCITY || sp_type == ATTITUDE) {
+			PointStamped desired;
-			// destination point and/or attitude
+			desired.header.frame_id = frame_id;
-			PoseStamped ps;
+			desired.header.stamp = stamp;
-			ps.header.frame_id = frame_id;
+			desired.point.x = safe(x);
-			ps.header.stamp = stamp;
+			desired.point.y = safe(y);
-			ps.pose.position.x = x;
+			desired.point.z = safe(z);
-			ps.pose.position.y = y;
-			ps.pose.position.z = z;
-			ps.pose.orientation.w = 1.0; // Ensure quaternion is always valid
 
-			if (sp_type == ATTITUDE) {
+			// transform to reference frame
-				ps.pose.position.x = 0;
+			desired = tf_buffer.transform(desired, reference_frame);
-				ps.pose.position.y = 0;
+
-				ps.pose.position.z = 0;
+			// set horizontal position
-				ps.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);
+			if (isfinite(x) && isfinite(y)) {
-			} else if (std::isnan(yaw)) {
+				setpoint_position = desired;
-				setpoint_yaw_type = YAW_RATE;
+			} else if (setpoint_position.header.frame_id.empty()) {
-				setpoint_yaw_rate = yaw_rate;
+				 // TODO: use transform for current stamp
-			} else if (std::isinf(yaw) && yaw > 0) {
+				setpoint_position.header = local_position.header;
-				// yaw towards
+				setpoint_position.point = local_position.pose.position;
-				setpoint_yaw_type = TOWARDS;
-				yaw = 0;
-				setpoint_yaw_rate = 0;
-			} else {
-				setpoint_yaw_type = YAW;
-				setpoint_yaw_rate = 0;
-				ps.pose.orientation = tf::createQuaternionMsgFromYaw(yaw);
 			}
 
-			tf_buffer.transform(ps, setpoint_position, reference_frame);
+			// 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) {
-			Vector3Stamped vel;
+			// TODO: allow setting different modes by altitude and xy
-			vel.header.frame_id = frame_id;
+			Vector3Stamped desired;
-			vel.header.stamp = stamp;
+			desired.header.frame_id = frame_id;
-			vel.vector.x = vx;
+			desired.header.stamp = stamp;
-			vel.vector.y = vy;
+			desired.vector.x = safe(vx);
-			vel.vector.z = vz;
+			desired.vector.y = safe(vy);
-			tf_buffer.transform(vel, setpoint_velocity, reference_frame);
+			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;
+			}
 		}
 
-		if (sp_type == ATTITUDE || sp_type == RATES) {
+		// handle yaw
-			thrust_msg.thrust = thrust;
+		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) {
+				// 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
+				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;
+			}
 		}
 
-		if (sp_type == RATES) {
+		// handle roll
-			rates_msg.twist.angular.x = roll_rate;
+		if (isfinite(roll)) {
-			rates_msg.twist.angular.y = pitch_rate;
+			setpoint_roll = roll;
-			rates_msg.twist.angular.z = yaw_rate;
+		}
+
+		// handle pitch
+		if (isfinite(pitch)) {
+			setpoint_pitch = pitch;
+		}
+
+		// 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;
 		}
 
 		wait_armed = auto_arm;
 
-publish_setpoint:
 		publish(stamp); // calculate initial transformed messages first
 		setpoint_timer.start();
 
-		// publish target frame
+		if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL || setpoint_type == POSITION) {
-		if (!target.child_frame_id.empty()) {
+			publishTarget(stamp, true);
-			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;
@@ -788,27 +972,39 @@ publish_setpoint:
 }
 
 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, req.yaw_rate, 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, NAN, 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, req.yaw_rate, req.lat, req.lon, NAN, req.speed, req.frame_id, req.auto_arm, res.success, res.message);
+	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);
 }
 
 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, req.yaw_rate, 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, NAN, 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, req.yaw_rate, 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, NAN, 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.pitch, req.roll, 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.roll, req.pitch, 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.pitch_rate, req.roll_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.roll_rate, req.pitch_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)
@@ -840,9 +1036,7 @@ 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") {
-				res.success = true;
+				break;
-				busy = false;
-				return true;
 			}
 			if (ros::Time::now() - start > land_timeout)
 				throw std::runtime_error("Land request timed out");
@@ -851,6 +1045,18 @@ 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());
@@ -860,6 +1066,18 @@ bool land(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)
 	return false;
 }
 
+bool release(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)
+{
+	setpoint_timer.stop();
+	setpoint_type = NONE;
+	setpoint_position.header.frame_id = "";
+	setpoint_altitude.header.frame_id = "";
+	yaw_frame_id = "";
+	publishState();
+	res.success = true;
+	return true;
+}
+
 int main(int argc, char **argv)
 {
 	ros::init(argc, argv, "simple_offboard");
@@ -881,6 +1099,7 @@ 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.getParam("reference_frames", reference_frames);
 
 	// Default reference frames
@@ -916,6 +1135,12 @@ 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;
+		altitude_sub = nh.subscribe(mavros + "/altitude", 1, &handleAltitude);
+	}
+
 	// Setpoint publishers
 	position_pub = nh.advertise<PoseStamped>(mavros + "/setpoint_position/local", 1);
 	position_raw_pub = nh.advertise<PositionTarget>(mavros + "/setpoint_raw/local", 1);
@@ -924,15 +1149,22 @@ int main(int argc, char **argv)
 	rates_pub = nh.advertise<TwistStamped>(mavros + "/setpoint_attitude/cmd_vel", 1);
 	thrust_pub = nh.advertise<Thrust>(mavros + "/setpoint_attitude/thrust", 1);
 
+	// State publisher
+	state_pub = nh_priv.advertise<clover::State>("state", 1, true);
+
 	 // Service servers
 	auto gt_serv = nh.advertiseService("get_telemetry", &getTelemetry);
 	auto na_serv = nh.advertiseService("navigate", &navigate);
 	auto ng_serv = nh.advertiseService("navigate_global", &navigateGlobal);
+	auto sl_serv = nh.advertiseService("set_altitude", &setAltitude);
+	auto ya_serv = nh.advertiseService("set_yaw", &setYaw);
+	auto yr_serv = nh.advertiseService("set_yaw_rate", &setYawRate);
 	auto sp_serv = nh.advertiseService("set_position", &setPosition);
 	auto sv_serv = nh.advertiseService("set_velocity", &setVelocity);
 	auto sa_serv = nh.advertiseService("set_attitude", &setAttitude);
 	auto sr_serv = nh.advertiseService("set_rates", &setRates);
 	auto ld_serv = nh.advertiseService("land", &land);
+	auto rl_serv = nh_priv.advertiseService("release", &release);
 
 	// Setpoint timer
 	setpoint_timer = nh.createTimer(ros::Duration(1 / nh_priv.param("setpoint_rate", 30.0)), &publishSetpoint, false, false);
@@ -940,7 +1172,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

@@ -25,7 +25,7 @@
 using std::string;
 using namespace geometry_msgs;
 
-bool reset_flag = false;
+bool reset_flag = true; // offset should be reset on the start
 string local_frame_id, frame_id, child_frame_id, offset_frame_id;
 tf2_ros::Buffer tf_buffer;
 ros::Publisher vpe_pub;

clover/src/www

@@ -0,0 +1,4 @@
+#!/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 pitch
 float32 roll
+float32 pitch
 float32 yaw
-float32 pitch_rate
 float32 roll_rate
+float32 pitch_rate
 float32 yaw_rate
 float32 voltage
 float32 cell_voltage

clover/srv/Navigate.srv

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

clover/srv/NavigateGlobal.srv

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

clover/srv/SetAltitude.srv

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

clover/srv/SetAttitude.srv

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

clover/srv/SetPosition.srv

@@ -2,7 +2,6 @@ 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 pitch_rate
 float32 roll_rate
+float32 pitch_rate
 float32 yaw_rate
 float32 thrust
 bool auto_arm

clover/srv/SetVelocity.srv

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

clover/srv/SetYaw.srv

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

clover/srv/SetYawRate.srv

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

clover/test/basic.py

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

clover/test/basic.test

@@ -37,6 +37,9 @@
 
     <node name="clover_blocks" pkg="clover_blocks" type="clover_blocks" output="screen" required="true"/>
 
+    <node pkg="topic_tools" name="main_camera_throttle" type="throttle" ns="main_camera"
+        args="messages image_raw 5.0 image_raw_throttled" required="true"/>
+
     <param name="test_module" value="$(find clover)/test/basic.py"/>
     <test test-name="basic_test" pkg="ros_pytest" type="ros_pytest_runner"/>
 </launch>

clover/test/offboard.py

@@ -0,0 +1,402 @@
+import rospy
+import pytest
+from pytest import approx
+import threading
+import mavros_msgs.msg
+from geometry_msgs.msg import PoseStamped
+from clover import srv
+from clover.msg import State
+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)
+    res = navigate()
+    assert res.success == False
+    assert res.message.startswith('State timeout')
+
+    telem = get_telemetry()
+    assert telem.connected == False
+
+    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)
+
+    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 invalidate the setpoint
+    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'
+
+    # 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'

clover/test/offboard.test

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

clover/www/js/topics.js

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

clover/www/topics.html

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

clover_blocks/www/blocks.js

@@ -15,6 +15,7 @@ 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;
@@ -22,7 +23,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') {
+		if (frameId == 'BODY' || frameId == 'NAVIGATE_TARGET' || frameId == 'BASE_LINK' || frameId == 'TERRAIN') {
 			this.getInput('X').fieldRow[0].setValue('forward');
 			this.getInput('Y').fieldRow[0].setValue('left');
 			this.getInput('Z').fieldRow[0].setValue('up');
@@ -59,8 +60,8 @@ function updateSetpointBlock(e) {
 	this.getInput('VY').setVisible(velocity);
 	this.getInput('VZ').setVisible(velocity);
 	this.getInput('YAW').setVisible(attitude);
-	this.getInput('PITCH').setVisible(attitude);
 	this.getInput('ROLL').setVisible(attitude);
+	this.getInput('PITCH').setVisible(attitude);
 	this.getInput('THRUST').setVisible(attitude);
 	this.getInput('RELATIVE_TO').setVisible(type != 'RATES');
 
@@ -73,7 +74,7 @@ function updateSetpointBlock(e) {
 
 Blockly.Blocks['navigate'] = {
 	init: function () {
-		let navFrameId = frameIds.slice();
+		let navFrameId = frameIdsWithTerrain.slice();
 		navFrameId.push(['global', 'GLOBAL_LOCAL'])
 		navFrameId.push(['global, WGS 84 alt.', 'GLOBAL'])
 		this.appendDummyInput()
@@ -163,14 +164,14 @@ Blockly.Blocks['setpoint'] = {
 		this.appendValueInput("VZ")
 			.setCheck("Number")
 			.appendField("vz");
-		this.appendValueInput("PITCH")
-			.setCheck("Number")
-			.appendField("pitch")
-			.setVisible(false);
 		this.appendValueInput("ROLL")
 			.setCheck("Number")
 			.appendField("roll")
 			.setVisible(false);
+		this.appendValueInput("PITCH")
+			.setCheck("Number")
+			.appendField("pitch")
+			.setVisible(false);
 		this.appendValueInput("YAW")
 			.setCheck("Number")
 			.appendField("yaw")
@@ -213,7 +214,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(frameIds), "FRAME_ID");
+			.appendField(new Blockly.FieldDropdown(frameIdsWithTerrain), "FRAME_ID");
 		this.appendValueInput("ID")
 			.setCheck("Number")
 			.appendField("with ID")
@@ -247,7 +248,7 @@ Blockly.Blocks['get_attitude'] = {
 	init: function () {
 		this.appendDummyInput()
 			.appendField("current")
-			.appendField(new Blockly.FieldDropdown([["pitch", "PITCH"], ["roll", "ROLL"], ["pitch rate", "PITCH_RATE"], ["roll rate", "ROLL_RATE"], ["yaw rate", "YAW_RATE"]]), "FIELD");
+			.appendField(new Blockly.FieldDropdown([["roll", "ROLL"], ["pitch", "PITCH"], ["roll rate", "ROLL_RATE"], ["pitch rate", "PITCH_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).");
@@ -509,7 +510,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"]]), "FRAME_ID");
+			.appendField(new Blockly.FieldDropdown([["markers map", "ARUCO_MAP"], ["marker", "ARUCO"], ["last navigate target", "NAVIGATE_TARGET"], ["terrain", "TERRAIN"]]), "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="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="PITCH"><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>

clover_blocks/www/python.js

@@ -81,7 +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`;
+			code += `navigate_global = rospy.ServiceProxy('navigate_global', srv.NavigateGlobal)\n`;
 		}
 		if (rosDefinitions.setVelocity) {
 			code += `set_velocity = rospy.ServiceProxy('set_velocity', srv.SetVelocity)\n`;
@@ -276,10 +276,11 @@ 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 = `navigate(x=float('nan'), y=float('nan'), z=float('nan'), yaw=${yaw}, frame_id=${frameId})\n`;
+	let code = `set_yaw(yaw=${yaw}, frame_id=${frameId})\n`;
 	if (block.getFieldValue('WAIT') == 'TRUE') {
 		rosDefinitions.waitYaw = true;
 		simpleOffboard();
@@ -328,11 +329,11 @@ Blockly.Python.setpoint = function(block) {
 	} else if (type == 'ATTITUDE') {
 		rosDefinitions.setAttitude = true;
 		simpleOffboard();
-		return `set_attitude(pitch=${pitch}, roll=${roll}, yaw=${yaw}, thrust=${thrust}, frame_id=${frameId})\n`;
+		return `set_attitude(roll=${roll}, pitch=${pitch}, yaw=${yaw}, thrust=${thrust}, frame_id=${frameId})\n`;
 	} else if (type == 'RATES') {
 		rosDefinitions.setRates = true;
 		simpleOffboard();
-		return `set_rates(pitch_rate=${pitch}, roll_rate=${roll}, yaw_rate=${yaw}, thrust=${thrust})\n`;
+		return `set_rates(roll_rate=${roll}, pitch_rate=${pitch}, yaw_rate=${yaw}, thrust=${thrust})\n`;
 	}
 }
 

clover_description/urdf/sensors/distance_sensor.urdf.xacro

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

clover_simulation/README.md

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

clover_simulation/package.xml

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

clover_simulation/src/sim_leds.cpp

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

docs/en/SUMMARY.md

@@ -36,7 +36,7 @@
   * [Optical Flow](optical_flow.md)
   * [Autonomous flight (OFFBOARD)](simple_offboard.md)
   * [Coordinate systems (frames)](frames.md)
-  * [Code snippets](snippets.md)
+  * [Code examples](snippets.md)
   * [Interfacing with a laser rangefinder](laser.md)
   * [LED strip](leds.md)
   * [Working with GPIO](gpio.md)

docs/en/camera.md

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

docs/en/connection.md

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

docs/en/copterhack2023.md

@@ -8,6 +8,28 @@ To learn more about the articles of the CopterHack finalist teams follow the lin
 
 The proposed projects are supposed to be open-source and be compatible with the Clover quadcopter platform. Teams-participants are supposed to work on their projects throughout the competition, bringing them closer to the state of the finished product while being assisted by industry experts through lectures and regular feedback.
 
+## Projects of the contest's participants {#participants}
+
+|Place|Team|Project|Points|
+|:-:|-|-|-|
+||🇷🇺 Clover Cloud Team|[Clover Cloud Platform](https://github.com/DevMBS/clover/blob/clover-cloud-platform/docs/en/clover-cloud-platform.md)||
+||🇰🇬 Zavarka|[Система обмена грузами с помощью конвейера](https://github.com/aiurobotics/clover/blob/conveyance/docs/ru/conveyance.md)||
+||🇮🇳 DJS PHOENIX|[Autonomous Racing Drone](https://github.com/DJSPhoenix/clover/blob/DJSPhoenix_chetak/docs/ru/djs_phoenix_chetak.md)||
+||🇷🇺 FSOTM|[Drone Interceptor](https://github.com/deadln/clover/blob/interceptor/docs/ru/interceptor.md)||
+||🇰🇬 Homelesses|[Trash Collector](https://github.com/Isa-jesus/clover/blob/trash-collector/docs/ru/trash-collector.md)||
+||🇷🇺 Digital otters|[Digital otters](https://github.com/Mentalsupernova/clover_cool/blob/new-article.md/docs/ru/new-article.md)||
+||🇷🇺 Light Flight|[Сопровождение БПЛА при посадке](https://github.com/SirSerow/clover_inertial_ns/blob/inertial-1/Description.md)||
+||🇰🇬 LiveSavers|[LiveSavers](https://github.com/Sarvar00/clover/blob/livesavers/docs/ru/livesaver.md)||
+||🇷🇺 C305|[Система радио-навигации](https://github.com/Lukerrr/clover-c305/blob/nav_beacon/docs/ru/nav-beacon.md)||
+||🇷🇺 XenCOM|[Bound by fate](https://github.com/xenkek/clover/blob/xenkek-patch-1/docs/ru/bound_by_fate.md)||
+||🇨🇦 Clover with Motion Capture System|[Clover with Motion Capture System](https://github.com/ssmith-81/clover/blob/MoCap_Clover/docs/en/mocap_clover.md)||
+||🇧🇷 Atena|[Swarm in Blocks 2](https://github.com/Grupo-SEMEAR-USP/clover/blob/swarm_in_blocks_2/docs/en/swarm_in_blocks_2.md)||
+||🇧🇾 FTL|[Advanced Clover 2](https://github.com/FTL-team/clover/blob/FTL-advancedClover3/docs/ru/advanced_clover_simulator_platform.md)||
+||🇷🇺 Лицей №128|[Платформа для зарядки квадрокоптера](https://github.com/Juli-Shvetsova/clover/blob/liceu128-1/docs/ru/liceu128.md)||
+||🇷🇺 Ava_Clover|[DoubleClover](https://github.com/bessiaka/clover/blob/Ava_Clover/docs/ru/soosocta.md)||
+||🇷🇺 TPU_1|[Совместная транспортировка груза](https://github.com/shamoleg/clover/blob/tpu_1/docs/ru/tpu_1.md)||
+||🇷🇺 TPU_2|[Алгоритм полета сквозь лесную местность](https://github.com/shamoleg/clover/blob/tpu_2/docs/ru/tpu_2.md)| |
+
 ## CopterHack 2023 stages
 
 The qualifying and project development stages will be held in an online format, however, the final round will be in a hybrid mode (offline + online). The competition involves monthly updates from the teams with regular feedback from the jury. All teams are required to prepare a final video and presentation on the project's results to participate in the final stage.

docs/en/educational_contests.md

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

docs/en/mavros.md

@@ -40,6 +40,8 @@ Messages published in the topics may be viewed with the `rostopic` utility, e.g.
 
 `/mavros/setpoint_position/local` — set target position and yaw of the drone \(in the ENU coordinate system\).
 
+`/mavros/setpoint_position/global` – set target position in global coordinates (latitude, longitude, altitude) and yaw of the drone.
+
 `/mavros/setpoint_position/cmd_vel` — set target linear velocity of the drone.
 
 `/mavros/setpoint_attitude/attitude` and `/mavros/setpoint_attitude/att_throttle` — set target attitude and throttle level.
@@ -52,4 +54,4 @@ Messages published in the topics may be viewed with the `rostopic` utility, e.g.
 
 `/mavros/setpoint_raw/attitude` — sends [SET\_ATTITUDE\_TARGET](https://mavlink.io/en/messages/common.html#SET_ATTITUDE_TARGET) message. Allows setting the target attitude /angular velocity and throttle level. The values to be set are selected using the `type_mask` field
 
-`/mavros/setpoint_raw/global` — sends [SET\_POSITION\_TARGET\_GLOBAL\_INT](https://mavlink.io/en/messages/common.html#SET_POSITION_TARGET_GLOBAL_INT). Allows setting the target attitude in global coordinates \(latitude, longitude, altitude\) and flight speed. **Not supported in PX4** \([issue](https://github.com/PX4/Firmware/issues/7552)\).
+`/mavros/setpoint_raw/global` — sends [SET\_POSITION\_TARGET\_GLOBAL\_INT](https://mavlink.io/en/messages/common.html#SET_POSITION_TARGET_GLOBAL_INT). Allows setting the target attitude in global coordinates \(latitude, longitude, altitude\) and flight speed.

docs/en/parameters.md

@@ -60,8 +60,8 @@ The `SYS_MC_EST_GROUP` parameter defines the estimator subsystem to use.
 
 Estimator subsystem is a group of modules that calculates the current state of the copter using readings from the sensors. The copter state includes:
 
-* Angle rate of the copter – pitch_rate, roll_rate, yaw_rate;
+* Angle rate of the copter – roll_rate, pitch_rate, yaw_rate;
-* Copter orientation (in the local coordinate system) – pitch, roll, yaw (one of presentations);
+* Copter orientation (in the local coordinate system) – roll, pitch, yaw (one of presentations);
 * Copter position (in the local coordinate system) – x, y, z;
 * Copter speed (in the local coordinate system) – vx, vy, vz;
 * Global coordinates of the copter – latitude, longitude, altitude;

docs/en/simple_offboard.md

@@ -51,11 +51,11 @@ Response format:
 * `lat, lon` – drone latitude and longitude *(degrees)*, requires [GPS](gps.md) module;
 * `alt` – altitude in the global coordinate system (according to [WGS-84](https://ru.wikipedia.org/wiki/WGS_84) standard, not <abbr title="Above Mean Sea Level">AMSL</abbr>!), requires [GPS](gps.md) module;
 * `vx, vy, vz` – drone velocity *(m/s)*;
-* `pitch` – pitch angle *(radians)*;
 * `roll` – roll angle *(radians)*;
+* `pitch` – pitch angle *(radians)*;
 * `yaw` — yaw angle *(radians)*;
-* `pitch_rate` — angular pitch velocity *(rad/s)*;
 * `roll_rate` – angular roll velocity *(rad/s)*;
+* `pitch_rate` — angular pitch velocity *(rad/s)*;
 * `yaw_rate` – angular yaw velocity *(rad/s)*;
 * `voltage` – total battery voltage *(V)*;
 * `cell_voltage` – battery cell voltage *(V)*.
@@ -103,7 +103,7 @@ Parameters:
 * `yaw_rate` – angular yaw velocity (will be used if yaw is set to `NaN`) *(rad/s)*;
 * `speed` – flight speed (setpoint speed) *(m/s)*;
 * `auto_arm` – switch the drone to `OFFBOARD` mode and arm automatically (**the drone will take off**);
-* `frame_id` – [coordinate system](frames.md) for values `x`, `y`, `z`, `vx`, `vy`, `vz`. Example: `map`, `body`, `aruco_map`. Default value: `map`.
+* `frame_id` – [coordinate system](frames.md) for values `x`, `y`, `z` and `yaw`. Example: `map`, `body`, `aruco_map`. Default value: `map`.
 
 > **Note** If you don't want to change your current yaw set the `yaw` parameter to `NaN` (angular velocity by default is 0).
 
@@ -261,22 +261,22 @@ set_velocity(vx=1, vy=0.0, vz=0, frame_id='body')
 
 ### set_attitude
 
-Set pitch, roll, yaw and throttle level (similar to [the `STABILIZED` mode](modes.md)). This service may be used for lower level control of the drone behavior, or controlling the drone when no reliable data on its position is available.
+Set roll, pitch, yaw and throttle level (similar to [the `STABILIZED` mode](modes.md)). This service may be used for lower level control of the drone behavior, or controlling the drone when no reliable data on its position is available.
 
 Parameters:
 
-* `pitch`, `roll`, `yaw` – requested pitch, roll, and yaw angle *(radians)*;
+* `roll`, `pitch`, `yaw` – requested roll, pitch, and yaw angle *(radians)*;
 * `thrust` — throttle level, ranges from 0 (no throttle, propellers are stopped) to 1 (full throttle).
 * `auto_arm` – switch the drone to `OFFBOARD` mode and arm automatically (**the drone will take off**);
 * `frame_id` – [coordinate system](frames.md) for `yaw` (Default value: `map`).
 
 ### set_rates
 
-Set pitch, roll, and yaw rates and the throttle level (similar to [the `ACRO` mode](modes.md)). This is the lowest drone control level (excluding direct control of motor rotation speed). This service may be used to automatically perform aerobatic tricks (e.g., flips).
+Set roll, pitch, and yaw rates and the throttle level (similar to [the `ACRO` mode](modes.md)). This is the lowest drone control level (excluding direct control of motor rotation speed). This service may be used to automatically perform aerobatic tricks (e.g., flips).
 
 Parameters:
 
-* `pitch_rate`, `roll_rate`, `yaw_rate` – pitch, roll, and yaw rates *(rad/s)*;
+* `roll_rate`, `pitch_rate`, `yaw_rate` – pitch, roll, and yaw rates *(rad/s)*;
 * `thrust` — throttle level, ranges from 0 (no throttle, propellers are stopped) to 1 (full throttle).
 * `auto_arm` – switch the drone to `OFFBOARD` and arm automatically (**the drone will take off**);
 
@@ -305,6 +305,16 @@ rosservice call /land "{}"
 
 > **Caution** In recent PX4 versions, the vehicle will be switched out of LAND mode to manual mode, if the remote control sticks are moved significantly.
 
+### release
+
+If it's necessary to pause sending setpoint messages, use the `simple_offboard/release` service:
+
+```python
+release = rospy.ServiceProxy('simple_offboard/release', Trigger)
+
+release()
+```
+
 ## Additional materials
 
 * [ArUco-based position estimation and navigation](aruco.md).

docs/en/simulation_native.md

@@ -2,7 +2,7 @@
 
 Setting up the simulation environment from scratch requires some effort, but results in the most performant setup, with less chance of driver issues.
 
-> **Hint** See up-to-date commands set for installation Clover simulation software in the script, that builds the virtual machine image with the simulator: [`install_software.sh`](https://github.com/CopterExpress/clover_vm/blob/master/scripts/install_software.sh).
+<!-- > **Hint** See up-to-date commands set for installation Clover simulation software in the script, that builds the virtual machine image with the simulator: [`install_software.sh`](https://github.com/CopterExpress/clover_vm/blob/master/scripts/install_software.sh). -->
 
 Prerequisites: **Ubuntu 20.04**.
 
@@ -66,7 +66,7 @@ PX4 will be built along with the other packages in our workspace. You may clone
 Clone PX4 sources and make the required symlinks:
 
 ```bash
-git clone --recursive --depth 1 --branch v1.12.0 https://github.com/PX4/PX4-Autopilot.git ~/PX4-Autopilot
+git clone --recursive --depth 1 --branch v1.12.3 https://github.com/PX4/PX4-Autopilot.git ~/PX4-Autopilot
 ln -s ~/PX4-Autopilot ~/catkin_ws/src/
 ln -s ~/PX4-Autopilot/Tools/sitl_gazebo ~/catkin_ws/src/
 ln -s ~/PX4-Autopilot/mavlink ~/catkin_ws/src/
@@ -147,6 +147,8 @@ sudo systemctl enable roscore
 sudo systemctl start roscore
 ```
 
+### Web tools setup
+
 Install any web server to serve Clover's web tools (`~/.ros/www` directory), e. g. Monkey:
 
 ```bash
@@ -158,3 +160,11 @@ sudo cp ~/catkin_ws/src/clover/builder/assets/monkey.service /etc/systemd/system
 sudo systemctl enable monkey
 sudo systemctl start monkey
 ```
+
+Create `~/.ros/www` using the following command:
+
+```bash
+rosrun clover www
+```
+
+If the set of packages containing a web part (through `www` directory) is changed, the above command also must be run.

docs/en/simulation_usage.md

@@ -97,3 +97,13 @@ PX4_SIM_SPEED_FACTOR=0.42 roslaunch clover_simulation simulator.launch
 The virtual machine may benefit from several CPU cores, especially if the cores are not very performant. In our tests, a four-core machine with only a single core allocated to the VM was unable to run the simulation, with constant interface freezes and dropped ROS messages. The same machine with all four cores available to the VM was able to run the simulation at 0.25 real-time speed.
 
 Do note that you should not allocate more resources than you have on your host hardware.
+
+### Changing the map of ArUco-markers in the simulator
+
+In order to change the map of ArUco-markers in the simulator, you can use the following command:
+
+```bash
+rosrun clover_simulation aruco_gen --single-model --source-world=$(catkin_find clover_simulation resources/worlds/clover.world) $(catkin_find aruco_pose map/map.txt) > $(catkin_find clover_simulation resources/worlds/clover_aruco.world)
+```
+
+In this example, `map.txt` is the name of markers name.

docs/en/snippets.md

@@ -144,7 +144,7 @@ Determine whether the copter is turned upside-down:
 PI_2 = math.pi / 2
 telem = get_telemetry()
 
-flipped = abs(telem.pitch) > PI_2 or abs(telem.roll) > PI_2
+flipped = abs(telem.roll) > PI_2 or abs(telem.pitch) > PI_2
 ```
 
 ### # {#angle-hor}
@@ -155,8 +155,8 @@ Calculate the copter horizontal angle:
 PI_2 = math.pi / 2
 telem = get_telemetry()
 
-flipped = not -PI_2 <= telem.pitch <= PI_2 or not -PI_2 <= telem.roll <= PI_2
+flipped = not -PI_2 <= telem.roll <= PI_2 or not -PI_2 <= telem.pitch <= PI_2
-angle_to_horizon = math.atan(math.hypot(math.tan(telem.pitch), math.tan(telem.roll)))
+angle_to_horizon = math.atan(math.hypot(math.tan(telem.roll), math.tan(telem.pitch)))
 if flipped:
     angle_to_horizon = math.pi - angle_to_horizon
 ```
@@ -207,9 +207,9 @@ def pose_update(pose):
     # Processing new data of copter's position
     pass
 
-rospy.Subscriber('/mavros/local_position/pose', PoseStamped, pose_update)
+rospy.Subscriber('mavros/local_position/pose', PoseStamped, pose_update)
-rospy.Subscriber('/mavros/local_position/velocity', TwistStamped, velocity_update)
+rospy.Subscriber('mavros/local_position/velocity', TwistStamped, velocity_update)
-rospy.Subscriber('/mavros/battery', BatteryState, battery_update)
+rospy.Subscriber('mavros/battery', BatteryState, battery_update)
 rospy.Subscriber('mavros/rc/in', RCIn, rc_callback)
 
 rospy.spin()
@@ -240,6 +240,30 @@ ros_msg = mavlink.convert_to_rosmsg(msg)
 mavlink_pub.publish(ros_msg)
 ```
 
+<!-- markdownlint-disable MD044 -->
+
+### # {#mavlink-receive}
+
+<!-- markdownlint-enable MD044 -->
+
+Subscribe to all MAVLink messages from the flight controller and decode them:
+
+```python
+from mavros_msgs.msg import Mavlink
+from mavros import mavlink
+from pymavlink import mavutil
+
+link = mavutil.mavlink.MAVLink('', 255, 1)
+
+def mavlink_cb(msg):
+    mav_msg = link.decode(mavlink.convert_to_bytes(msg))
+    print('msgid =', msg.msgid, mav_msg) # print message id and parsed message
+
+mavlink_sub = rospy.Subscriber('mavlink/from', Mavlink, mavlink_cb)
+
+rospy.spin()
+```
+
 ### # {#rc-sub}
 
 React to the drone's mode switching (may be used for starting an autonomous flight, see [example](https://gist.github.com/okalachev/b709f04522d2f9af97e835baedeb806b)):
@@ -300,7 +324,7 @@ def flip():
 
     while True:
         telem = get_telemetry()
-        flipped = abs(telem.pitch) > PI_2 or abs(telem.roll) > PI_2
+        flipped = abs(telem.roll) > PI_2 or abs(telem.pitch) > PI_2
         if flipped:
             break
 
@@ -325,7 +349,7 @@ from pymavlink import mavutil
 from mavros_msgs.srv import CommandLong
 from mavros_msgs.msg import State
 
-send_command = rospy.ServiceProxy('/mavros/cmd/command', CommandLong)
+send_command = rospy.ServiceProxy('mavros/cmd/command', CommandLong)
 
 def calibrate_gyro():
     rospy.loginfo('Calibrate gyro')
@@ -456,3 +480,11 @@ param_set(param_id='COM_FLTMODE1', value=ParamValue(integer=8))
 # Set parameter of type FLOAT:
 param_set(param_id='MPC_Z_P', value=ParamValue(real=1.5))
 ```
+
+### # {#is-simulation}
+
+Check, if the code is running inside a [Gazebo simulation](simulation.md):
+
+```python
+is_simulation = rospy.get_param('/use_sim_time', False)
+```

docs/en/wall_aruco.md

@@ -49,10 +49,10 @@ If you are using the marker map, where the markers have equal distances along th
 
 After you fill out the map, you need to apply it. To do it, edit the file `aruco.launch`, located in `~/catkin_ws/src/clover/clover/launch/`. Change the line `<param name="map" value="$(find aruco_pose)/map/map_name.txt"/>`, where `map_name.txt` is the name of your map file.
 
-If you are using markers that are not linked to horizontal surfaces (floor, ceiling), you must disable the parameter `known_tilt` both in the module `aruco_detect` and `aruco_map` in the same file. To do it automatically, enter:
+If you are using markers that are not linked to horizontal surfaces (floor, ceiling), you must blank the `placement` argument in the same file:
 
-```bash
+```xml
-sed -i "/known_tilt/s/value=\".*\"/value=\"\"/" /home/pi/catkin_ws/src/clover/clover/launch/aruco.launch
+<arg name="placement" default=""/>
 ```
 
 After all the settings, call `sudo systemctl restart clover` to restart the `clover` service.

docs/ru/camera.md

@@ -1,7 +1,5 @@
 # Работа с камерой
 
-> **Note** В версии образа **0.20** пакет и сервис `clever` был переименован в `clover`. Для более ранних версий см. документацию для версии [**0.19**](https://github.com/CopterExpress/clover/blob/v0.19/docs/ru/camera.md).
-
 <!-- TODO: физическое подключение -->
 
 Для работы с основной камерой необходимо убедиться что она включена в файле `~/catkin_ws/src/clover/clover/launch/clover.launch`:
@@ -56,8 +54,6 @@ raspistill -o test.jpg
 
 ### Python
 
-Основная статья: http://wiki.ros.org/cv_bridge/Tutorials/ConvertingBetweenROSImagesAndOpenCVImagesPython.
-
 Пример создания подписчика на топик с изображением с основной камеры для обработки с использованием OpenCV:
 
 ```python
@@ -65,12 +61,14 @@ import rospy
 import cv2
 from sensor_msgs.msg import Image
 from cv_bridge import CvBridge
+from clover import long_callback
 
-rospy.init_node('computer_vision_sample')
+rospy.init_node('cv')
 bridge = CvBridge()
 
+@long_callback
 def image_callback(data):
-    cv_image = bridge.imgmsg_to_cv2(data, 'bgr8')  # OpenCV image
+    img = bridge.imgmsg_to_cv2(data, 'bgr8')  # OpenCV image
     # Do any image processing with cv2...
 
 image_sub = rospy.Subscriber('main_camera/image_raw', Image, image_callback)
@@ -78,19 +76,31 @@ image_sub = rospy.Subscriber('main_camera/image_raw', Image, image_callback)
 rospy.spin()
 ```
 
+> **Note** Обработка изображения может занимать значительное время. Это может вызвать [проблему](https://github.com/ros/ros_comm/issues/1901) в библиотеке rospy, которая приведет к обработке устаревших кадров с камеры. Для решения этой проблемы необходимо использовать декоратор `long_callback` из библиотеки `clover`, как в примере выше.
+
+#### Ограничение использования CPU
+
+При использовании топика `main_camera/image_raw` скрипт будет обрабатывать максимальное количество кадров с камеры, активно используя CPU (вплоть до 100%). В задачах, где обработка каждого кадра не критична, можно использовать топик, где кадры публикуются с частотой 5 Гц: `main_camera/image_raw_throttled`:
+
+```python
+image_sub = rospy.Subscriber('main_camera/image_raw_throttled', Image, image_callback, queue_size=1)
+```
+
+#### Публикация изображений
+
 Для отладки обработки изображения можно публиковать отдельный топик с обработанным изображением:
 
 ```python
 image_pub = rospy.Publisher('~debug', Image)
 ```
 
-Публикация обработанного изображения (в конце функции image_callback):
+Публикация обработанного изображения:
 
 ```python
-image_pub.publish(bridge.cv2_to_imgmsg(cv_image, 'bgr8'))
+image_pub.publish(bridge.cv2_to_imgmsg(img, 'bgr8'))
 ```
 
-Получаемые изображения можно просматривать используя [web_video_server](web_video_server.md).
+Получаемые изображения можно просматривать используя [web_video_server](web_video_server.md) или [rqt](rviz.md).
 
 #### Получение одного кадра
 
@@ -101,12 +111,12 @@ import rospy
 from sensor_msgs.msg import Image
 from cv_bridge import CvBridge
 
-rospy.init_node('computer_vision_sample')
+rospy.init_node('cv')
 bridge = CvBridge()
 
 # ...
 
-# Получение кадра:
+# Retrieve a frame:
 img = bridge.imgmsg_to_cv2(rospy.wait_for_message('main_camera/image_raw', Image), 'bgr8')
 ```
 
@@ -123,38 +133,32 @@ img = bridge.imgmsg_to_cv2(rospy.wait_for_message('main_camera/image_raw', Image
 ```python
 import rospy
 from pyzbar import pyzbar
+import cv2
 from cv_bridge import CvBridge
 from sensor_msgs.msg import Image
+from clover import long_callback
 
+rospy.init_node('cv')
 bridge = CvBridge()
 
-rospy.init_node('barcode_test')
+@long_callback
-
+def image_callback(msg):
-# Image subscriber callback function
+    img = bridge.imgmsg_to_cv2(msg, 'bgr8')
-def image_callback(data):
+    barcodes = pyzbar.decode(img)
-    cv_image = bridge.imgmsg_to_cv2(data, 'bgr8')  # OpenCV image
-    barcodes = pyzbar.decode(cv_image)
     for barcode in barcodes:
-        b_data = barcode.data.decode("utf-8")
+        b_data = barcode.data.decode('utf-8')
         b_type = barcode.type
         (x, y, w, h) = barcode.rect
         xc = x + w/2
         yc = y + h/2
-        print("Found {} with data {} with center at x={}, y={}".format(b_type, b_data, xc, yc))
+        print('Found {} with data {} with center at x={}, y={}'.format(b_type, b_data, xc, yc))
 
-image_sub = rospy.Subscriber('main_camera/image_raw', Image, image_callback, queue_size=1)
+image_sub = rospy.Subscriber('main_camera/image_raw_throttled', Image, image_callback, queue_size=1)
 
 rospy.spin()
 ```
 
-Скрипт будет занимать 100% процессора. Для искусственного замедления работы скрипта можно запустить [throttling](http://wiki.ros.org/topic_tools/throttle) кадров с камеры, например, в 5 Гц (`main_camera.launch`):
+> **Hint** Смотрите другие примеры по работе с компьютерным зрением в каталоге `~/examples` [образа для RPi](image.md).
-
-```xml
-<node pkg="topic_tools" name="cam_throttle" type="throttle"
-    args="messages main_camera/image_raw 5.0 main_camera/image_raw_throttled"/>
-```
-
-Топик для подписчика в этом случае необходимо поменять на `main_camera/image_raw_throttled`.
 
 ## Запись видео
 

docs/ru/connection.md

@@ -20,15 +20,14 @@
 
 ## Подключение по UART
 
-> **Note** В версии образа **0.20** пакет и сервис `clever` был переименован в `clover`. Для более ранних версий см. документацию для версии [**0.19**](https://github.com/CopterExpress/clover/blob/v0.19/docs/ru/connection.md).
-
 <!-- TODO схема подключения -->
 
 Дополнительным способом подключения является подключение подключение по интерфейсу UART.
 
-1. Подключите Raspberry Pi к полетному контроллеру по UART.
+1. Подключите Raspberry Pi к полетному контроллеру по UART. Для этого соедините кабелем порт TELEM 2 на полетном контроллере к пинам на Raspberry Pi следующем образом: черный провод (GND) к Ground, зеленый (*UART_RX*) к *GPIO14*, желтый (*UART_TX*) к *GPIO15*. Красный провод (*5V*) подключать не нужно.
-2. [Подключитесь в Raspberry Pi по SSH](ssh.md).
+2. Измените значения параметров PX4: `MAV_1_CONFIG` на TELEM 2, `SER_TEL2_BAUND` на 921600 8N1. В PX4 до версии v1.10.0 необходима установка параметра `SYS_COMPANION` в значение 921600.
-3. Поменяйте в launch-файле Клевера (`~/catkin_ws/src/clover/clover/launch/clover.launch`) тип подключения на UART:
+3. [Подключитесь в Raspberry Pi по SSH](ssh.md).
+4. Поменяйте в launch-файле Клевера (`~/catkin_ws/src/clover/clover/launch/clover.launch`) тип подключения на UART:
 
     ```xml
     <arg name="fcu_conn" default="uart"/>
@@ -40,15 +39,4 @@
     sudo systemctl restart clover
     ```
 
-> **Hint** Для корректной работы подключения Raspberry Pi и полетного контроллера по UART необходимо установить значение параметра `SYS_COMPANION` на 921600.
-
-## Подключение к SITL
-
-Для того, чтобы подсоединиться к локально/удаленно запущенному [SITL](sitl.md), необходимо установить аргумент `fcu_conn` в `udp`, и `fcu_ip` в IP-адрес машины, где запущен SITL (`127.0.0.1` для локального):
-
-```xml
-<arg name="fcu_conn" default="udp"/>
-<arg name="fcu_ip" default="127.0.0.1"/>
-```
-
 **Далее**: [Подключение QGroundControl по Wi-Fi](gcs_bridge.md).

docs/ru/copterhack2023.md

@@ -8,6 +8,28 @@ CopterHack 2023 — это международный конкурс по ра
 
 На конкурс принимаются проекты с открытым исходным кодом и совместимые с платформой квадрокоптера "Клевер". На протяжении конкурса команды работают на собственными идеями и разработками, приближая их к состоянию готового продукта. В этом участникам помогают эксперты отрасли через лекции и регулярную обратную связь.
 
+## Проекты участников конкурса {#participants}
+
+|Место|Команда|Проект|Балл|
+|:-:|-|-|-|
+||🇷🇺 Clover Cloud Team|[Clover Cloud Platform](https://github.com/DevMBS/clover/blob/clover-cloud-platform/docs/en/clover-cloud-platform.md)||
+||🇰🇬 Zavarka|[Система обмена грузами с помощью конвейера](https://github.com/aiurobotics/clover/blob/conveyance/docs/ru/conveyance.md)||
+||🇮🇳 DJS PHOENIX|[Autonomous Racing Drone](https://github.com/DJSPhoenix/clover/blob/DJSPhoenix_chetak/docs/ru/djs_phoenix_chetak.md)||
+||🇷🇺 FSOTM|[Дрон-перехватчик](https://github.com/deadln/clover/blob/interceptor/docs/ru/interceptor.md)||
+||🇰🇬 Бездомные|[Дрон-бездомный](https://github.com/Isa-jesus/clover/blob/trash-collector/docs/ru/trash-collector.md)||
+||🇷🇺 Digital otters|[Digital otters](https://github.com/Mentalsupernova/clover_cool/blob/new-article.md/docs/ru/new-article.md)||
+||🇷🇺 Light Flight|[Сопровождение БПЛА при посадке](https://github.com/SirSerow/clover_inertial_ns/blob/inertial-1/Description.md)||
+||🇰🇬 LiveSavers|[LiveSavers](https://github.com/Sarvar00/clover/blob/livesavers/docs/ru/livesaver.md)||
+||🇷🇺 C305|[Система радио-навигации](https://github.com/Lukerrr/clover-c305/blob/nav_beacon/docs/ru/nav-beacon.md)||
+||🇷🇺 XenCOM|[Bound by fate](https://github.com/xenkek/clover/blob/xenkek-patch-1/docs/ru/bound_by_fate.md)||
+||🇨🇦 Clover with Motion Capture System|[Clover with Motion Capture System](https://github.com/ssmith-81/clover/blob/MoCap_Clover/docs/en/mocap_clover.md)||
+||🇧🇷 Atena|[Swarm in Blocks 2](https://github.com/Grupo-SEMEAR-USP/clover/blob/swarm_in_blocks_2/docs/en/swarm_in_blocks_2.md)||
+||🇧🇾 FTL|[Advanced Clover 2](https://github.com/FTL-team/clover/blob/FTL-advancedClover3/docs/ru/advanced_clover_simulator_platform.md)||
+||🇷🇺 Лицей №128|[Платформа для зарядки квадрокоптера](https://github.com/Juli-Shvetsova/clover/blob/liceu128-1/docs/ru/liceu128.md)||
+||🇷🇺 Ava_Clover|[DoubleClover](https://github.com/bessiaka/clover/blob/Ava_Clover/docs/ru/soosocta.md)||
+||🇷🇺 TPU_1|[Совместная транспортировка груза](https://github.com/shamoleg/clover/blob/tpu_1/docs/ru/tpu_1.md)||
+||🇷🇺 TPU_2|[Алгоритм полета сквозь лесную местность](https://github.com/shamoleg/clover/blob/tpu_2/docs/ru/tpu_2.md)| |
+
 ## Этапы CopterHack 2023
 
 Отборочный и проектный этапы конкурса проходят в онлайн-формате, формат проведения финала – гибридный (оффлайн + онлайн). Конкурс подразумевает ежемесячные апдейты от команд с получением регулярной обратной связи от жюри. Для участия в заключительном этапе необходимо подготовить финальное видео и презентацию о результатах проекта.

docs/ru/educational_contests.md

@@ -26,7 +26,7 @@
 
 Прием заявок осуществляется через [Google Форму](https://docs.google.com/forms/d/e/1FAIpQLScE2kN5dO2OYNSM8hOYzOa5Qvh2uDdd9Fjx8OnL1W93bfEBgw/viewform).
 
-Дедлайн подачи заявок: 1 сентября 2022 года.
+Дедлайн подачи заявок: 30 ноября 2022 года.
 
 ### Призы
 
@@ -64,7 +64,7 @@
 
 Прием заявок осуществляется через [Google Форму](https://docs.google.com/forms/d/e/1FAIpQLSdelVy6yQ1iN6u88KeiEIKGj7gGaM0xccSt2tiYKB46ICmjkQ/viewform).
 
-Дедлайн подачи заявок: 1 сентября 2022 года.
+Дедлайн подачи заявок: 30 ноября 2022 года.
 
 ### Призы
 
@@ -106,7 +106,7 @@
 
 Прием заявок осуществляется через [Google Форму](https://docs.google.com/forms/d/e/1FAIpQLSdf2Q68X4hPnFE9f3EP95AxPNnzHKqIsFHtTRT6EBKiH93wzg/viewform).
 
-Дедлайн подачи заявок: 1 сентября 2022 года
+Дедлайн подачи заявок: 30 ноября 2022 года
 
 ### Призы
 

docs/ru/mavros.md

@@ -40,6 +40,8 @@ MAVROS подписывается на определенные ROS-топики
 
 `/mavros/setpoint_position/local` — установить целевую позицию  и рысканье \(yaw\) беспилотника \(в системе координат ENU\).
 
+`/mavros/setpoint_position/global` – установить целевую позицию в глобальных координатах (ширина, долгота и высота) и рысканье беспилотника.
+
 `/mavros/setpoint_velocity/cmd_vel` — установить целевую линейную скорость беспилотника.
 
 `/mavros/setpoint_attitude/attitude` и `/mavros/setpoint_attitude/att_throttle` — установить целевую ориентацию \(Attitude\) и уровень газа.
@@ -52,4 +54,4 @@ MAVROS подписывается на определенные ROS-топики
 
 `/mavros/setpoint_raw/attitude` — отправка пакета [SET\_ATTITUDE\_TARGET](https://mavlink.io/en/messages/common.html#SET_ATTITUDE_TARGET). Позволяет установить целевую ориентацию / угловые скорости и уровень газа. Выбор устанавливаемых величин осуществляется с помощью поля `type_mask`
 
-`/mavros/setpoint_raw/global` — отправка пакета [SET\_POSITION\_TARGET\_GLOBAL\_INT](https://mavlink.io/en/messages/common.html#SET_POSITION_TARGET_GLOBAL_INT). Позволяет установить целевую позицию в глобальных координатах \(ширина, долгота, высота\), а также скорости полета. **Не поддерживается в PX4** \([issue](https://github.com/PX4/Firmware/issues/7552)\).
+`/mavros/setpoint_raw/global` — отправка пакета [SET\_POSITION\_TARGET\_GLOBAL\_INT](https://mavlink.io/en/messages/common.html#SET_POSITION_TARGET_GLOBAL_INT). Позволяет установить целевую позицию в глобальных координатах \(ширина, долгота, высота\), а также скорости полета.

docs/ru/parameters.md

@@ -60,8 +60,8 @@
 
 Estimator это подсистема, которая вычисляет текущее состояние (state) коптера, используя показания с датчиков. В состояние коптера входит:
 
-* угловая скорость коптера – pitch_rate, roll_rate, yaw_rate;
+* угловая скорость коптера – roll_rate, pitch_rate, yaw_rate;
-* ориентация коптера (в локальной системе координат) – pitch (тангаж), roll (крен), yaw (рысканье) (одно из представлений);
+* ориентация коптера (в локальной системе координат) – roll (крен), pitch (тангаж), yaw (рысканье) (одно из представлений);
 * позиция коптера (в локальной системе координат) – x, y, z;
 * скорость коптера (в локальной системе координат) – vx, vy, vz;
 * глобальные координаты коптера – latitude, longitude, altitude;

docs/ru/simple_offboard.md

@@ -51,11 +51,11 @@ land = rospy.ServiceProxy('land', Trigger)
 * `lat, lon` – широта, долгота *(градусы)*, необходимо наличие [GPS](gps.md);
 * `alt` – высота в глобальной системе координат (стандарт [WGS-84](https://ru.wikipedia.org/wiki/WGS_84), не <abbr title="Above Mean Sea Level, выше среднего уровня моря">AMSL</abbr>!), необходимо наличие [GPS](gps.md);
 * `vx, vy, vz` – скорость коптера *(м/с)*;
-* `pitch` – угол по тангажу *(радианы)*;
 * `roll` – угол по крену *(радианы)*;
+* `pitch` – угол по тангажу *(радианы)*;
 * `yaw` – угол по рысканью *(радианы)*;
-* `pitch_rate` – угловая скорость по тангажу *(рад/с)*;
 * `roll_rate` – угловая скорость по крену *(рад/с)*;
+* `pitch_rate` – угловая скорость по тангажу *(рад/с)*;
 * `yaw_rate` – угловая скорость по рысканью *(рад/с)*;
 * `voltage` – общее напряжение аккумулятора *(В)*;
 * `cell_voltage` – напряжение аккумулятора на ячейку *(В)*.
@@ -265,7 +265,7 @@ set_velocity(vx=1, vy=0.0, vz=0, frame_id='body')
 
 Параметры:
 
-* `pitch`, `roll`, `yaw` – необходимый угол по тангажу, крену и рысканью *(радианы)*;
+* `roll`, `pitch`, `yaw` – необходимый угол по тангажу, крену и рысканью *(радианы)*;
 * `thrust` – уровень газа от 0 (нет газа, пропеллеры остановлены) до 1 (полный газ);
 * `auto_arm` – перевести коптер в `OFFBOARD` и заармить автоматически (**коптер взлетит**);
 * `frame_id` – [система координат](frames.md), в которой задан `yaw` (по умолчанию: `map`).
@@ -276,7 +276,7 @@ set_velocity(vx=1, vy=0.0, vz=0, frame_id='body')
 
 Параметры:
 
-* `pitch_rate`, `roll_rate`, `yaw_rate` – угловая скорость по тангажу, крену и рыканью *(рад/с)*;
+* `roll_rate`, `pitch_rate`, `yaw_rate` – угловая скорость по тангажу, крену и рыканью *(рад/с)*;
 * `thrust` – уровень газа от 0 (нет газа, пропеллеры остановлены) до 1 (полный газ).
 * `auto_arm` – перевести коптер в `OFFBOARD` и заармить автоматически (**коптер взлетит**);
 
@@ -305,6 +305,16 @@ rosservice call /land "{}"
 
 > **Caution** В более новых версиях PX4 коптер выйдет из режима LAND в ручной режим, если сильно перемещать стики.
 
+### release
+
+В случае необходимости приостановки отправки setpoint-сообщений, используйте сервис `simple_offboard/release`:
+
+```python
+release = rospy.ServiceProxy('simple_offboard/release', Trigger)
+
+release()
+```
+
 ## Дополнительные материалы
 
 * [Полеты в поле ArUco-маркеров](aruco.md).

docs/ru/simulation_native.md

@@ -2,7 +2,7 @@
 
 Настройка среды для симуляции с нуля требует некоторых усилий, однако это приведет к улучшению производительности и к уменьшению вероятности появления проблем с драйверами.
 
-> **Hint** Смотрите актуальный набор команд установки необходимого ПО для запуска симулятора Клевера в скрипте сборки виртуальной машины с симулятором: [`install_software.sh`](https://github.com/CopterExpress/clover_vm/blob/master/scripts/install_software.sh).
+<!-- > **Hint** Смотрите актуальный набор команд установки необходимого ПО для запуска симулятора Клевера в скрипте сборки виртуальной машины с симулятором: [`install_software.sh`](https://github.com/CopterExpress/clover_vm/blob/master/scripts/install_software.sh). -->
 
 Требования для сборки: **Ubuntu 20.04**.
 
@@ -66,7 +66,7 @@ sudo /usr/bin/python3 -m pip install -r ~/catkin_ws/src/clover/clover/requiremen
 Склонируйте исходный код PX4 и создайте необходимые симлинки:
 
 ```bash
-git clone --recursive --depth 1 --branch v1.12.0 https://github.com/PX4/PX4-Autopilot.git ~/PX4-Autopilot
+git clone --recursive --depth 1 --branch v1.12.3 https://github.com/PX4/PX4-Autopilot.git ~/PX4-Autopilot
 ln -s ~/PX4-Autopilot ~/catkin_ws/src/
 ln -s ~/PX4-Autopilot/Tools/sitl_gazebo ~/catkin_ws/src/
 ln -s ~/PX4-Autopilot/mavlink ~/catkin_ws/src/
@@ -147,6 +147,8 @@ sudo systemctl enable roscore
 sudo systemctl start roscore
 ```
 
+### Конфигурация веб-инструментов
+
 Установите любой веб-сервер, чтобы раздавать веб-инструменты Клевера (директория `~/.ros/www`), например, Monkey:
 
 ```bash
@@ -158,3 +160,11 @@ sudo cp ~/catkin_ws/src/clover/builder/assets/monkey.service /etc/systemd/system
 sudo systemctl enable monkey
 sudo systemctl start monkey
 ```
+
+Создайте директорию `~/.ros/www` следующей командой:
+
+```bash
+rosrun clover www
+```
+
+При обновлении набора пакетов, содержащих веб-часть (через каталог `www`), также необходимо выполнение данной команды.

docs/ru/simulation_usage.md

@@ -99,3 +99,13 @@ PX4_SIM_SPEED_FACTOR=0.42 roslaunch clover_simulation simulator.launch
 Выделение нескольких процессорных ядер для виртуальной машины может значительно повысить производительность симуляции. В наших испытаниях виртуальная машина, для которой было выделено одно ядро, не позволяла работать в симуляторе: окно Gazebo не реагировало на пользовательский ввод, сообщения ROS терялись. После выделения четырёх ядер для этой же виртуальной машины симуляция стала работать со скоростью 0.25 от реального времени.
 
 При этом не следует пытаться выделить для виртуальной машины больше ресурсов, чем доступно на основной системе.
+
+### Изменение карты ArUco-меток в симуляторе
+
+Для того, чтобы изменить карту ArUco-меток в симуляторе, можно использовать следующую команду:
+
+```bash
+rosrun clover_simulation aruco_gen --single-model --source-world=$(catkin_find clover_simulation resources/worlds/clover.world) $(catkin_find aruco_pose map/map.txt) > $(catkin_find clover_simulation resources/worlds/clover_aruco.world)
+```
+
+В данном примере `map.txt` – имя карты меток.

docs/ru/snippets.md

@@ -154,7 +154,7 @@ new_pose = tf_buffer.transform(pose, frame_id, transform_timeout)
 PI_2 = math.pi / 2
 telem = get_telemetry()
 
-flipped = abs(telem.pitch) > PI_2 or abs(telem.roll) > PI_2
+flipped = abs(telem.roll) > PI_2 or abs(telem.pitch) > PI_2
 ```
 
 ### # {#angle-hor}
@@ -165,7 +165,7 @@ flipped = abs(telem.pitch) > PI_2 or abs(telem.roll) > PI_2
 PI_2 = math.pi / 2
 telem = get_telemetry()
 
-flipped = not -PI_2 <= telem.pitch <= PI_2 or not -PI_2 <= telem.roll <= PI_2
+flipped = not -PI_2 <= telem.roll <= PI_2 or not -PI_2 <= telem.pitch <= PI_2
 angle_to_horizon = math.atan(math.hypot(math.tan(telem.pitch), math.tan(telem.roll)))
 if flipped:
     angle_to_horizon = math.pi - angle_to_horizon
@@ -217,9 +217,9 @@ def pose_update(pose):
     # Обработка новых данных о позиции коптера
     pass
 
-rospy.Subscriber('/mavros/local_position/pose', PoseStamped, pose_update)
+rospy.Subscriber('mavros/local_position/pose', PoseStamped, pose_update)
-rospy.Subscriber('/mavros/local_position/velocity', TwistStamped, velocity_update)
+rospy.Subscriber('mavros/local_position/velocity', TwistStamped, velocity_update)
-rospy.Subscriber('/mavros/battery', BatteryState, battery_update)
+rospy.Subscriber('mavros/battery', BatteryState, battery_update)
 rospy.Subscriber('mavros/rc/in', RCIn, rc_callback)
 
 rospy.spin()
@@ -251,6 +251,30 @@ ros_msg = mavlink.convert_to_rosmsg(msg)
 mavlink_pub.publish(ros_msg)
 ```
 
+<!-- markdownlint-disable MD044 -->
+
+### # {#mavlink-receive}
+
+<!-- markdownlint-enable MD044 -->
+
+Подписка на все MAVLink-сообщения от полетного контроллера и их декодирование:
+
+```python
+from mavros_msgs.msg import Mavlink
+from mavros import mavlink
+from pymavlink import mavutil
+
+link = mavutil.mavlink.MAVLink('', 255, 1)
+
+def mavlink_cb(msg):
+    mav_msg = link.decode(mavlink.convert_to_bytes(msg))
+    print('msgid =', msg.msgid, mav_msg) # print message id and parsed message
+
+mavlink_sub = rospy.Subscriber('mavlink/from', Mavlink, mavlink_cb)
+
+rospy.spin()
+```
+
 ### # {#rc-sub}
 
 Реакция на переключение режима на пульте радиоуправления (может быть использовано для запуска автономного полета, см. [пример](https://gist.github.com/okalachev/b709f04522d2f9af97e835baedeb806b)):
@@ -311,7 +335,7 @@ def flip():
 
     while True:
         telem = get_telemetry()
-        flipped = abs(telem.pitch) > PI_2 or abs(telem.roll) > PI_2
+        flipped = abs(telem.roll) > PI_2 or abs(telem.pitch) > PI_2
         if flipped:
             break
 
@@ -336,7 +360,7 @@ from pymavlink import mavutil
 from mavros_msgs.srv import CommandLong
 from mavros_msgs.msg import State
 
-send_command = rospy.ServiceProxy('/mavros/cmd/command', CommandLong)
+send_command = rospy.ServiceProxy('mavros/cmd/command', CommandLong)
 
 def calibrate_gyro():
     rospy.loginfo('Calibrate gyro')
@@ -467,3 +491,11 @@ param_set(param_id='COM_FLTMODE1', value=ParamValue(integer=8))
 # Изменить параметр типа FLOAT:
 param_set(param_id='MPC_Z_P', value=ParamValue(real=1.5))
 ```
+
+### # {#is-simulation}
+
+Проверить, что код запущен в [симуляции Gazebo](simulation.md):
+
+```python
+is_simulation = rospy.get_param('/use_sim_time', False)
+```

docs/ru/wall_aruco.md

@@ -51,10 +51,10 @@ sed -i "/direction_y/s/default=\".*\"/default=\"\"/" /home/pi/catkin_ws/src/clov
 
 После того, как вы заполните карту, необходимо применить ее — для этого отредактируйте файл `aruco.launch`, расположенный в `~/catkin_ws/src/clover/clover/launch/`. Измените в нем строку `<param name="map" value="$(find aruco_pose)/map/map_name.txt"/>`, где `map_name.txt` название вашего файла с картой.
 
-При использовании маркеров,  не привязанных к горизонтальным плоскостям(пол, потолок), необходимо отключить параметр `known_tilt` как в модуле `aruco_detect`, так и в модуле `aruco_map` в том же файле. Для того, чтобы сделать это автоматически, введите:
+При использовании маркеров, не привязанных к горизонтальным плоскостям (пол, потолок), необходимо также сделать пустым значение аргумента `placement` в том же файле:
 
-```bash
+```xml
-sed -i "/known_tilt/s/value=\".*\"/value=\"\"/" /home/pi/catkin_ws/src/clover/clover/launch/aruco.launch
+<arg name="placement" default=""/>
 ```
 
 После всех настроек вызовите `sudo systemctl restart clover` для перезагрузки сервиса `clover`.

redirects.json

@@ -35,7 +35,7 @@
         { "from": "snippets.html", "to": "ru/snippets.html" },
         { "from": "camera_frame.html", "to": "ru/camera_setup.html" },
         { "from": "ru/camera_frame.html", "to": "camera_setup.html" },
-        { "from": "camera.html", "to": "ru/camera.html" },
+        { "from": "camera.html", "to": "en/camera.html" },
         { "from": "led.html", "to": "en/leds.html" },
         { "from": "leds.html", "to": "ru/leds.html" },
         { "from": "rviz.html", "to": "ru/rviz.html" },
@@ -51,7 +51,7 @@
         { "from": "firmware/", "to": "en/firmware.html" },
         { "from": "simple_offboard/", "to": "en/simple_offboard.html" },
         { "from": "offboard/", "to": "en/simple_offboard.html" },
-        { "from": "camera/", "to": "ru/camera.html" },
+        { "from": "camera/", "to": "en/camera.html" },
         { "from": "snippets/", "to": "ru/snippets.html" },
         { "from": "optical_flow/", "to": "ru/optical_flow.html" },
         { "from": "laser/", "to": "ru/laser.html" },

roswww_static/CMakeLists.txt

@@ -5,8 +5,6 @@ find_package(catkin REQUIRED)
 
 catkin_package()
 
-install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+catkin_install_python(PROGRAMS src/update
-
-catkin_install_python(PROGRAMS main.py
     DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )

roswww_static/README.md

@@ -6,12 +6,14 @@ Note: you should configure your web server to make it follow symlinks.
 
 ## Instructions
 
-* Run `main.py` node and it will generate the symlinks and index file.
+* Run `update` script and it will generate the symlinks and index file: `rosrun roswww_static update`.
 * Point your static web server path to `~/.ros/www`.
 
-You can rerun `main.py` if the list of installed packages changes.
+You can rerun `update` if the list of installed packages changes.
 
 ## Parameters
 
-* `index` – path for index page, otherwise packages list would be generated.
+Parameters are passed through environment variables:
-* `default_package` – if set then the index page would redirect to this package's page.
+
+* `ROSWWW_INDEX` – path for index page, otherwise packages list would be generated.
+* `ROSWWW_DEFAULT` – if set then the index page would redirect to this package's page.

roswww_static/launch/example.launch

@@ -1,6 +0,0 @@
-<launch>
-    <node pkg="roswww_static" name="roswww_static" type="main.py" clear_params="true" output="screen">
-        <!-- <param name="index" value="$(find my_package)/www/index.html"/> -->
-        <!-- <param name="default_package" value="my_package"/> -->
-    </node>
-</launch>

roswww_static/src/update

@@ -13,17 +13,15 @@
 
 import os
 import shutil
-import rospy
 import rospkg
 
-rospy.init_node('roswww_static')
-
 rospack = rospkg.RosPack()
 
 www = rospkg.get_ros_home() + '/www'
-index_file = rospy.get_param('~index_file', None)
+index_file = os.environ.get('ROSWWW_INDEX')
-default_package = rospy.get_param('~default_package', None)
+default_package = os.environ.get('ROSWWW_DEFAULT')
 
+print('using www dir: ' + www)
 shutil.rmtree(www, ignore_errors=True)  # reset www directory content
 os.mkdir(www)
 
@@ -34,7 +32,7 @@ index = '<h1>Packages list</h1>\n<ul>\n'
 for name in packages:
     path = rospack.get_path(name)
     if os.path.exists(path + '/www'):
-        rospy.loginfo('found www path for %s package', name)
+        print('found www path for %s package' % name)
         os.symlink(path + '/www', www + '/' + name)
         index += '<li><a href="{name}/">{name}</a></li>'.format(name=name)
 
@@ -42,7 +40,7 @@ if default_package is not None:
     redirect_html = '<meta http-equiv=refresh content="0; url={name}/">'.format(name=default_package)
     open(www + '/index.html', 'w').write(redirect_html)
 elif index_file is not None:
-    rospy.loginfo('symlinking index file')
+    print('symlinking index file')
     os.symlink(index_file, www + '/index.html')
 else:
     open(www + '/index.html', 'w').write(index)