Enable strict warnings for aruco_pose

As this flag breaks vision position aiding

.github/workflows/build.yml

@@ -7,13 +7,13 @@ on:
     branches: [ master ]
 
 jobs:
-  melodic:
-    runs-on: ubuntu-latest
-    steps:
-      - uses: actions/checkout@v2
-      - name: Native Melodic build
-        run: |
-          docker run --rm -v $(pwd):/root/catkin_ws/src/clover ros:melodic-ros-base /root/catkin_ws/src/clover/builder/standalone-install.sh
+  # melodic:
+  #   runs-on: ubuntu-latest
+  #   steps:
+  #     - uses: actions/checkout@v2
+  #     - name: Native Melodic build
+  #       run: |
+  #         docker run --rm -v $(pwd):/root/catkin_ws/src/clover ros:melodic-ros-base /root/catkin_ws/src/clover/builder/standalone-install.sh
   noetic:
     runs-on: ubuntu-latest
     steps:

.github/workflows/docs.yml

@@ -10,6 +10,10 @@ jobs:
   docs:
     runs-on: ubuntu-18.04
     steps:
+      - name: Cancel previous runs
+        uses: styfle/cancel-workflow-action@0.9.1
+        with:
+          access_token: ${{ github.token }}
       - uses: actions/checkout@v2
       - name: Use Node.js
         uses: actions/setup-node@v1
@@ -19,7 +23,7 @@ jobs:
           sudo sh -c "echo ttf-mscorefonts-installer msttcorefonts/accepted-mscorefonts-eula select true | debconf-set-selections"
           sudo apt-get update && sudo apt-get install -y calibre msttcorefonts
           builder/assets/install_gitbook.sh
-          npm install markdownlint-cli -g
+          npm install markdownlint-cli@0.28.1 -g # FIXME: https://github.com/DavidAnson/markdownlint/issues/435
           npm install svgexport -g
           gitbook -V
           markdownlint -V
@@ -34,7 +38,11 @@ jobs:
          gitbook install
          gitbook build
       - name: Generate PDF
-        if: ${{ github.event_name == 'push' && github.ref == 'refs/heads/master' }}
+        id: generate-pdf
+        env:
+          GITBOOK_SKIP_PDF: ${{ secrets.GITBOOK_SKIP_PDF }}
+        continue-on-error: ${{ env.GITBOOK_SKIP_PDF != '' }}
+        if: ${{ github.event_name == 'push' }}
         run: |
           for i in 1 2 3 4; do gitbook pdf ./ _book/clover.pdf && break || sleep 1; done
           sudo apt-get -q install ghostscript
@@ -43,6 +51,13 @@ jobs:
           rm _book/clover_ru.pdf && mv _book/clover_ru_compressed.pdf _book/clover_ru.pdf
           rm _book/clover_en.pdf && mv _book/clover_en_compressed.pdf _book/clover_en.pdf
           ls -lah _book/clover*.pdf
+          echo '::set-output name=GITBOOK_PDF_OK::1'
+      - name: Download older PDFs
+        if: ${{ !steps.generate-pdf.outputs.GITBOOK_PDF_OK }}
+        run: |
+          rm -f _book/clover*.pdf
+          wget --no-verbose https://clover.coex.tech/clover_ru.pdf -P _book/
+          wget --no-verbose https://clover.coex.tech/clover_en.pdf -P _book/
       - name: Deploy
         uses: JamesIves/github-pages-deploy-action@4.1.3
         if: ${{ github.event_name == 'push' && github.ref == 'refs/heads/master' }}

.github/workflows/editorconfig.yaml

@@ -15,4 +15,4 @@ jobs:
         run: |
           wget --no-verbose https://github.com/okalachev/editorconfig-checker/releases/download/1.2.1-disable-spaces-amount/ec-linux-amd64
           chmod +x ec-linux-amd64
-          ./ec-linux-amd64 -spaces-after-tabs -e "roslib.js|ros3d.js|eventemitter2.js|json-to-pretty-yaml.js|draw.cpp|BinUtils.swift|\.idea|apps/android/app|blockly/|clover_blocks/programs/|highlight/|python.js|Assets.xcassets|test_parser_pass.txt|test_node_failure.txt|aruco_pose/vendor|\.stl|\.dxf|\.dae"
+          ./ec-linux-amd64 -spaces-after-tabs -e "roslib.js|ros3d.js|eventemitter2.js|json-to-pretty-yaml.js|draw.cpp|BinUtils.swift|\.idea|apps/android/app|blockly/|clover_blocks/programs/|highlight/|python.js|Assets.xcassets|test_parser_pass.txt|test_node_failure.txt|aruco_pose/vendor|\.stl|\.dxf|\.dae|\.material"

aruco_pose/CMakeLists.txt

@@ -4,6 +4,8 @@ project(aruco_pose)
 ## Compile as C++11, supported in ROS Kinetic and newer
 add_compile_options(-std=c++11)
 
+add_compile_options(-Wall -Wextra -Werror)
+
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
@@ -83,11 +85,10 @@ add_message_files(
 )
 
 ## Generate services in the 'srv' folder
-# add_service_files(
-#   FILES
-#   Service1.srv
-#   Service2.srv
-# )
+add_service_files(
+  FILES
+  SetMarkers.srv
+)
 
 ## Generate actions in the 'action' folder
 # add_action_files(

aruco_pose/README.md

@@ -51,6 +51,7 @@ It's recommended to run it within the same nodelet manager with the camera nodel
 
 * `image_raw` (*sensor_msgs/Image*) – camera image
 * `camera_info` (*sensor_msgs/CameraInfo*) – camera calibration info
+* `map_markers` (*aruco_pose/MarkerArray*) – list of markers to disable TF transform publishing
 
 #### Published
 
@@ -97,6 +98,7 @@ See examples in [`map`](map/) directory.
 #### Published
 
 * `~pose` (*geometry_msgs/PoseWithCovarianceStamped*) – estimated map pose
+* `~map` (*aruco_pose/MarkerArray*) – list of markers in the loaded map
 * `~image` (*sensor_msgs/Image*) – planarized map image
 * `~visualization` (*visualization_msgs/MarkerArray*) – markers map visualization for rviz
 * `~debug` (*sensor_msgs/Image*) – debug image with detected markers and map axis

aruco_pose/cfg/Detector.cfg

@@ -10,6 +10,8 @@ gen = ParameterGenerator()
 
 gen.add("enabled", bool_t, 0, "if detection enabled", True)
 
+gen.add("length", double_t, 0, "markers' side length", min=0, max=10)
+
 gen.add("adaptiveThreshConstant",                 double_t, 0,
         "Constant for adaptive thresholding before finding contours",
          p.adaptiveThreshConstant, 0, 100)

aruco_pose/map/cmit.txt

@@ -1,3 +1,4 @@
+# id	length	x	y	z	rot_z	rot_y	rot_x
 0	0.33	0.0	9.0	0	0	0	0	
 1	0.33	1.0	9.0	0	0	0	0	
 2	0.33	2.0	9.0	0	0	0	0	

aruco_pose/map/office.txt

@@ -1,3 +1,4 @@
+# id	length	x	y	z	rot_z	rot_y	rot_x
 107	0.33	0	0	0	0	0	0
 106	0.33	0.77	0	0	0	0	0
 105	0.33	0	0.77	0	0	0	0

aruco_pose/map/office_ceiling.txt

@@ -1,3 +1,4 @@
+# id	length	x	y	z	rot_z	rot_y	rot_x
 14	0.365	0.000	0.0	0	0	0	0
 15	0.365	1.335	0.0	0	0	0	0
 30	0.365	2.865	0.0	0	0	0	0

aruco_pose/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>aruco_pose</name>
-  <version>0.21.1</version>
+  <version>0.23.0</version>
   <description>Positioning with ArUco markers</description>
 
   <maintainer email="okalachev@gmail.com">Oleg Kalachev</maintainer>

aruco_pose/src/aruco_detect.cpp

@@ -48,6 +48,7 @@
 #include <aruco_pose/Marker.h>
 #include <aruco_pose/MarkerArray.h>
 #include <aruco_pose/DetectorConfig.h>
+#include <aruco_pose/SetMarkers.h>
 
 #include "utils.h"
 #include <memory>
@@ -69,8 +70,10 @@ private:
 	image_transport::CameraSubscriber img_sub_;
 	ros::Publisher markers_pub_, vis_markers_pub_;
 	ros::Subscriber map_markers_sub_;
+	ros::ServiceServer set_markers_srv_;
 	bool estimate_poses_, send_tf_, auto_flip_;
 	double length_;
+	ros::Duration transform_timeout_;
 	std::unordered_map<int, double> length_override_;
 	std::string frame_id_prefix_, known_tilt_;
 	Mat camera_matrix_, dist_coeffs_;
@@ -97,6 +100,7 @@ public:
 			ros::shutdown();
 		}
 		readLengthOverride(nh_priv_);
+		transform_timeout_ = ros::Duration(nh_priv_.param("transform_timeout", 0.02));
 
 		known_tilt_ = nh_priv_.param<std::string>("known_tilt", "");
 		auto_flip_ = nh_priv_.param("auto_flip", false);
@@ -114,6 +118,8 @@ public:
 		dyn_srv_ = std::make_shared<dynamic_reconfigure::Server<aruco_pose::DetectorConfig>>(nh_priv_);
 		dyn_srv_->setCallback(std::bind(&ArucoDetect::paramCallback, this, std::placeholders::_1, std::placeholders::_2));
 
+		set_markers_srv_ = nh_priv_.advertiseService("set_length_override", &ArucoDetect::setMarkers, this);
+
 		debug_pub_ = it_priv.advertise("debug", 1);
 		markers_pub_ = nh_priv_.advertise<aruco_pose::MarkerArray>("markers", 1);
 		vis_markers_pub_ = nh_priv_.advertise<visualization_msgs::MarkerArray>("visualization", 1);
@@ -172,7 +178,7 @@ private:
 				if (!known_tilt_.empty()) {
 					try {
 						snap_to = tf_buffer_->lookupTransform(msg->header.frame_id, known_tilt_,
-						                                      msg->header.stamp, ros::Duration(0.02));
+						                                      msg->header.stamp, transform_timeout_);
 					} catch (const tf2::TransformException& e) {
 						NODELET_WARN_THROTTLE(5, "can't snap: %s", e.what());
 					}
@@ -346,6 +352,29 @@ private:
 		}
 	}
 
+	bool setMarkers(aruco_pose::SetMarkers::Request& req, aruco_pose::SetMarkers::Response& res)
+	{
+		for (auto const& marker : req.markers) {
+			if (marker.id > 999) {
+				res.message = "Invalid marker id: " + std::to_string(marker.id);
+				ROS_ERROR("%s", res.message.c_str());
+				return true;
+			}
+			if (!std::isfinite(marker.length) || marker.length <= 0) {
+				res.message = "Invalid marker " + std::to_string(marker.id) + " length: " + std::to_string(marker.length);
+				ROS_ERROR("%s", res.message.c_str());
+				return true;
+			}
+		}
+
+		for (auto const& marker : req.markers) {
+			length_override_[marker.id] = marker.length;
+		}
+
+		res.success = true;
+		return true;
+	}
+
 	void mapMarkersCallback(const aruco_pose::MarkerArray& msg)
 	{
 		map_markers_ids_.clear();
@@ -356,7 +385,8 @@ private:
 
 	void paramCallback(aruco_pose::DetectorConfig &config, uint32_t level)
 	{
-		enabled_ = config.enabled;
+		enabled_ = config.enabled && config.length > 0;
+		length_ = config.length;
 		parameters_->adaptiveThreshConstant = config.adaptiveThreshConstant;
 		parameters_->adaptiveThreshWinSizeMin = config.adaptiveThreshWinSizeMin;
 		parameters_->adaptiveThreshWinSizeMax = config.adaptiveThreshWinSizeMax;

aruco_pose/src/aruco_map.cpp

@@ -89,7 +89,7 @@ public:
 
 		// TODO: why image_transport doesn't work here?
 		img_pub_ = nh_priv_.advertise<sensor_msgs::Image>("image", 1, true);
-		markers_pub_ = nh_priv_.advertise<aruco_pose::MarkerArray>("markers", 1, true);
+		markers_pub_ = nh_priv_.advertise<aruco_pose::MarkerArray>("map", 1, true);
 
 		board_ = cv::makePtr<cv::aruco::Board>();
 		board_->dictionary = cv::aruco::getPredefinedDictionary(

aruco_pose/src/genmap.py

@@ -30,7 +30,7 @@ Options:
   -o <filename>  Output map file name in the 'map' subdirectory of aruco_pose package
 
 Example:
-  rosrun aruco_pose genmap.py 0.33 2 4 1 1 0 > $(catkin_find aruco_pose map)/test_map.txt
+  rosrun aruco_pose genmap.py 0.33 2 4 1 1 0 -o test_map.txt
 """
 
 from __future__ import print_function

aruco_pose/srv/SetMarkers.srv

@@ -0,0 +1,7 @@
+# * Add or change markers in the map
+# * Change markers' properties, e. g. lengths
+
+Marker[] markers # if length or pose is nan - remove from map
+---
+bool success
+string message

aruco_pose/test/basic.py

@@ -143,7 +143,7 @@ def test_map_image(node):
     assert img.encoding in ('mono8', 'rgb8')
 
 def test_map_markers(node):
-    markers = rospy.wait_for_message('aruco_map/markers', MarkerArray, timeout=5)
+    markers = rospy.wait_for_message('aruco_map/map', MarkerArray, timeout=5)
     assert markers.markers[0].id == 1
     assert markers.markers[1].id == 2
     assert markers.markers[2].id == 3

builder/image-ros.sh

@@ -13,7 +13,7 @@
 # copies or substantial portions of the Software.
 #
 
-set -e # Exit immidiately on non-zero result
+set -ex # exit on error, echo commands
 
 REPO=$1
 REF=$2
@@ -90,7 +90,7 @@ echo_stamp "Installing OpenCV 4.2-compatible ROS packages"
 apt install -y --no-install-recommends \
 ros-${ROS_DISTRO}-compressed-image-transport=1.14.0-0buster \
 ros-${ROS_DISTRO}-cv-bridge=1.15.0-0buster \
-ros-${ROS_DISTRO}-cv-camera=0.5.0-0buster \
+ros-${ROS_DISTRO}-cv-camera=0.5.1-0buster \
 ros-${ROS_DISTRO}-image-publisher=1.15.3-0buster \
 ros-${ROS_DISTRO}-web-video-server=0.2.1-0buster
 apt-mark hold \
@@ -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 -DCATKIN_BLACKLIST_PACKAGES=clover_gazebo_plugins
+catkin_make -j2 -DCMAKE_BUILD_TYPE=RelWithDebInfo
 source devel/setup.bash
 
 echo_stamp "Install clever package (for backwards compatibility)"

builder/image-software.sh

@@ -137,6 +137,8 @@ pip3 --version
 echo_stamp "Install and enable Butterfly (web terminal)"
 echo_stamp "Workaround for tornado >= 6.0 breaking butterfly"
 export CRYPTOGRAPHY_DONT_BUILD_RUST=1
+my_travis_retry pip3 install cryptography==3.4.6 # https://stackoverflow.com/a/68472128/6850197
+my_travis_retry pip3 install pyOpenSSL==20.0.1
 my_travis_retry pip3 install tornado==5.1.1
 my_travis_retry pip3 install butterfly
 my_travis_retry pip3 install butterfly[systemd]

builder/test/tests.sh

@@ -58,5 +58,9 @@ 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.12" ]]
+
 # validate examples are present
 [[ $(ls /home/pi/examples/*) ]]

clover/CMakeLists.txt

@@ -4,6 +4,8 @@ project(clover)
 ## Compile as C++11, supported in ROS Kinetic and newer
 add_compile_options(-std=c++11)
 
+add_compile_options(-Wall -Wextra -Werror)
+
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
@@ -24,6 +26,7 @@ find_package(catkin REQUIRED COMPONENTS
   tf2_ros
   image_transport
   cv_bridge
+  dynamic_reconfigure
 )
 
 list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/cmake")
@@ -126,10 +129,9 @@ generate_messages(
 ##     and list every .cfg file to be processed
 
 ## Generate dynamic reconfigure parameters in the 'cfg' folder
-# generate_dynamic_reconfigure_options(
-#   cfg/DynReconf1.cfg
-#   cfg/DynReconf2.cfg
-# )
+generate_dynamic_reconfigure_options(
+  cfg/Flow.cfg
+)
 
 ###################################
 ## catkin specific configuration ##
@@ -211,6 +213,8 @@ add_dependencies(clover_led ${PROJECT_NAME}_generate_messages_cpp)
 
 add_dependencies(shell ${PROJECT_NAME}_generate_messages_cpp)
 
+add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencfg)
+
 ## Rename C++ executable without prefix
 ## The above recommended prefix causes long target names, the following renames the
 ## target back to the shorter version for ease of user use

clover/cfg/Flow.cfg

@@ -0,0 +1,10 @@
+#!/usr/bin/env python
+PACKAGE = "clover"
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+gen.add("enabled", bool_t, 0, "if optical flow enabled", True)
+
+exit(gen.generate(PACKAGE, "clover", "Flow"))

clover/examples/subscriber.py

@@ -0,0 +1,15 @@
+# Information: https://clover.coex.tech/en/laser.html
+
+import rospy
+from sensor_msgs.msg import Range
+
+rospy.init_node('process_rangefinder')
+
+def range_callback(msg):
+    # Process data from the rangefinder
+    print('Rangefinder distance:', msg.range)
+
+# Subscribe to laser rangefinder data
+rospy.Subscriber('rangefinder/range', Range, range_callback)
+
+rospy.spin()

clover/launch/aruco.launch

@@ -8,16 +8,20 @@
 
     <!-- For additional help go to https://clover.coex.tech/aruco -->
 
+    <arg name="force_init" default="false"/>
+    <arg name="disable" default="false"/> <!-- only force init -->
+
     <!-- aruco_detect: detect aruco markers, estimate poses -->
-    <node name="aruco_detect" pkg="nodelet" if="$(arg aruco_detect)" type="nodelet" args="load aruco_pose/aruco_detect main_camera_nodelet_manager" output="screen" clear_params="true" respawn="true">
+    <node name="aruco_detect" pkg="nodelet" if="$(eval aruco_detect and not disable)" type="nodelet" args="load aruco_pose/aruco_detect main_camera_nodelet_manager" output="screen" clear_params="true" respawn="true">
         <remap from="image_raw" to="main_camera/image_raw"/>
         <remap from="camera_info" to="main_camera/camera_info"/>
-        <remap from="map_markers" to="aruco_map/markers"/>
+        <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="known_tilt" value="map_flipped" if="$(eval placement == 'ceiling')"/>
         <param name="length" value="$(arg length)"/>
+        <param name="transform_timeout" value="0.1"/>
         <!-- aruco detector parameters -->
         <param name="cornerRefinementMethod" value="2"/> <!-- contour refinement -->
         <param name="minMarkerPerimeterRate" value="0.075"/> <!-- 0.075 for 320x240, 0.0375 for 640x480 -->
@@ -26,7 +30,7 @@
     </node>
 
     <!-- aruco_map: estimate aruco map pose -->
-    <node name="aruco_map" pkg="nodelet" type="nodelet" if="$(arg aruco_map)" args="load aruco_pose/aruco_map main_camera_nodelet_manager" output="screen" clear_params="true" respawn="true">
+    <node name="aruco_map" pkg="nodelet" type="nodelet" if="$(eval aruco_map and not disable)" args="load aruco_pose/aruco_map main_camera_nodelet_manager" output="screen" clear_params="true" respawn="true">
         <remap from="image_raw" to="main_camera/image_raw"/>
         <remap from="camera_info" to="main_camera/camera_info"/>
         <remap from="markers" to="aruco_detect/markers"/>
@@ -41,11 +45,11 @@
     </node>
 
     <!-- vpe publisher from aruco markers -->
-    <node name="vpe_publisher" pkg="clover" type="vpe_publisher" if="$(arg aruco_vpe)" output="screen" clear_params="true">
-        <remap from="~pose_cov" to="aruco_map/pose"/>
+    <node name="vpe_publisher" pkg="clover" type="vpe_publisher" if="$(eval aruco_vpe or force_init)" output="screen" clear_params="true">
+        <remap from="~pose_cov" to="aruco_map/pose" if="$(arg aruco_vpe)"/>
         <remap from="~vpe" to="mavros/vision_pose/pose"/>
-        <param name="frame_id" value="aruco_map_detected"/>
-        <param name="publish_zero" value="true"/>
+        <param name="frame_id" value="aruco_map_detected" if="$(arg aruco_vpe)"/>
+        <param name="force_init" value="$(arg force_init)"/>
         <param name="offset_frame_id" value="aruco_map"/>
     </node>
 </launch>

clover/launch/clover.launch

@@ -12,6 +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="simulator" default="false"/> <!-- flag that we are operating on a simulated drone -->
 
@@ -33,7 +34,10 @@
     </node>
 
     <!-- aruco markers -->
-    <include file="$(find clover)/launch/aruco.launch" if="$(arg aruco)"/>
+    <include file="$(find clover)/launch/aruco.launch" if="$(eval aruco or force_init)">
+        <arg name="force_init" value="$(arg force_init)"/>
+        <arg name="disable" value="$(eval not aruco)"/>
+    </include>
 
     <!-- optical flow -->
     <node pkg="nodelet" type="nodelet" name="optical_flow" args="load clover/optical_flow main_camera_nodelet_manager" if="$(arg optical_flow)" clear_params="true" output="screen" respawn="true">
@@ -47,9 +51,6 @@
 
     <!-- simplified offboard control -->
     <node name="simple_offboard" pkg="clover" type="simple_offboard" output="screen" clear_params="true">
-        <param name="reference_frames/body" value="map"/>
-        <param name="reference_frames/base_link" value="map"/>
-        <param name="reference_frames/navigate_target" value="map"/>
         <param name="reference_frames/main_camera_optical" value="map"/>
     </node>
 

clover/launch/mavros.launch

@@ -39,7 +39,7 @@
         <rosparam command="load" file="$(find clover)/launch/mavros_config.yaml"/>
 
         <!-- remap rangefinder -->
-        <remap from="mavros/distance_sensor/rangefinder_sub" to="rangefinder/range"/>
+        <remap from="mavros/distance_sensor/rangefinder_sub" to="$(arg distance_sensor_remap)" if="$(eval bool(distance_sensor_remap))"/>
 
         <rosparam param="plugin_whitelist">
             - altitude

clover/launch/mavros_config.yaml

@@ -1,11 +1,11 @@
 # Config file for mavros
 # Based on https://raw.githubusercontent.com/mavlink/mavros/master/mavros/launch/px4_config.yaml
 
-startup_px4_usb_quirk: true
+startup_px4_usb_quirk: false
 
 conn:
-  heartbeat_rate: 1.0    # send hertbeat rate in Hertz
-  timeout: 10.0          # hertbeat timeout in seconds
+  heartbeat_rate: 1.0    # send heartbeat rate in Hertz
+  timeout: 10.0          # heartbeat timeout in seconds
   timesync_rate: 10.0    # TIMESYNC rate in Hertz (feature disabled if 0.0)
   system_time_rate: 1.0  # send system time to FCU rate in Hertz (disabled if 0.0)
 
@@ -13,6 +13,7 @@ time:
   time_ref_source: "fcu"  # time_reference source
   timesync_mode: MAVLINK
   timesync_avg_alpha: 0.6 # timesync averaging factor
+  publish_sim_time: false # don't publish /clock
 
 global_position:
   frame_id: "map"             # origin frame
@@ -77,6 +78,9 @@ distance_sensor:
       field_of_view: 0.5
   rangefinder_sub:
       subscriber: true
+      id: 1
+      orientation: PITCH_270
+      covariance: 1  # cm
 
 # fake_gps
 fake_gps:

clover/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>clover</name>
-  <version>0.21.1</version>
+  <version>0.23.0</version>
   <description>The Clover package</description>
 
   <maintainer email="okalachev@gmail.com">Oleg Kalachev</maintainer>
@@ -39,6 +39,7 @@
   <depend>tf2_web_republisher</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>gtest</test_depend> -->

clover/src/autotest/autotest_aruco.py

@@ -0,0 +1,87 @@
+#!/usr/bin/env python3
+
+import rospy
+import math
+import signal
+import sys
+import dynamic_reconfigure.client
+from clover import srv
+from std_srvs.srv import Trigger
+from sensor_msgs.msg import Range
+from aruco_pose.msg import MarkerArray
+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')
+get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry)
+navigate = handle_response(rospy.ServiceProxy('navigate', srv.Navigate))
+land = handle_response(rospy.ServiceProxy('land', Trigger))
+
+def interrupt(sig, frame):
+    print('\nInterrupted, landing...')
+    land()
+    sys.exit(0)
+
+signal.signal(signal.SIGINT, interrupt)
+
+def print_current_map_position():
+    telem = get_telemetry()
+    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, \
+        frame_id='body', tolerance=0.2, auto_arm=False):
+
+    res = navigate(x=x, y=y, z=z, yaw=yaw, yaw_rate=yaw_rate, speed=speed, \
+        frame_id=frame_id, auto_arm=auto_arm)
+
+    if not res.success:
+        return res
+
+    while not rospy.is_shutdown():
+        telem = get_telemetry(frame_id='navigate_target')
+        if math.sqrt(telem.x ** 2 + telem.y ** 2 + telem.z ** 2) < tolerance:
+            return res
+        rospy.sleep(0.2)
+
+markers = rospy.wait_for_message('aruco_map/map', MarkerArray, timeout=3)
+left = min(marker.pose.position.x for marker in markers.markers)
+bottom = min(marker.pose.position.y for marker in markers.markers)
+width = max(marker.pose.position.x for marker in markers.markers)
+height = max(marker.pose.position.y for marker in markers.markers)
+center_x = left + width / 2
+center_y = bottom + height / 2
+
+print('Map rect: %g %g - %g %g' % (left, bottom, width, height))
+
+input('Take off and hover 1 m [enter] ')
+navigate_wait(x=0, y=0, z=1, frame_id='body', auto_arm=True)
+print_current_map_position()
+
+input('Go to corner %g %g 1.5 speed 1 [enter] ' % (width, height))
+navigate_wait(x=width, y=height, z=1.5, speed=1, frame_id='aruco_map')
+print_current_map_position()
+
+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] ')
+flow_client.update_configuration({'enabled': False})
+rospy.sleep(5)
+
+input('Enable optical flow back [enter] ')
+flow_client.update_configuration({'enabled': True})
+
+input('Go to side 1 %g 2 heading top [enter] ' % (center_y))
+navigate_wait(x=1, y=center_y, z=2, yaw=1.57, frame_id='aruco_map')
+print_current_map_position()
+
+marker_id = markers.markers[0].id
+input('Go to marker %d z=1.5 [enter] ' % marker_id)
+navigate_wait(x=0, y=0, z=1.5, yaw=0, frame_id='aruco_%d' % marker_id)
+print_current_map_position()
+
+input('Perform landing [enter] ')
+land()

clover/src/autotest/autotest_flight.py

@@ -0,0 +1,100 @@
+#!/usr/bin/env python3
+
+import rospy
+import math
+from math import nan
+import signal
+import sys
+from clover import srv
+from std_srvs.srv import Trigger
+from sensor_msgs.msg import Range
+from util import handle_response
+
+rospy.init_node('autotest_flight', disable_signals=True) # disable signals to allow interrupting with ctrl+c
+
+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_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))
+set_rates = handle_response(rospy.ServiceProxy('set_rates', srv.SetRates))
+land = handle_response(rospy.ServiceProxy('land', Trigger))
+
+def interrupt(sig, frame):
+    print('\nInterrupted, landing...')
+    land()
+    sys.exit(0)
+
+signal.signal(signal.SIGINT, interrupt)
+
+def navigate_wait(x=0, y=0, z=0, yaw=nan, yaw_rate=0, speed=0.5, \
+        frame_id='body', tolerance=0.2, auto_arm=False):
+
+    res = navigate(x=x, y=y, z=z, yaw=yaw, yaw_rate=yaw_rate, speed=speed, \
+        frame_id=frame_id, auto_arm=auto_arm)
+
+    if not res.success:
+        return res
+
+    while not rospy.is_shutdown():
+        telem = get_telemetry(frame_id='navigate_target')
+        if math.sqrt(telem.x ** 2 + telem.y ** 2 + telem.z ** 2) < tolerance:
+            return res
+        rospy.sleep(0.2)
+
+def print_distance():
+    dist = rospy.wait_for_message('rangefinder/range', Range).range
+    print('Distance: {:.2f}'.format(dist))
+
+input('Take off and hover 1 m [enter] ')
+navigate_wait(z=1, frame_id='body', auto_arm=True)
+print_distance()
+start = get_telemetry()
+
+input('Fly forward 2 m [enter] ')
+navigate_wait(x=2, frame_id='navigate_target')
+print_distance()
+
+input('Climb 0.5 m [enter] ')
+navigate_wait(z=0.5, frame_id='navigate_target')
+print_distance()
+
+input('Rotate left 90° [enter] ')
+navigate(yaw=math.pi / 2, frame_id='navigate_target')
+rospy.sleep(3)
+
+input('Use set_velocity to fly forward 2 m speed 1 [enter]')
+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] ')
+navigate(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')
+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')
+rospy.sleep(0.5)
+set_position(frame_id='body')
+
+input('Use set_rates to fly right [enter]')
+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]')
+set_position(x=nan, y=nan, z=nan, frame_id='body', yaw=nan, yaw_rate=-1)
+rospy.sleep(2 * math.pi)
+set_position(frame_id='body')
+
+input('Return to start point [enter]')
+navigate_wait(x=start.x, y=start.y, z=start.z, yaw=start.yaw, speed=1, frame_id='map')
+
+input('Land [enter]')
+land()

clover/src/autotest/autotest_led.py

@@ -0,0 +1,72 @@
+#!/usr/bin/env python3
+
+import rospy
+import functools
+from clover.srv import SetLEDEffect
+from led_msgs.srv import SetLEDs
+from led_msgs.msg import LEDStateArray, LEDState
+from util import handle_response
+
+rospy.init_node('autotest_led', disable_signals=True)
+
+set_leds = handle_response(rospy.ServiceProxy('led/set_leds', SetLEDs))
+set_effect = handle_response(rospy.ServiceProxy('led/set_effect', SetLEDEffect))
+
+led_count = len(rospy.wait_for_message('led/state', LEDStateArray, timeout=10).leds)
+print('LED count =', led_count)
+
+print('== Testing effects ==')
+
+input('Fill red [enter] ')
+set_effect(r=255, g=0, b=0)
+
+input('Fill green [enter] ')
+set_effect(r=0, g=100, b=0)
+
+input('Blink white [enter] ')
+set_effect(effect='blink', r=255, g=255, b=255)
+rospy.sleep(3)
+
+input('Blink fast violet [enter] ')
+set_effect(effect='blink_fast', r=220, g=20, b=250)
+rospy.sleep(3)
+
+input('Fade to blue [enter] ')
+set_effect(effect='fade', r=0, g=0, b=255)
+
+input('Wipe to yellow [enter] ')
+set_effect(effect='wipe', r=255, g=255, b=40)
+
+input('Flash red [enter] ')
+set_effect(effect='flash', r=255, g=0, b=0)
+rospy.sleep(1)
+
+input('Rainbow [enter] ')
+set_effect(effect='rainbow')
+rospy.sleep(4)
+
+input('Rainbow fill [enter] ')
+set_effect(effect='rainbow_fill')
+rospy.sleep(4)
+
+input('Turn off [enter] ')
+set_effect()
+
+print('== Testing low-level control ==')
+
+input('Fill orange [enter] ')
+set_leds(leds=[LEDState(index=i, r=245, g=155, b=0) for i in range(led_count)])
+
+input('Fill blue gradient [enter] ')
+set_leds(leds=[LEDState(index=i, r=0, g=20, b=int(255 * i / led_count)) for i in range(led_count)])
+
+input('Animate green dot [enter] ')
+set_effect()
+for i in range(led_count):
+    if i > 0:
+        set_leds(leds=[LEDState(index=i - 1, r=0, g=0, b=0)])
+    set_leds(leds=[LEDState(index=i, r=0, g=255, b=0)])
+    rospy.sleep(0.05)
+
+input('Turn off [enter] ')
+set_effect()

clover/src/autotest/util.py

@@ -0,0 +1,11 @@
+import functools
+
+# decorator to handle response and print error message
+def handle_response(fn):
+    @functools.wraps(fn)
+    def wrapper(*args, **kwargs):
+        res = fn(*args, **kwargs)
+        if not res.success:
+            print('\033[91mError:\033[0m {}'.format(res.message))
+        return res
+    return wrapper

clover/src/optical_flow.cpp

@@ -22,11 +22,13 @@
 #include <tf2/utils.h>
 #include <tf2_ros/transform_listener.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <dynamic_reconfigure/server.h>
 #include <mavros_msgs/OpticalFlowRad.h>
 #include <sensor_msgs/Imu.h>
 #include <geometry_msgs/Vector3Stamped.h>
 #include <geometry_msgs/PointStamped.h>
 #include <geometry_msgs/TwistStamped.h>
+#include <clover/FlowConfig.h>
 
 using cv::Mat;
 
@@ -38,6 +40,7 @@ public:
 	{}
 
 private:
+	bool enabled_;
 	ros::Publisher flow_pub_, velo_pub_, shift_pub_;
 	ros::Time prev_stamp_;
 	std::string fcu_frame_id_, local_frame_id_;
@@ -53,6 +56,8 @@ private:
 	std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
 	std::unique_ptr<tf2_ros::TransformListener> tf_listener_;
 	bool calc_flow_gyro_;
+	float flow_gyro_default_;
+	std::shared_ptr<dynamic_reconfigure::Server<clover::FlowConfig>> dyn_srv_;
 
 	void onInit()
 	{
@@ -69,6 +74,7 @@ private:
 		roi_px_ = nh_priv.param("roi", 128);
 		roi_rad_ = nh_priv.param("roi_rad", 0.0);
 		calc_flow_gyro_ = nh_priv.param("calc_flow_gyro", false);
+		flow_gyro_default_ = nh_priv.param("flow_gyro_default", NAN);
 
 		img_pub_ = it_priv.advertise("debug", 1);
 		flow_pub_ = nh.advertise<mavros_msgs::OpticalFlowRad>("mavros/px4flow/raw/send", 1);
@@ -81,6 +87,12 @@ private:
 
 		img_sub_ = it.subscribeCamera("image_raw", 1, &OpticalFlow::flow, this);
 
+		dyn_srv_ = std::make_shared<dynamic_reconfigure::Server<clover::FlowConfig>>(nh_priv);
+		dynamic_reconfigure::Server<clover::FlowConfig>::CallbackType cb;
+
+		cb = std::bind(&OpticalFlow::paramCallback, this, std::placeholders::_1, std::placeholders::_2);
+		dyn_srv_->setCallback(cb);
+
 		NODELET_INFO("Optical Flow initialized");
 	}
 
@@ -107,6 +119,8 @@ private:
 
 	void flow(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cinfo)
 	{
+		if (!enabled_) return;
+
 		parseCameraInfo(cinfo);
 
 		auto img = cv_bridge::toCvShare(msg, "mono8")->image;
@@ -194,9 +208,9 @@ private:
 			uint32_t integration_time_us = integration_time.toSec() * 1.0e6;
 
 			// Calculate flow gyro
-			flow_.integrated_xgyro = NAN;
-			flow_.integrated_ygyro = NAN;
-			flow_.integrated_zgyro = NAN;
+			flow_.integrated_xgyro = flow_gyro_default_;
+			flow_.integrated_ygyro = flow_gyro_default_;
+			flow_.integrated_zgyro = flow_gyro_default_;
 
 			if (calc_flow_gyro_) {
 				try {
@@ -222,7 +236,6 @@ private:
 			prev_ = curr_.clone();
 			prev_stamp_ = msg->header.stamp;
 
-publish_debug:
 			// Publish debug image
 			if (img_pub_.getNumSubscribers() > 0) {
 				// publish debug image
@@ -262,6 +275,14 @@ publish_debug:
 
 		return flow;
 	}
+
+	void paramCallback(clover::FlowConfig &config, [[maybe_unused]] uint32_t level)
+	{
+		enabled_ = config.enabled;
+		if (!enabled_) {
+			prev_ = Mat(); // clear previous frame
+		}
+	}
 };
 
 PLUGINLIB_EXPORT_CLASS(OpticalFlow, nodelet::Nodelet)

clover/src/selfcheck.py

@@ -30,6 +30,7 @@ from visualization_msgs.msg import MarkerArray as VisualizationMarkerArray
 import tf.transformations as t
 from aruco_pose.msg import MarkerArray
 from mavros import mavlink
+import locale
 
 
 # TODO: check attitude is present
@@ -43,6 +44,10 @@ from mavros import mavlink
 
 rospy.init_node('selfcheck')
 
+os.environ['ROSCONSOLE_FORMAT']='[${severity}]: ${message}'
+
+# use user's locale to convert numbers, etc
+locale.setlocale(locale.LC_ALL, '')
 
 tf_buffer = tf2_ros.Buffer()
 tf_listener = tf2_ros.TransformListener(tf_buffer)
@@ -193,24 +198,27 @@ def check_fcu():
             failure('no connection to the FCU (check wiring)')
             return
 
+        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')
 
-        r = re.compile(r'^FW (git tag|version): (v?\d\.\d\.\d.*)$')
-        is_clover_firmware = False
-        for ver_line in version_str.split('\n'):
-            match = r.search(ver_line)
-            if match is not None:
-                field, version = match.groups()
-                info('firmware %s: %s' % (field, version))
-                if 'clover' in version or 'clever' in version:
-                    is_clover_firmware = True
+        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 is_clover_firmware:
-            failure('not running Clover PX4 firmware, 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:
@@ -483,6 +491,12 @@ def check_local_position():
             failure('roll is %.2f deg; place copter horizontally or redo level horizon calib',
                     math.degrees(roll))
 
+        if not tf_buffer.can_transform('base_link', pose.header.frame_id, rospy.get_rostime(), rospy.Duration(0.5)):
+            failure('can\'t transform from %s to base_link (timeout 0.5 s): is TF enabled?', pose.header.frame_id)
+
+        if not tf_buffer.can_transform('body', pose.header.frame_id, rospy.get_rostime(), rospy.Duration(0.5)):
+            failure('can\'t transform from %s to body (timeout 0.5 s)', pose.header.frame_id)
+
     except rospy.ROSException:
         failure('no local position')
 
@@ -612,13 +626,13 @@ def check_boot_duration():
     output = subprocess.check_output('systemd-analyze').decode()
     r = re.compile(r'([\d\.]+)s\s*$', flags=re.MULTILINE)
     duration = float(r.search(output).groups()[0])
-    if duration > 15:
+    if duration > 20:
         failure('long Raspbian boot duration: %ss (systemd-analyze for analyzing)', duration)
 
 
 @check('CPU usage')
 def check_cpu_usage():
-    WHITELIST = 'nodelet',
+    WHITELIST = 'nodelet', 'gzclient', 'gzserver'
     CMD = "top -n 1 -b -i | tail -n +8 | awk '{ printf(\"%-8s\\t%-8s\\t%-8s\\n\", $1, $9, $12); }'"
     output = subprocess.check_output(CMD, shell=True).decode()
     processes = output.split('\n')
@@ -627,7 +641,7 @@ def check_cpu_usage():
             continue
         pid, cpu, cmd = process.split('\t')
 
-        if cmd.strip() not in WHITELIST and float(cpu) > 30:
+        if cmd.strip() not in WHITELIST and locale.atof(cpu) > 30:
             failure('high CPU usage (%s%%) detected: %s (PID %s)',
                     cpu.strip(), cmd.strip(), pid.strip())
 
@@ -646,13 +660,22 @@ def check_clover_service():
     elif 'failed' in output:
         failure('service failed to run, check your launch-files')
 
-    r = re.compile(r'^(.*)\[(FATAL|ERROR)\] \[\d+.\d+\]: (.*?)(\x1b(.*))?$')
+    BLACKLIST = 'Unexpected command 520', 'Time jump detected', 'different index:'
+
+    r = re.compile(r'^(.*)\[(FATAL|ERROR| WARN)\] \[\d+.\d+\]: (.*?)(\x1b(.*))?$')
     error_count = OrderedDict()
     try:
         for line in open('/tmp/clover.err', 'r'):
+            skip = False
+            for substr in BLACKLIST:
+                if substr in line:
+                    skip = True
+            if skip:
+                continue
+
             node_error = r.search(line)
             if node_error:
-                msg = node_error.groups()[1] + ': ' + node_error.groups()[2]
+                msg = node_error.groups()[1].strip() + ': ' + node_error.groups()[2]
                 if msg in error_count:
                     error_count[msg] += 1
                 else:
@@ -723,6 +746,14 @@ def check_network():
 
 @check('RPi health')
 def check_rpi_health():
+    try:
+        import shutil
+        total, used, free = shutil.disk_usage('/')
+        if free < 1024 * 1024 * 1024:
+            failure('disk space is less than 1 GB; consider removing logs (~/.ros/log/)')
+    except Exception as e:
+        info('could not check the disk free space: %s', str(e))
+
     # `vcgencmd get_throttled` output codes taken from
     # https://github.com/raspberrypi/documentation/blob/JamesH65-patch-vcgencmd-vcdbg-docs/raspbian/applications/vcgencmd.md#get_throttled
     # TODO: support more base platforms?
@@ -753,7 +784,7 @@ def check_rpi_health():
         # with some of the FLAGs OR'ed together
         output = subprocess.check_output(['vcgencmd', 'get_throttled']).decode()
     except OSError:
-        failure('could not call vcgencmd binary; not a Raspberry Pi?')
+        info('could not call vcgencmd binary; not a Raspberry Pi?')
         return
 
     throttle_mask = int(output.split('=')[1], base=16)

clover/src/simple_offboard.cpp

@@ -61,6 +61,7 @@ std::shared_ptr<tf2_ros::TransformBroadcaster> transform_broadcaster;
 std::shared_ptr<tf2_ros::StaticTransformBroadcaster> static_transform_broadcaster;
 
 // Parameters
+string mavros;
 string local_frame;
 string fcu_frame;
 ros::Duration transform_timeout;
@@ -181,6 +182,7 @@ inline bool waitTransform(const string& target, const string& source,
 		ros::spinOnce();
 		r.sleep();
 	}
+	return false;
 }
 
 #define TIMEOUT(msg, timeout) (msg.header.stamp.isZero() || (ros::Time::now() - msg.header.stamp > timeout))
@@ -501,10 +503,10 @@ inline void checkManualControl()
 
 	if (check_kill_switch) {
 		// switch values: https://github.com/PX4/PX4-Autopilot/blob/c302514a0809b1765fafd13c014d705446ae1113/msg/manual_control_setpoint.msg#L3
-		const uint8_t SWITCH_POS_NONE = 0; // switch is not mapped
-		const uint8_t SWITCH_POS_ON = 1; // switch activated
-		const uint8_t SWITCH_POS_MIDDLE = 2; // middle position
-		const uint8_t SWITCH_POS_OFF = 3; // switch not activated
+		[[maybe_unused]] const uint8_t SWITCH_POS_NONE = 0; // switch is not mapped
+		[[maybe_unused]] const uint8_t SWITCH_POS_ON = 1; // switch activated
+		[[maybe_unused]] const uint8_t SWITCH_POS_MIDDLE = 2; // middle position
+		[[maybe_unused]] const uint8_t SWITCH_POS_OFF = 3; // switch not activated
 
 		const int KILL_SWITCH_BIT = 12; // https://github.com/PX4/Firmware/blob/c302514a0809b1765fafd13c014d705446ae1113/src/modules/mavlink/mavlink_messages.cpp#L3975
 		uint8_t kill_switch = (manual_control.buttons & (0b11 << KILL_SWITCH_BIT)) >> KILL_SWITCH_BIT;
@@ -689,7 +691,7 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl
 		// }
 
 		if (sp_type == POSITION || sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == VELOCITY || sp_type == ATTITUDE) {
-			// destination point and/or yaw
+			// destination point and/or attitude
 			PoseStamped ps;
 			ps.header.frame_id = frame_id;
 			ps.header.stamp = stamp;
@@ -698,7 +700,12 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl
 			ps.pose.position.z = z;
 			ps.pose.orientation.w = 1.0; // Ensure quaternion is always valid
 
-			if (std::isnan(yaw)) {
+			if (sp_type == ATTITUDE) {
+				ps.pose.position.x = 0;
+				ps.pose.position.y = 0;
+				ps.pose.position.z = 0;
+				ps.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);
+			} else if (std::isnan(yaw)) {
 				setpoint_yaw_type = YAW_RATE;
 				setpoint_yaw_rate = yaw_rate;
 			} else if (std::isinf(yaw) && yaw > 0) {
@@ -801,7 +808,7 @@ 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);
 }
 
-bool land(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)
+bool land([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)
 {
 	try {
 		if (busy)
@@ -847,6 +854,7 @@ bool land(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)
 		busy = false;
 		return true;
 	}
+	return false;
 }
 
 int main(int argc, char **argv)
@@ -859,8 +867,9 @@ int main(int argc, char **argv)
 	static_transform_broadcaster = std::make_shared<tf2_ros::StaticTransformBroadcaster>();
 
 	// Params
-	nh.param<string>("mavros/local_position/tf/frame_id", local_frame, "map");
-	nh.param<string>("mavros/local_position/tf/child_frame_id", fcu_frame, "base_link");
+	nh_priv.param("mavros", mavros, string("mavros")); // for case of using multiple connections
+	nh.param<string>(mavros + "/local_position/tf/frame_id", local_frame, "map");
+	nh.param<string>(mavros + "/local_position/tf/child_frame_id", fcu_frame, "base_link");
 	nh_priv.param("target_frame", target.child_frame_id, string("navigate_target"));
 	nh_priv.param("setpoint", setpoint.child_frame_id, string("setpoint"));
 	nh_priv.param("auto_release", auto_release, true);
@@ -871,6 +880,13 @@ int main(int argc, char **argv)
 	nh_priv.param<string>("body_frame", body.child_frame_id, "body");
 	nh_priv.getParam("reference_frames", reference_frames);
 
+	// Default reference frames
+	std::map<string, string> default_reference_frames;
+	default_reference_frames[body.child_frame_id] = local_frame;
+	default_reference_frames[fcu_frame] = local_frame;
+	if (!target.child_frame_id.empty()) default_reference_frames[target.child_frame_id] = local_frame;
+	reference_frames.insert(default_reference_frames.begin(), default_reference_frames.end()); // merge defaults
+
 	state_timeout = ros::Duration(nh_priv.param("state_timeout", 3.0));
 	local_position_timeout = ros::Duration(nh_priv.param("local_position_timeout", 2.0));
 	velocity_timeout = ros::Duration(nh_priv.param("velocity_timeout", 2.0));
@@ -885,25 +901,25 @@ int main(int argc, char **argv)
 	arming_timeout = ros::Duration(nh_priv.param("arming_timeout", 4.0));
 
 	// Service clients
-	arming = nh.serviceClient<mavros_msgs::CommandBool>("mavros/cmd/arming");
-	set_mode = nh.serviceClient<mavros_msgs::SetMode>("mavros/set_mode");
+	arming = nh.serviceClient<mavros_msgs::CommandBool>(mavros + "/cmd/arming");
+	set_mode = nh.serviceClient<mavros_msgs::SetMode>(mavros + "/set_mode");
 
 	// Telemetry subscribers
-	auto state_sub = nh.subscribe("mavros/state", 1, &handleState);
-	auto velocity_sub = nh.subscribe("mavros/local_position/velocity_body", 1, &handleMessage<TwistStamped, velocity>);
-	auto global_position_sub = nh.subscribe("mavros/global_position/global", 1, &handleMessage<NavSatFix, global_position>);
-	auto battery_sub = nh.subscribe("mavros/battery", 1, &handleMessage<BatteryState, battery>);
-	auto statustext_sub = nh.subscribe("mavros/statustext/recv", 1, &handleMessage<mavros_msgs::StatusText, statustext>);
-	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);
+	auto state_sub = nh.subscribe(mavros + "/state", 1, &handleState);
+	auto velocity_sub = nh.subscribe(mavros + "/local_position/velocity_body", 1, &handleMessage<TwistStamped, velocity>);
+	auto global_position_sub = nh.subscribe(mavros + "/global_position/global", 1, &handleMessage<NavSatFix, global_position>);
+	auto battery_sub = nh.subscribe(mavros + "/battery", 1, &handleMessage<BatteryState, battery>);
+	auto statustext_sub = nh.subscribe(mavros + "/statustext/recv", 1, &handleMessage<mavros_msgs::StatusText, statustext>);
+	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);
 
 	// Setpoint publishers
-	position_pub = nh.advertise<PoseStamped>("mavros/setpoint_position/local", 1);
-	position_raw_pub = nh.advertise<PositionTarget>("mavros/setpoint_raw/local", 1);
-	attitude_pub = nh.advertise<PoseStamped>("mavros/setpoint_attitude/attitude", 1);
-	attitude_raw_pub = nh.advertise<AttitudeTarget>("mavros/setpoint_raw/attitude", 1);
-	rates_pub = nh.advertise<TwistStamped>("mavros/setpoint_attitude/cmd_vel", 1);
-	thrust_pub = nh.advertise<Thrust>("mavros/setpoint_attitude/thrust", 1);
+	position_pub = nh.advertise<PoseStamped>(mavros + "/setpoint_position/local", 1);
+	position_raw_pub = nh.advertise<PositionTarget>(mavros + "/setpoint_raw/local", 1);
+	attitude_pub = nh.advertise<PoseStamped>(mavros + "/setpoint_attitude/attitude", 1);
+	attitude_raw_pub = nh.advertise<AttitudeTarget>(mavros + "/setpoint_raw/attitude", 1);
+	rates_pub = nh.advertise<TwistStamped>(mavros + "/setpoint_attitude/cmd_vel", 1);
+	thrust_pub = nh.advertise<Thrust>(mavros + "/setpoint_attitude/thrust", 1);
 
 	 // Service servers
 	auto gt_serv = nh.advertiseService("get_telemetry", &getTelemetry);

clover/src/vpe_publisher.cpp

@@ -38,9 +38,9 @@ TransformStamped offset;
 
 void publishZero(const ros::TimerEvent& e)
 {
-	if (e.current_real - vpe.header.stamp < publish_zero_timout) return; // have vpe
+	if (!vpe.header.stamp.isZero() && e.current_real - vpe.header.stamp < publish_zero_timout) return; // have vpe
 
-	if (e.current_real - pose.header.stamp < publish_zero_timout) { // have local position
+	if (!pose.header.stamp.isZero() && e.current_real - pose.header.stamp < publish_zero_timout) { // have local position
 		if (got_local_pos.isZero()) {
 			ROS_INFO("got local position");
 			got_local_pos = e.current_real;
@@ -109,7 +109,7 @@ void callback(const T& msg)
 	}
 }
 
-bool reset(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)
+bool reset([[maybe_unused]] std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)
 {
 	reset_flag = true;
 	res.success = true;
@@ -141,11 +141,11 @@ int main(int argc, char **argv) {
 	vpe_pub = nh_priv.advertise<PoseStamped>("vpe", 1);
 	//vpe_cov_pub = nh_priv_.advertise<PoseStamped>("pose_cov_pub", 1);
 
-	if (nh_priv.param("publish_zero", false)) {
+	if (nh_priv.param("force_init", false) || nh_priv.param("publish_zero", false)) { // publish_zero is old name
 		// publish zero to initialize the local position
 		zero_timer = nh.createTimer(ros::Duration(0.1), &publishZero);
-		publish_zero_timout = ros::Duration(nh_priv.param("publish_zero_timout", 5.0));
-		publish_zero_duration = ros::Duration(nh_priv.param("publish_zero_duration", 5.0));
+		publish_zero_timout = ros::Duration(nh_priv.param("force_init_timeout", 5.0));
+		publish_zero_duration = ros::Duration(nh_priv.param("force_init_duration", 5.0));
 		local_position_sub = nh.subscribe("mavros/local_position/pose", 1, &localPositionCallback);
 	}
 

clover/test/basic.py

@@ -33,3 +33,29 @@ def test_web_video_server(node):
         # Python 3
         import urllib.request as urllib
     urllib.urlopen("http://localhost:8080").read()
+
+def test_blocks(node):
+    rospy.wait_for_service('clover_blocks/run', timeout=5)
+    rospy.wait_for_service('clover_blocks/stop', timeout=5)
+    rospy.wait_for_service('clover_blocks/load', timeout=5)
+    rospy.wait_for_service('clover_blocks/store', timeout=5)
+
+    from std_msgs.msg import String
+    from clover_blocks.srv import Run
+
+    def wait_print():
+        try:
+            wait_print.result = rospy.wait_for_message('clover_blocks/print', String, timeout=5).data
+        except Exception as e:
+            wait_print.result = str(e)
+
+    import threading
+    t = threading.Thread(target=wait_print)
+    t.start()
+    rospy.sleep(0.1)
+
+    run = rospy.ServiceProxy('clover_blocks/run', Run)
+    assert run(code='print("test")').success == True
+
+    t.join()
+    assert wait_print.result == 'test'

clover/test/basic.test

@@ -23,10 +23,7 @@
 
     <node pkg="tf2_ros" type="static_transform_publisher" name="map_flipped_frame" args="0 0 0 3.1415926 3.1415926 0 map map_flipped" required="true"/>
 
-    <node name="simple_offboard" pkg="clover" type="simple_offboard" required="true" output="screen">
-        <param name="reference_frames/body" value="map"/>
-        <param name="reference_frames/base_link" value="map"/>
-    </node>
+    <node name="simple_offboard" pkg="clover" type="simple_offboard" required="true" output="screen"/>
 
     <node name="tf2_web_republisher" pkg="tf2_web_republisher" type="tf2_web_republisher" required="true"/>
 
@@ -38,6 +35,8 @@
         <rosparam param="notify">startup: { r: 255, g: 255, b: 255 }</rosparam>
     </node>
 
+    <node name="clover_blocks" pkg="clover_blocks" type="clover_blocks" output="screen" 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/udev/99-px4fmu.rules

@@ -12,4 +12,6 @@ SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0016", ATTRS{produ
 SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0013", ATTRS{product}=="PX4 FMU v4.x PRO", SYMLINK+="px4fmu"
 # Omnibus
 SUBSYSTEM=="tty", ATTRS{idVendor}=="26ac", ATTRS{idProduct}=="0001", ATTRS{product}=="PX4 OmnibusF4SD", SYMLINK+="px4fmu"
+# CUAV X7 Pro
+SUBSYSTEM=="tty", ATTRS{idVendor}=="3163", ATTRS{idProduct}=="004c", ATTRS{product}=="PX4 CUAV X7Pro", SYMLINK+="px4fmu"
 

clover/www/console.html

@@ -0,0 +1,23 @@
+<h1>
+	/var/log/clover.log
+	<a style="font-size: 0.5em; vertical-align: super; font-weight: normal" href="clover.log" download>download</a>
+</h1>
+
+<pre></pre>
+
+<script type="module">
+	var pre = document.querySelector('pre');
+
+	fetch('clover.log?' + Math.random()).then(function(response) { // random to forbid caching
+		if (response.status == 404) {
+			pre.innerHTML = '/var/log/clover.log does not exist';
+			return;
+		} else if (response.status !== 200) {
+			pre.innerHTML('Error ' + response.status);
+			return;
+		}
+		response.text().then(function(content) {
+			pre.innerHTML = content;
+		});
+	});
+</script>

clover/www/index.html

@@ -9,7 +9,7 @@
 	<li><a href="" id="butterfly">Open web terminal</a> (<code>Butterfly</code>)</li>
 	<li>View <a href="viz.html">View 3D visualization</a>, <a href="aruco_map.html">3D visualization for markers map</a> (<code>ros3djs</code>)</li>
 	<li><a href="../clover_blocks/">Blocks programming</a> (<code>Blockly</code>)</li>
-	<li><a href="clover.log">Clover console</a> (<code>/var/log/clover.log</code>)</li>
+	<li><a href="console.html">Clover console</a> (<code>/var/log/clover.log</code>)</li>
 </ul>
 
 <div class="version"></div>

clover/www/js/topics.js

@@ -1,13 +1,16 @@
 const url = 'ws://' + location.hostname + ':9090';
 const ros = new ROSLIB.Ros({ url: url });
+const params = Object.fromEntries(new URLSearchParams(window.location.search).entries());
 
 ros.on('connection', function () {
 	document.body.classList.add('connected');
+	document.body.classList.remove('closed');
 	init();
 });
 
 ros.on('close', function () {
 	document.body.classList.remove('connected');
+	document.body.classList.add('closed');
 	setTimeout(function() {
 		// reconnect
 		ros.connect(url);
@@ -37,18 +40,28 @@ function viewTopicsList() {
 let rosdistro;
 
 function viewTopic(topic) {
-	title.innerHTML = topic;
+	let index = '<a href=topics.html>Topics</a>';
+	title.innerHTML = `${index}: ${topic}`;
 	topicMessage.style.display = 'block';
 
 	ros.getTopicType(topic, function(typeStr) {
 		const [pack, type] = typeStr.split('/');
 		let href = `https://docs.ros.org/en/${rosdistro}/api/${pack}/html/msg/${type}.html`;
-		title.innerHTML = `${topic} <a id="topic-type" href=${href} target="_blank">${typeStr}</a>`;
+		title.innerHTML = `${index}: ${topic} <a id="topic-type" href=${href} target="_blank">${typeStr}</a>`;
 	});
 
 	new ROSLIB.Topic({ ros: ros, name: topic }).subscribe(function(msg) {
 		document.title = topic;
 		if (mouseDown) return;
+
+		if (msg.header.stamp) {
+			if (params.date || params.offset) {
+				let date = new Date(msg.header.stamp.secs * 1e3 + msg.header.stamp.nsecs * 1e-6);
+				if (params.date) msg.header.date = date.toISOString();
+				if (params.offset) msg.header.offset = (new Date() - date) * 1e-3;
+			}
+		}
+
 		topicMessage.innerHTML = yamlStringify(msg); // JSON.stringify(msg, null, 4);
 	});
 }
@@ -59,8 +72,6 @@ topicMessage.addEventListener('mousedown', function() { mouseDown = true; });
 topicMessage.addEventListener('mouseup', function() { mouseDown = false; });
 
 function init() {
-	const params = Object.fromEntries(new URLSearchParams(window.location.search).entries());
-
 	if (!params.topic) {
 		viewTopicsList();
 	} else {

clover/www/topics.html

@@ -17,11 +17,12 @@
 			}
 			#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); }
 		</style>
 	</head>
 	<body>
 		<h1>&nbsp;</h1>
 		<ul id="topics"></ul>
-		<code id="topic-message" title="Hold mouse button to pause">No messages received</code>
+		<code id="topic-message">No messages received</code>
 	</body>
 </html>

clover_blocks/package.xml

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

clover_blocks/www/main.js

@@ -211,7 +211,7 @@ function loadPrograms() {
 		updateChanged();
 	}, function(err) {
 		document.querySelector('.backend-fail').style.display = 'inline';
-		alert(`Error loading programs list.\n\nHave you enabled clover_blocks in clover.launch?`);
+		alert(`Error loading programs list.\n\nHave you enabled 'blocks' in clover.launch?`);
 		runButton.disabled = true;
 	})
 }

clover_blocks/www/python.js

@@ -464,7 +464,7 @@ Blockly.Python.led_count = function(block) {
 
 function pigpio() {
 	Blockly.Python.definitions_['import_pigpio'] = 'import pigpio';
-	Blockly.Python.definitions_['init_pigpio'] = 'pi = pigpio.pi()';
+	Blockly.Python.definitions_['init_pigpio'] = 'pi = pigpio.pi()\nif not pi.connected: raise Exception(\'Cannot connect to pigpiod\')';
 }
 
 const GPIO_READ = `\ndef gpio_read(pin):

clover_description/package.xml

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

clover_description/urdf/clover/clover4_physics.xacro

@@ -35,7 +35,7 @@
   <xacro:property name="sqrt2" value="1.4142135623730951" />
   <xacro:property name="rotor_drag_coefficient" value="1.75e-04" />
   <xacro:property name="rolling_moment_coefficient" value="0.000001" />
-  <xacro:property name="color" value="$(arg visual_material)" />
+  <xacro:property name="color" value="DarkGrey" />
 
   <!-- Property Blocks -->
   <!-- Clover body inertia -->

clover_description/urdf/leds/led_strip.xacro

@@ -64,6 +64,12 @@
         <!-- <gazebo>
             <static>true</static>
         </gazebo> -->
+        <gazebo>
+            <plugin name="${name}_ros_controller" filename="libsim_leds_controller.so">
+                <robotNamespace></robotNamespace>
+                <ledCount>${led_count}</ledCount>
+            </plugin>
+        </gazebo>
     </xacro:macro>
 
 </robot>

clover_simulation/CMakeLists.txt

@@ -37,6 +37,14 @@ target_compile_options(sim_leds PRIVATE -std=c++11)
 
 add_dependencies(sim_leds ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
+add_library(sim_leds_controller src/sim_leds_controller.cpp)
+
+target_include_directories(sim_leds_controller PRIVATE ${catkin_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS})
+target_link_libraries(sim_leds_controller PRIVATE ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES})
+target_compile_options(sim_leds_controller PRIVATE -std=c++11)
+
+add_dependencies(sim_leds_controller ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
 # Gazebo throttling camera plugin
 # for some reason, CMake does not support per-target link directories, and Gazebo does not put
 # CameraPlugin into ${GAZEBO_LIBRARIES}

clover_simulation/airframes/4500_clover

@@ -21,11 +21,11 @@ param set LPE_VIS_Z 0.1
 param set LPE_FUSION 86
 
 param set SENS_FLOW_ROT 0
-param set SENS_FLOW_MINHGT 0.01
+param set SENS_FLOW_MINHGT 0.0
 param set SENS_FLOW_MAXHGT 4.0
 param set SENS_FLOW_MAXR 10.0
 
-param set EKF2_AID_MASK 27 # gps + flow + vis pos + vis yaw
+param set EKF2_AID_MASK 26 # flow + vis pos + vis yaw
 param set EKF2_OF_DELAY 0
 param set EKF2_OF_QMIN 10
 param set EKF2_OF_N_MIN 0.05

clover_simulation/launch/simulator.launch

@@ -4,6 +4,7 @@
     <arg name="est" default="ekf2"/> <!-- PX4 estimator: lpe, ekf2 -->
     <arg name="vehicle" default="clover"/> <!-- PX4 vehicle configuration: clover, clover_vpe -->
     <arg name="main_camera" default="true"/> <!-- Simulated vision position estimation camera (optical flow, ArUco) -->
+    <arg name="maintain_camera_rate" default="false"/> <!-- Slow simulation down to maintain camera rate -->
     <arg name="rangefinder" default="true"/> <!-- Simulated downward-facing rangefinder, vl53l1x-like -->
     <arg name="led" default="true"/> <!-- Simulated LED strip, ws281x-like -->
     <arg name="gps" default="false"/> <!--Simulated GPS module -->
@@ -28,6 +29,7 @@
     <!-- Clover model -->
     <include file="$(find clover_description)/launch/spawn_drone.launch" if="$(eval type == 'gazebo')">
         <arg name="main_camera" value="$(arg main_camera)"/>
+        <arg name="maintain_camera_rate" value="$(arg maintain_camera_rate)"/>
         <arg name="rangefinder" value="$(arg rangefinder)"/>
         <arg name="led" value="$(arg led)"/>
         <arg name="gps" value="$(arg gps)"/>

clover_simulation/models/aruco_100/marker_100.sdf

@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<sdf version="1.5">
+    <model name="aruco_100">
+        <static>true</static>
+        <link name="marker_100_link">
+            <pose>0 0 1e-3 0 0 1.5707963267948966</pose>
+            <visual name="visual_marker_100">
+                <cast_shadows>false</cast_shadows>
+                <geometry>
+                    <box>
+                        <size>0.22 0.22 1e-3</size>
+                    </box>
+                </geometry>
+                <material>
+                    <script>
+                        <uri>model://aruco_100/materials/scripts</uri>
+                        <uri>model://aruco_100/materials/textures</uri>
+                        <name>aruco/marker_100</name>
+                    </script>
+                </material>
+            </visual>
+        </link>
+    </model>
+</sdf>

clover_simulation/models/aruco_100/materials/scripts/aruco_100.material

@@ -0,0 +1,15 @@
+material aruco/marker_100
+{
+    technique
+    {
+        pass
+        {
+            texture_unit
+            {
+                texture aruco_marker_100.png
+                filtering none
+                scale 1.0 1.0
+            }
+        }
+    }
+}

clover_simulation/models/aruco_100/model.config

@@ -0,0 +1,13 @@
+<?xml version="1.0"?>
+<model>
+    <name>ArUco Marker 100</name>
+    <version>1.0</version>
+    <sdf version="1.5">marker_100.sdf</sdf>
+    <author>
+        <name>Marker Generator script</name>
+        <email>marker@generator.sh</email>
+    </author>
+    <description>
+        ArUco marker #100
+    </description>
+</model>

clover_simulation/models/aruco_101/marker_101.sdf

@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<sdf version="1.5">
+    <model name="aruco_101">
+        <static>true</static>
+        <link name="marker_101_link">
+            <pose>0 0 1e-3 0 0 1.5707963267948966</pose>
+            <visual name="visual_marker_101">
+                <cast_shadows>false</cast_shadows>
+                <geometry>
+                    <box>
+                        <size>0.44 0.44 1e-3</size>
+                    </box>
+                </geometry>
+                <material>
+                    <script>
+                        <uri>model://aruco_101/materials/scripts</uri>
+                        <uri>model://aruco_101/materials/textures</uri>
+                        <name>aruco/marker_101</name>
+                    </script>
+                </material>
+            </visual>
+        </link>
+    </model>
+</sdf>

clover_simulation/models/aruco_101/materials/scripts/aruco_101.material

@@ -0,0 +1,15 @@
+material aruco/marker_101
+{
+    technique
+    {
+        pass
+        {
+            texture_unit
+            {
+                texture aruco_marker_101.png
+                filtering none
+                scale 1.0 1.0
+            }
+        }
+    }
+}

clover_simulation/models/aruco_101/model.config

@@ -0,0 +1,13 @@
+<?xml version="1.0"?>
+<model>
+    <name>ArUco Marker 101</name>
+    <version>1.0</version>
+    <sdf version="1.5">marker_101.sdf</sdf>
+    <author>
+        <name>Marker Generator script</name>
+        <email>marker@generator.sh</email>
+    </author>
+    <description>
+        ArUco marker #101
+    </description>
+</model>

clover_simulation/models/aruco_102/marker_102.sdf

@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<sdf version="1.5">
+    <model name="aruco_102">
+        <static>true</static>
+        <link name="marker_102_link">
+            <pose>0 0 1e-3 0 0 1.5707963267948966</pose>
+            <visual name="visual_marker_102">
+                <cast_shadows>false</cast_shadows>
+                <geometry>
+                    <box>
+                        <size>0.44 0.44 1e-3</size>
+                    </box>
+                </geometry>
+                <material>
+                    <script>
+                        <uri>model://aruco_102/materials/scripts</uri>
+                        <uri>model://aruco_102/materials/textures</uri>
+                        <name>aruco/marker_102</name>
+                    </script>
+                </material>
+            </visual>
+        </link>
+    </model>
+</sdf>

clover_simulation/models/aruco_102/materials/scripts/aruco_102.material

@@ -0,0 +1,15 @@
+material aruco/marker_102
+{
+    technique
+    {
+        pass
+        {
+            texture_unit
+            {
+                texture aruco_marker_102.png
+                filtering none
+                scale 1.0 1.0
+            }
+        }
+    }
+}

clover_simulation/models/aruco_102/model.config

@@ -0,0 +1,13 @@
+<?xml version="1.0"?>
+<model>
+    <name>ArUco Marker 102</name>
+    <version>1.0</version>
+    <sdf version="1.5">marker_102.sdf</sdf>
+    <author>
+        <name>Marker Generator script</name>
+        <email>marker@generator.sh</email>
+    </author>
+    <description>
+        ArUco marker #102
+    </description>
+</model>

clover_simulation/models/camera/camera.sdf

@@ -0,0 +1,33 @@
+<?xml version="1.0"?>
+<sdf version="1.5">
+    <model name="camera">
+        <static>true</static>
+        <link name='camera_link'>
+            <pose>0 0 0 0 0 0</pose>
+            <sensor name='camera' type='camera'>
+                <camera>
+                    <horizontal_fov>1.8</horizontal_fov>
+                    <image>
+                        <format>B8G8R8</format>
+                        <width>640</width>
+                        <height>480</height>
+                    </image>
+                    <clip>
+                        <near>0.02</near>
+                        <far>300</far>
+                    </clip>
+                </camera>
+                <always_on>1</always_on>
+                <update_rate>30</update_rate>
+                <visualize>1</visualize>
+                <plugin name='camera_plugin' filename='libgazebo_ros_camera.so'>
+                    <alwaysOn>1</alwaysOn>
+                    <imageTopicName>image_raw</imageTopicName>
+                    <cameraTopicName>camera_info</cameraTopicName>
+                    <cameraName>camera</cameraName>
+                    <frameName>/camera</frameName>
+                </plugin>
+            </sensor>
+        </link>
+    </model>
+</sdf>

clover_simulation/models/camera/model.config

@@ -0,0 +1,13 @@
+<?xml version="1.0"?>
+<model>
+    <name>Camera</name>
+    <version>1.0</version>
+    <sdf version="1.5">camera.sdf</sdf>
+    <author>
+        <name>Oleg Kalachev</name>
+        <email>okalachev@gmail.com</email>
+    </author>
+    <description>
+        External camera
+    </description>
+</model>

clover_simulation/package.xml

@@ -1,6 +1,6 @@
 <package format="2">
     <name>clover_simulation</name>
-    <version>0.21.1</version>
+    <version>0.23.0</version>
     <description>The clover_simulation package provides worlds and launch files for Gazebo.</description>
 
     <maintainer email="okalachev@gmail.com">Oleg Kalachev</maintainer>

clover_simulation/scripts/gzweb

@@ -0,0 +1,8 @@
+#!/usr/bin/env bash
+
+# script for running gzweb
+# usage: ./gzweb [<port>]
+
+export NVM_DIR=$HOME/.nvm
+source $NVM_DIR/nvm.sh
+npm start --prefix $HOME/gzweb -p ${1-8080}

clover_simulation/src/sim_leds.cpp

@@ -49,14 +49,9 @@ private:
 
 	std::unique_ptr<ros::NodeHandle> nh;
 
-	ros::ServiceServer setLedsSrv;
-	// Note: LED state should only be published by the /gazebo node
-	led_msgs::LEDStateArray ledState;
-	ros::Publisher statePublisher;
 	// LED state will be read from the topic to avoid creating more services
 	ros::Subscriber stateSubscriber;
 
-	bool setLeds(led_msgs::SetLEDs::Request& req, led_msgs::SetLEDs::Response& resp);
 	void handleLedsMsg(const led_msgs::LEDStateArrayConstPtr& leds);
 
 public:
@@ -73,16 +68,8 @@ public:
 		ROS_INFO_NAMED(("LedController_" + robotNamespace).c_str(), "LedController has started (as %s)", role == Role::Client ? "client" : "server");
 
 		nh.reset(new ros::NodeHandle(robotNamespace));
-		if (role == Role::Server)
-		{
-			setLedsSrv = nh->advertiseService("led/set_leds", &LedController::setLeds, this);
-			statePublisher = nh->advertise<led_msgs::LEDStateArray>("led/state", 1, true);
-		}
-		else
-		{
-			// LED state should be published to the "led/state" topic, so we grab our data there
-			stateSubscriber = nh->subscribe<led_msgs::LEDStateArray>("led/state", 1, &LedController::handleLedsMsg, this);
-		}
+
+		stateSubscriber = nh->subscribe<led_msgs::LEDStateArray>("led/state", 1, &LedController::handleLedsMsg, this);
 	};
 
 	~LedController()
@@ -96,13 +83,9 @@ public:
 		std::lock_guard<std::mutex> lock(registryMutex);
 		if (totalLeds > 0) {
 			registeredLeds.resize(totalLeds);
-			ledState.leds.resize(totalLeds);
 		}
 		ROS_DEBUG_NAMED(("LedController_" + robotNamespace).c_str(), "Registering LED visual plugin to %s (LED id=%d)", (role == Role::Client) ? "client" : "server", ledIdx);
 		registeredLeds[ledIdx] = plugin;
-		ledState.leds[ledIdx].index = ledIdx;
-		if (role == Role::Server)
-			statePublisher.publish(ledState);
 	}
 
 	void unregisterPlugin(sim_led::LedVisualPlugin* plugin)
@@ -157,7 +140,8 @@ public:
 		{
 			auto indexStr = parentName.substr(lastDashPos + 1);
 			try {
-				myIndex = std::stoi(indexStr);
+				if (indexStr == "visual") myIndex = 0; // the first visual doesn't have index
+				else myIndex = std::stoi(indexStr);
 			} catch(const std::exception &e) {
 				gzwarn << "Failed to convert " << indexStr << " to integer: " << e.what() <<  ", assuming 0\n";
 				myIndex = 0;
@@ -195,26 +179,6 @@ public:
 };
 }
 
-// FIXME: These two functions basically do the same thing, maybe they can be merged?
-bool led_controller::LedController::setLeds(led_msgs::SetLEDs::Request &req, led_msgs::SetLEDs::Response &resp)
-{
-	std::lock_guard<std::mutex> lock(registryMutex);
-	for(const auto& led : req.leds)
-	{
-		if (led.index < registeredLeds.size()) {
-			auto color = GazeboColor(led.r / 255.0f, led.g / 255.0f, led.b / 255.0f);
-			auto ledPlugin = registeredLeds[led.index];
-			if (ledPlugin) ledPlugin->SetColor(color);
-			ledState.leds[led.index].r = led.r;
-			ledState.leds[led.index].g = led.g;
-			ledState.leds[led.index].b = led.b;
-		}
-	}
-	statePublisher.publish(ledState);
-	resp.success = true;
-	return true;
-}
-
 void led_controller::LedController::handleLedsMsg(const led_msgs::LEDStateArrayConstPtr& leds)
 {
 	std::lock_guard<std::mutex> lock(registryMutex);

clover_simulation/src/sim_leds_controller.cpp

@@ -0,0 +1,71 @@
+#include <led_msgs/SetLEDs.h>
+#include <led_msgs/LEDStateArray.h>
+
+#include <ros/ros.h>
+
+#include <gazebo/gazebo.hh>
+#include <gazebo/physics/physics.hh>
+#include <gazebo/common/common.hh>
+
+class LedControllerPlugin : public gazebo::ModelPlugin {
+private:
+	std::unique_ptr<ros::NodeHandle> nh;
+	std::string ns;
+	ros::ServiceServer setLedsSrv;
+	led_msgs::LEDStateArray ledState;
+	ros::Publisher statePublisher;
+	std::mutex handleMutex;
+
+public:
+	bool setLeds(led_msgs::SetLEDs::Request &req, led_msgs::SetLEDs::Response &resp)
+	{
+		std::lock_guard<std::mutex> lock(handleMutex);
+		for(const auto& led : req.leds)
+		{
+			if (led.index < ledState.leds.size()) {
+				ledState.leds[led.index].r = led.r;
+				ledState.leds[led.index].g = led.g;
+				ledState.leds[led.index].b = led.b;
+			}
+		}
+		statePublisher.publish(ledState);
+		resp.success = true;
+		return true;
+	}
+
+	virtual void Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf) override
+	{
+		ROS_INFO("Initialize LED Controller");
+
+		// We need "libgazebo_ros_api.so" to be loaded
+		if (!ros::isInitialized())
+		{
+			ROS_FATAL_NAMED("LedController", "Tried to load ROS plugin when ROS Gazebo API is not loaded. Please use gazebo_ros node to"
+			                                 "launch Gazebo.");
+		}
+
+		ns = "";
+
+		if (sdf->HasElement("robotNamespace")) {
+			ns = sdf->Get<std::string>("robotNamespace");
+		}
+		if (!sdf->HasElement("ledCount")) {
+			gzerr << "ledCount is not set, but is required for the plugin to function correctly\n";
+			return;
+		}
+		int totalLeds = sdf->Get<int>("ledCount");
+		ledState.leds.resize(totalLeds);
+		for (int i = 0; i < totalLeds; i++) {
+			ledState.leds[i].index = i;
+		}
+
+		nh.reset(new ros::NodeHandle(ns));
+
+		setLedsSrv = nh->advertiseService("led/set_leds", &LedControllerPlugin::setLeds, this);
+		statePublisher = nh->advertise<led_msgs::LEDStateArray>("led/state", 1, true);
+
+		statePublisher.publish(ledState);
+	}
+};
+
+GZ_REGISTER_MODEL_PLUGIN(LedControllerPlugin);