Merge branch 'master' into v0.24-release

.github/workflows/docs.yml

@@ -11,10 +11,6 @@ permissions:
   pages: write
   id-token: write
 
-concurrency:
-  group: "pages"
-  cancel-in-progress: true
-
 defaults:
   run:
     shell: bash
@@ -75,6 +71,9 @@ jobs:
 
   deploy-docs:
     if: ${{ github.event_name == 'push' && github.ref == 'refs/heads/master' }}
+    concurrency:
+        group: "pages"
+        cancel-in-progress: true
     environment:
       name: github-pages
       url: ${{ steps.deployment.outputs.page_url }}

aruco_pose/package.xml

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

builder/image-ros.sh

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

builder/test/tests.py

@@ -2,6 +2,7 @@
 
 # validate all required modules installed
 
+import os
 import rospy
 from geometry_msgs.msg import PoseStamped
 from sensor_msgs.msg import Range, BatteryState
@@ -22,6 +23,7 @@ from clover.srv import GetTelemetry, Navigate, NavigateGlobal, SetPosition, SetV
 from led_msgs.srv import SetLEDs
 from led_msgs.msg import LEDStateArray, LEDState
 from aruco_pose.msg import Marker, MarkerArray, Point2D
+from clover import long_callback
 
 import dynamic_reconfigure.client
 
@@ -31,9 +33,12 @@ 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
+import docopt
 
 print(cv2.getBuildInformation())
+
+if not os.environ.get('VM'):
+    import rpi_ws281x
+    import pigpio

builder/test/tests.sh

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

clover/CMakeLists.txt

@@ -53,7 +53,7 @@ find_package(OpenCV ${_opencv_version} REQUIRED
 ## Uncomment this if the package has a setup.py. This macro ensures
 ## modules and global scripts declared therein get installed
 ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
-# catkin_python_setup()
+catkin_python_setup()
 
 ################################################
 ## Declare ROS messages, services and actions ##

clover/examples/camera.py

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

clover/launch/aruco.launch

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

@@ -48,8 +48,6 @@
         <param name="disable_on_vpe" value="true"/>
     </node>
 
-    <node pkg="tf2_ros" type="static_transform_publisher" name="map_flipped_frame" args="0 0 0 3.1415926 3.1415926 0 map map_flipped"/>
-
     <!-- simplified offboard control -->
     <node name="simple_offboard" pkg="clover" type="simple_offboard" output="screen" clear_params="true">
         <param name="reference_frames/main_camera_optical" value="map"/>
@@ -87,8 +85,4 @@
         <param name="use_fake_gcs" value="false"/>
     </node>
 
-    <!-- Update static directory -->
-    <node pkg="roswww_static" name="roswww_static" type="main.py" clear_params="true">
-        <param name="default_package" value="clover"/>
-    </node>
 </launch>

clover/package.xml

@@ -43,8 +43,7 @@
   <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> -->
+  <test_depend>ros_pytest</test_depend>
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>

clover/requirements.txt

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

clover/setup.py

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

clover/src/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/selfcheck.py

@@ -9,7 +9,7 @@
 # The above copyright notice and this permission notice shall be included in all
 # copies or substantial portions of the Software.
 
-import os
+import os, sys
 import math
 import subprocess
 import re
@@ -28,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, '')
@@ -58,6 +50,16 @@ thread_local = threading.local()
 reports_lock = Lock()
 
 
+# formatting colors
+if sys.stdout.isatty():
+    GREY = '\033[90m'
+    GREEN = '\033[92m'
+    RED = '\033[31m'
+    END = '\033[0m'
+else:
+    GREY = GREEN = RED = END = ''
+
+
 def failure(text, *args):
     msg = text % args
     thread_local.reports += [{'failure': msg}]
@@ -78,25 +80,34 @@ def check(name):
             except Exception as e:
                 traceback.print_exc()
                 rospy.logerr('%s: exception occurred', name)
-                return
             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('%s: %s', name, report['info'])
+                        rospy.loginfo(GREY + name + END + ': ' + report['info'])
                 if not thread_local.reports:
-                    rospy.loginfo('%s: OK', name)
+                    rospy.loginfo(GREY + name + END + ': ' + GREEN + 'OK' + END)
                 if rospy.get_param('~time', False):
                     rospy.loginfo('%s: %.1f sec', name, rospy.get_time() - start)
         return wrapper
     return inner
 
 
+def ff(value, precision=2):
+    # safely format float or int
+    if value is None:
+        return RED + '???' + END
+    if isinstance(value, float):
+        return ('{:.' + str(precision + 1) + '}').format(value)
+    elif isinstance(value, int):
+        return str(value)
+
+
 param_get = rospy.ServiceProxy('mavros/param/get', ParamGet)
 
 
-def get_param(name):
+def get_param(name, default=None):
     try:
         res = param_get(param_id=name)
     except rospy.ServiceException as e:
@@ -105,12 +116,17 @@ def get_param(name):
 
     if not res.success:
         failure('unable to retrieve PX4 parameter %s', name)
+        return default
     else:
         if res.value.integer != 0:
             return res.value.integer
         return res.value.real
 
 
+def get_paramf(name, precision=2):
+    return ff(get_param(name), precision)
+
+
 recv_event = Event()
 link = mavutil.mavlink.MAVLink('', 255, 1)
 mavlink_pub = rospy.Publisher('mavlink/to', Mavlink, queue_size=1)
@@ -155,6 +171,24 @@ def mavlink_exec(cmd, timeout=3.0):
     return mavlink_recv
 
 
+def read_diagnostics(name, key):
+    e = Event()
+    def cb(msg):
+        for status in msg.status:
+            if status.name.lower() == name.lower():
+                for value in status.values:
+                    if value.key.lower() == key.lower():
+                        cb.value = value.value
+                        e.set()
+                        return
+
+    cb.value = None
+    sub = rospy.Subscriber('/diagnostics', DiagnosticArray, cb)
+    e.wait(1.0)  # wait to read all the diagnostics from nodes publishing them
+    sub.unregister()
+    return cb.value
+
+
 BOARD_ROTATIONS = {
     0: 'no rotation',
     1: 'yaw 45°',
@@ -200,6 +234,7 @@ def check_fcu():
         state = rospy.wait_for_message('mavros/state', State, timeout=3)
         if not state.connected:
             failure('no connection to the FCU (check wiring)')
+            info('fcu_url = %s', rospy.get_param('mavros/fcu_url', '?'))
             return
 
         if not is_process_running('px4', exact=True): # can't use px4 console in SITL
@@ -274,8 +309,15 @@ def check_fcu():
             except rospy.ROSException:
                 failure('no battery state')
 
+        # time sync check
+        try:
+            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)')
+        info('fcu_url = %s', rospy.get_param('mavros/fcu_url', '?'))
 
 
 def describe_direction(v):
@@ -356,7 +398,7 @@ def check_aruco():
 
     if is_process_running('aruco_detect', full=True):
         try:
-            info('aruco_detect/length = %g m', rospy.get_param('aruco_detect/length'))
+            info('aruco_detect/length = %g m', rospy.get_param('aruco_detect/length', '?'))
         except KeyError:
             failure('aruco_detect/length parameter is not set')
         known_tilt = rospy.get_param('aruco_detect/known_tilt', '')
@@ -420,7 +462,7 @@ def check_vpe():
         except rospy.ROSException:
             if not is_process_running('vpe_publisher', full=True):
                 info('no vision position estimate, vpe_publisher is not running')
-            elif rospy.get_param('aruco_map/known_tilt') == 'map_flipped':
+            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')
@@ -437,14 +479,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 = %.2f m, LPE_VIS_Z = %.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):
@@ -453,10 +495,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=%.2f, but it should be zero', delay)
-        info('EKF2_EVA_NOISE = %.3f, EKF2_EVP_NOISE = %.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
@@ -557,7 +599,7 @@ def check_global_position():
         rospy.wait_for_message('mavros/global_position/global', NavSatFix, timeout=0.8)
     except rospy.ROSException:
         info('no global position')
-        if get_param('SYS_MC_EST_GROUP') == 2 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')
 
 
@@ -578,29 +620,26 @@ 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 = %.2f, but it should be 1.0', scale)
 
-            info('LPE_FLW_QMIN = %s, LPE_FLW_R = %.4f, LPE_FLW_RR = %.4f, SENS_FLOW_MINHGT = %.3f, SENS_FLOW_MAXHGT = %.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 = %.2f, but it should be zero', delay)
-            info('EKF2_OF_QMIN = %s, EKF2_OF_N_MIN = %.4f, EKF2_OF_N_MAX = %.4f, SENS_FLOW_MINHGT = %.3f, SENS_FLOW_MAXHGT = %.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'))
+            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:
         if rospy.get_param('optical_flow/disable_on_vpe', False):
@@ -634,7 +673,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:
@@ -736,6 +775,9 @@ def check_clover_service():
 def check_image():
     try:
         info('version: %s', open('/etc/clover_version').read().strip())
+    except IOError:
+        try:
+            info('VM version: %s', open('/etc/clover_vm_version').read().strip())
         except IOError:
             info('no /etc/clover_version file, not the Clover image?')
 

clover/src/www

@@ -0,0 +1,4 @@
+#!/usr/bin/env bash
+
+export ROSWWW_DEFAULT=clover
+rosrun roswww_static update

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():
@@ -60,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_simulation/package.xml

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

clover_simulation/src/sim_leds.cpp

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

docs/en/simulation_native.md

@@ -147,6 +147,8 @@ sudo systemctl enable roscore
 sudo systemctl start roscore
 ```
 
+### Web tools setup
+
 Install any web server to serve Clover's web tools (`~/.ros/www` directory), e. g. Monkey:
 
 ```bash
@@ -158,3 +160,11 @@ sudo cp ~/catkin_ws/src/clover/builder/assets/monkey.service /etc/systemd/system
 sudo systemctl enable monkey
 sudo systemctl start monkey
 ```
+
+Create `~/.ros/www` using the following command:
+
+```bash
+rosrun clover www
+```
+
+If the set of packages containing a web part (through `www` directory) is changed, the above command also must be run.

docs/en/wall_aruco.md

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

docs/ru/simulation_native.md

@@ -147,6 +147,8 @@ sudo systemctl enable roscore
 sudo systemctl start roscore
 ```
 
+### Конфигурация веб-инструментов
+
 Установите любой веб-сервер, чтобы раздавать веб-инструменты Клевера (директория `~/.ros/www`), например, Monkey:
 
 ```bash
@@ -158,3 +160,11 @@ sudo cp ~/catkin_ws/src/clover/builder/assets/monkey.service /etc/systemd/system
 sudo systemctl enable monkey
 sudo systemctl start monkey
 ```
+
+Создайте директорию `~/.ros/www` следующей командой:
+
+```bash
+rosrun clover www
+```
+
+При обновлении набора пакетов, содержащих веб-часть (через каталог `www`), также необходимо выполнение данной команды.

docs/ru/wall_aruco.md

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

redirects.json

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

roswww_static/CMakeLists.txt

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

roswww_static/README.md

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

roswww_static/launch/example.launch

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

roswww_static/src/update

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