Merge branch 'master' into roswww-static-target

Clover package Python library added.

.github/workflows/build.yml

@@ -25,13 +25,27 @@ jobs:
     - uses: actions/checkout@v2
       with:
         path: catkin_ws/src/clover
-    - name: Install pip
-      run: apt-get update && apt-get -y install python3-pip
+    - 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: Install
-      run: source devel/setup.bash && catkin_make install
+    - 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

aruco_pose/src/aruco_detect.cpp

@@ -71,7 +71,8 @@ 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_, auto_flip_, use_map_markers_;
+	bool waiting_for_map_;
 	double length_;
 	ros::Duration transform_timeout_;
 	std::unordered_map<int, double> length_override_;
@@ -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();
@@ -133,6 +136,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;
 
@@ -395,7 +399,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

@@ -503,7 +503,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/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)

builder/image-ros.sh

@@ -112,7 +112,7 @@ my_travis_retry pip3 install wheel
 my_travis_retry pip3 install -r /home/pi/catkin_ws/src/clover/clover/requirements.txt
 source /opt/ros/${ROS_DISTRO}/setup.bash
 # Don't build simulation plugins for actual drone
-catkin_make -j2 -DCMAKE_BUILD_TYPE=RelWithDebInfo
+sudo -E -u pi sh -c '. /opt/ros/${ROS_DISTRO}/setup.sh && catkin_make -j2 -DCMAKE_BUILD_TYPE=RelWithDebInfo'
 source devel/setup.bash
 
 echo_stamp "Install clever package (for backwards compatibility)"

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
 
@@ -31,9 +33,11 @@ import tf2_geometry_msgs
 import VL53L1X
 import pymavlink
 from pymavlink import mavutil
-import rpi_ws281x
-import pigpio
 # from espeak import espeak
 from pyzbar import pyzbar
 
 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,64 @@ 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_suite
-rosversion rosserial
+rosversion rosbridge_server
 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
+
+[ $VM ] && H="/home/clover" || H="/home/pi"
+
 # 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

@@ -46,14 +46,6 @@ find_package(OpenCV ${_opencv_version} REQUIRED
     imgproc
 )
 
-# Download GeographicLib datasets
-message(STATUS "Downloading GeographicLib datasets to ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_SHARE_DESTINATION}")
-add_custom_target(download_geographiclib_datasets ALL
-  COMMAND geographiclib-get-geoids -p ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_SHARE_DESTINATION}/GeographicLib egm96-5
-  COMMAND geographiclib-get-gravity -p ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_SHARE_DESTINATION}/GeographicLib egm96
-  COMMAND geographiclib-get-magnetic -p ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_SHARE_DESTINATION}/GeographicLib emm2015
-)
-
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
 
@@ -61,7 +53,7 @@ add_custom_target(download_geographiclib_datasets ALL
 ## 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 ##
@@ -238,6 +230,9 @@ target_link_libraries(${PROJECT_NAME}
   ${OpenCV_LIBRARIES}
 )
 
+# Set Clover to default www page
+set(ROSWWW_STATIC_DEFAULT ${PROJECT_NAME})
+
 #############
 ## Install ##
 #############

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'
+    if 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/launch/aruco.launch

@@ -18,6 +18,7 @@
         <remap from="map_markers" to="aruco_map/map"/>
         <param name="estimate_poses" value="true"/>
         <param name="send_tf" value="true"/>
+        <param name="use_map_markers" value="true"/>
         <param name="known_tilt" value="map" if="$(eval placement == 'floor')"/>
         <param name="known_tilt" value="map_flipped" if="$(eval placement == 'ceiling')"/>
         <param name="length" value="$(arg length)"/>
@@ -52,4 +53,8 @@
         <param name="force_init" value="$(arg force_init)"/>
         <param name="offset_frame_id" value="aruco_map"/>
     </node>
+
+    <!-- run map_flipped frame if placement is ceiling -->
+    <node pkg="tf2_ros" type="static_transform_publisher" name="map_flipped_frame"
+        args="0 0 0 3.1415926 3.1415926 0 map map_flipped" if="$(eval placement == 'ceiling')"/>
 </launch>

clover/launch/clover.launch

@@ -45,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="false"/>
     </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"/>
@@ -85,9 +84,4 @@
         <!-- Send fake GCS heartbeats. Set to "true" for upstream PX4 -->
         <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

@@ -44,9 +44,6 @@
         <!-- remap rangefinder -->
         <remap from="mavros/distance_sensor/rangefinder_sub" to="$(arg distance_sensor_remap)" if="$(eval bool(distance_sensor_remap))"/>
 
-        <!-- path to find GoegraphicLib datasets -->
-        <env name="GEOGRAPHICLIB_DATA" value="$(eval env('CMAKE_PREFIX_PATH').split(':')[0] + '/share/GeographicLib')"/>
-
         <rosparam param="plugin_whitelist">
             - altitude
             - command

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/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 battery_sub = nh.subscribe("/mavros/battery", 1, &handleBattery);
+	auto mavros_state_sub = nh.subscribe("mavros/state", 1, &handleMavrosState);
+	auto battery_sub = nh.subscribe("mavros/battery", 1, &handleBattery);
 	auto rosout_sub = nh.subscribe("/rosout_agg", 1, &handleLog);
 
 	timer = nh.createTimer(ros::Duration(0), &proceed, false, false);

clover/src/optical_flow.cpp

@@ -25,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;
@@ -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

@@ -15,7 +15,8 @@ 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,58 @@ tf_buffer = tf2_ros.Buffer()
 tf_listener = tf2_ros.TransformListener(tf_buffer)
 
 
-failures = []
-infos = []
-current_check = None
+thread_local = threading.local()
+reports_lock = Lock()
 
 
 def failure(text, *args):
     msg = text % args
-    rospy.logwarn('%s: %s', current_check, msg)
-    failures.append(msg)
+    thread_local.reports += [{'failure': msg}]
 
 
 def info(text, *args):
     msg = text % args
-    rospy.loginfo('%s: %s', current_check, msg)
-    infos.append(msg)
+    thread_local.reports += [{'info': msg}]
 
 
 def check(name):
     def inner(fn):
         def wrapper(*args, **kwargs):
-            failures[:] = []
-            infos[:] = []
-            global current_check
-            current_check = name
+            start = rospy.get_time()
+            thread_local.reports = []
             try:
                 fn(*args, **kwargs)
             except Exception as e:
                 traceback.print_exc()
                 rospy.logerr('%s: exception occurred', name)
-                return
-            if not failures and not infos:
-                rospy.loginfo('%s: OK', name)
+            with reports_lock:
+                for report in thread_local.reports:
+                    if 'failure' in report:
+                        rospy.logerr('%s: %s', name, report['failure'])
+                    elif 'info' in report:
+                        rospy.loginfo('\033[90m%s\033[0m: %s', name, report['info'])
+                if not thread_local.reports:
+                    rospy.loginfo('\033[90m%s\033[0m: \033[92mOK\033[0m', name)
+                if rospy.get_param('~time', False):
+                    rospy.loginfo('%s: %.1f sec', name, rospy.get_time() - start)
         return wrapper
     return inner
 
 
+def ff(value, precision=2):
+    # safely format float or int
+    if value is None:
+        return '\033[31m???\033[0m'
+    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 +106,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 +161,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°',
@@ -198,27 +226,28 @@ def check_fcu():
             failure('no connection to the FCU (check wiring)')
             return
 
-        clover_tag = re.compile(r'-cl[oe]ver\.\d+$')
-        clover_fw = False
+        if not is_process_running('px4', exact=True): # can't use px4 console in SITL
+            clover_tag = re.compile(r'-cl[oe]ver\.\d+$')
+            clover_fw = False
 
-        # Make sure the console is available to us
-        mavlink_exec('\n')
-        version_str = mavlink_exec('ver all')
-        if version_str == '':
-            info('no version data available from SITL')
+            # Make sure the console is available to us
+            mavlink_exec('\n')
+            version_str = mavlink_exec('ver all')
+            if version_str == '':
+                info('no version data available from SITL')
 
-        for line in version_str.split('\n'):
-            if line.startswith('FW version: '):
-                info(line[len('FW version: '):])
-            elif line.startswith('FW git tag: '): # only Clover's firmware
-                tag = line[len('FW git tag: '):]
-                clover_fw = clover_tag.search(tag)
-                info(tag)
-            elif line.startswith('HW arch: '):
-                info(line[len('HW arch: '):])
+            for line in version_str.split('\n'):
+                if line.startswith('FW version: '):
+                    info(line[len('FW version: '):])
+                elif line.startswith('FW git tag: '): # only Clover's firmware
+                    tag = line[len('FW git tag: '):]
+                    clover_fw = clover_tag.search(tag)
+                    info(tag)
+                elif line.startswith('HW arch: '):
+                    info(line[len('HW arch: '):])
 
-        if not clover_fw:
-            info('not Clover PX4 firmware, check https://clover.coex.tech/firmware')
+            if not clover_fw:
+                info('not Clover PX4 firmware, check https://clover.coex.tech/firmware')
 
         est = get_param('SYS_MC_EST_GROUP')
         if est == 1:
@@ -255,18 +284,25 @@ 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)
-            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')
+            info('time sync offset: %.2f s', float(read_diagnostics('mavros: Time Sync', 'Estimated time offset (s)')))
+        except:
+            failure('cannot read time sync offset')
 
     except rospy.ROSException:
         failure('no MAVROS state (check wiring)')
@@ -346,6 +382,8 @@ 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'))
@@ -356,9 +394,9 @@ def check_aruco():
             known_tilt += ' (ALL markers are on the floor)'
         elif known_tilt == 'map_flipped':
             known_tilt += ' (ALL markers are on the ceiling)'
-        info('aruco_detector/known_tilt = %s', known_tilt)
+        info('aruco_detect/known_tilt = %s', known_tilt)
         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
@@ -375,34 +413,49 @@ def check_aruco():
         info('aruco_map/known_tilt = %s', known_tilt)
 
         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')
-            # check if vpe_publisher is running
-            try:
-                subprocess.check_output(['pgrep', '-x', 'vpe_publisher'])
-            except subprocess.CalledProcessError:
-                return  # it's not running, skip following checks
+            if not is_process_running('vpe_publisher', full=True):
+                info('no vision position estimate, vpe_publisher is not running')
+            elif rospy.get_param('aruco_map/known_tilt') == 'map_flipped':
+                failure('no vision position estimate, markers are on the ceiling')
+            elif is_on_the_floor():
+                info('no vision position estimate, the drone is on the floor')
+            else:
+                failure('no vision position estimate')
 
     # check PX4 settings
     est = get_param('SYS_MC_EST_GROUP')
@@ -414,14 +467,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)
-        info('LPE_VIS_XY is %.2f m, LPE_VIS_Z is %.2f m', get_param('LPE_VIS_XY'), get_param('LPE_VIS_Z'))
+            failure('LPE_VIS_DELAY = %s, but it should be zero', delay)
+        info('LPE_VIS_XY = %s m, LPE_VIS_Z = %s m', get_paramf('LPE_VIS_XY'), get_paramf('LPE_VIS_Z'))
     elif est == 2:
         fuse = get_param('EKF2_AID_MASK')
         if not fuse & (1 << 3):
@@ -430,10 +483,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)
-        info('EKF2_EVA_NOISE is %.3f, EKF2_EVP_NOISE is %.3f',
-            get_param('EKF2_EVA_NOISE'),
-            get_param('EKF2_EVP_NOISE'))
+            failure('EKF2_EV_DELAY = %.2f, but it should be zero', delay)
+        info('EKF2_EVA_NOISE = %s, EKF2_EVP_NOISE = %s',
+            get_paramf('EKF2_EVA_NOISE', 3),
+            get_paramf('EKF2_EVP_NOISE', 3))
 
     if not vis:
         return
@@ -531,10 +584,10 @@ 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')
 
 
@@ -547,7 +600,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 +608,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',
-                          get_param('LPE_FLW_QMIN'),
-                          get_param('LPE_FLW_R'),
-                          get_param('LPE_FLW_RR'),
-                          get_param('SENS_FLOW_MINHGT'),
-                          get_param('SENS_FLOW_MAXHGT'))
+            info('LPE_FLW_QMIN = %s, LPE_FLW_R = %s, LPE_FLW_RR = %s',
+                          get_paramf('LPE_FLW_QMIN'),
+                          get_paramf('LPE_FLW_R', 4),
+                          get_paramf('LPE_FLW_RR', 4))
         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)
-            info('EKF2_OF_QMIN is %s, EKF2_OF_N_MIN is %.4f, EKF2_OF_N_MAX is %.4f, SENS_FLOW_MINHGT is %.3f, SENS_FLOW_MAXHGT is %.3f',
-                          get_param('EKF2_OF_QMIN'),
-                          get_param('EKF2_OF_N_MIN'),
-                          get_param('EKF2_OF_N_MAX'),
-                          get_param('SENS_FLOW_MINHGT'),
-                          get_param('SENS_FLOW_MAXHGT'))
+                failure('EKF2_OF_DELAY = %.2f, but it should be zero', delay)
+            info('EKF2_OF_QMIN = %s, EKF2_OF_N_MIN = %s, EKF2_OF_N_MAX = %s',
+                          get_paramf('EKF2_OF_QMIN'),
+                          get_paramf('EKF2_OF_N_MIN', 4),
+                          get_paramf('EKF2_OF_N_MAX', 4))
+        info('SENS_FLOW_MINHGT = %s, SENS_FLOW_MAXHGT = %s', get_paramf('SENS_FLOW_MINHGT', 3), get_paramf('SENS_FLOW_MAXHGT', 3))
 
     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 +661,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:
@@ -638,7 +695,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')
@@ -707,7 +764,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')
@@ -818,26 +878,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()
-    check_board()
-    check_clover_service()
-    check_network()
-    check_fcu()
-    check_imu()
-    check_local_position()
-    check_velocity()
-    check_global_position()
-    check_preflight_status()
-    check_main_camera()
-    check_aruco()
-    check_simpleoffboard()
-    check_optical_flow()
-    check_vpe()
-    check_rangefinder()
-    check_rpi_health()
-    check_cpu_usage()
-    check_boot_duration()
+    checks = [
+        check_image,
+        check_board,
+        check_clover_service,
+        check_network,
+        check_fcu,
+        check_imu,
+        check_local_position,
+        check_velocity,
+        check_global_position,
+        check_preflight_status,
+        check_main_camera,
+        check_aruco,
+        check_simpleoffboard,
+        check_optical_flow,
+        check_vpe,
+        check_rangefinder,
+        check_rpi_health,
+        check_cpu_usage,
+        check_boot_duration,
+    ]
+    if rospy.get_param('~parallel', False):
+        parallel_for(checks)
+    else:
+        consequentially_for(checks)
 
 
 if __name__ == '__main__':

clover/src/simple_offboard.cpp

@@ -860,6 +860,13 @@ 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();
+	res.success = true;
+	return true;
+}
+
 int main(int argc, char **argv)
 {
 	ros::init(argc, argv, "simple_offboard");
@@ -933,6 +940,7 @@ int main(int argc, char **argv)
 	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);

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

@@ -4,7 +4,6 @@
     <node pkg="mavros" type="mavros_node" name="mavros" required="true" output="screen">
         <param name="fcu_url" value="udp://@127.0.1:14557"/>
         <rosparam command="load" file="$(find clover)/launch/mavros_config.yaml"/>
-        <env name="GEOGRAPHICLIB_DATA" value="$(eval env('CMAKE_PREFIX_PATH').split(':')[0] + '/share/GeographicLib')"/>
     </node>
 
     <node name="visualization" pkg="mavros_extras" type="visualization" required="true">
@@ -38,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/www/index.html

@@ -12,6 +12,10 @@
 	<li><a href="console.html">Clover console</a> (<code>/var/log/clover.log</code>)</li>
 </ul>
 
+<p>
+	Update www using <code>rosrun roswww_static update</code>.
+</p>
+
 <div class="version"></div>
 
 <script type="text/javascript">

clover_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/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/camera.md

@@ -145,6 +145,8 @@ 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`):
 
+> **Note** Starting from [image](image.md) version **0.24** `image_raw_throttled` topic is available without addition configuration.
+
 ```xml
 <node pkg="topic_tools" name="cam_throttle" type="throttle"
     args="messages main_camera/image_raw 5.0 main_camera/image_raw_throttled"/>

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/simple_offboard.md

@@ -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).
 
@@ -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_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

@@ -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/velocity', TwistStamped, velocity_update)
-rospy.Subscriber('/mavros/battery', BatteryState, battery_update)
+rospy.Subscriber('mavros/local_position/pose', PoseStamped, pose_update)
+rospy.Subscriber('mavros/local_position/velocity', TwistStamped, velocity_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)):
@@ -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/ru/camera.md

@@ -147,6 +147,8 @@ rospy.spin()
 
 Скрипт будет занимать 100% процессора. Для искусственного замедления работы скрипта можно запустить [throttling](http://wiki.ros.org/topic_tools/throttle) кадров с камеры, например, в 5 Гц (`main_camera.launch`):
 
+> **Note** Начиная с версии [образа](image.md) **0.24** топик `image_raw_throttled` доступен без дополнительной конфигурации.
+
 ```xml
 <node pkg="topic_tools" name="cam_throttle" type="throttle"
     args="messages main_camera/image_raw 5.0 main_camera/image_raw_throttled"/>

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/simple_offboard.md

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

@@ -217,9 +217,9 @@ def pose_update(pose):
     # Обработка новых данных о позиции коптера
     pass
 
-rospy.Subscriber('/mavros/local_position/pose', PoseStamped, pose_update)
-rospy.Subscriber('/mavros/local_position/velocity', TwistStamped, velocity_update)
-rospy.Subscriber('/mavros/battery', BatteryState, battery_update)
+rospy.Subscriber('mavros/local_position/pose', PoseStamped, pose_update)
+rospy.Subscriber('mavros/local_position/velocity', TwistStamped, velocity_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)):
@@ -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)
+```

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,19 @@ find_package(catkin REQUIRED)
 
 catkin_package()
 
+macro(roswww_static_make_default)
+  message(STATUS "roswww_static: make ${PROJECT_NAME} package default")
+  set(ROSWWW_STATIC_DEFAULT ${PROJECT_NAME} CACHE STRING "Default roswww_static package")
+endmacro()
+
 install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
 
-catkin_install_python(PROGRAMS main.py
+catkin_install_python(PROGRAMS src/update
     DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
+
+message(status "CATKIN_ENV: ${CATKIN_ENV}")
+
+add_custom_target(roswww_static ALL
+    COMMAND ${CMAKE_COMMAND} -E env ROSWWW_STATIC_DEFAULT=${ROSWWW_STATIC_DEFAULT}
+    ${CATKIN_ENV} ${CMAKE_CURRENT_SOURCE_DIR}/src/update)

roswww_static/README.md

@@ -6,12 +6,12 @@ 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.
-* `default_package` – if set then the index page would redirect to this package's page.
+* `index` – path for index page, otherwise packages list would be generated.
+* `default_package` – if set then the index page would redirect to this package's page.

roswww_static/launch/example.launch

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

roswww_static/src/update

@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
 
 # Copyright (C) 2020 Copter Express Technologies
 #
@@ -16,13 +16,16 @@ import shutil
 import rospy
 import rospkg
 
-rospy.init_node('roswww_static')
+#rospy.init_node('roswww_static')
 
 rospack = rospkg.RosPack()
 
 www = rospkg.get_ros_home() + '/www'
-index_file = rospy.get_param('~index_file', None)
-default_package = rospy.get_param('~default_package', None)
+index_file = None # rospy.get_param('~index_file', None)
+default_package = os.environ.get('ROSWWW_STATIC_DEFAULT') # rospy.get_param('~default_package', None)
+
+print('roswww_static: destination directory:', www)
+print('roswww_static: default package:', default_package)
 
 shutil.rmtree(www, ignore_errors=True)  # reset www directory content
 os.mkdir(www)
@@ -34,7 +37,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('roswww_static: found www path for package', name)
         os.symlink(path + '/www', www + '/' + name)
         index += '<li><a href="{name}/">{name}</a></li>'.format(name=name)
 
@@ -42,7 +45,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('roswww_static: symlinking index file')
     os.symlink(index_file, www + '/index.html')
 else:
     open(www + '/index.html', 'w').write(index)