From 89bfc150f3d0be8cfc47b935ccf34ed60a68c6ad Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Mon, 31 Oct 2022 02:41:21 +0600 Subject: [PATCH 01/40] selfcheck.py: split up estimator's and flow sensor's parameters reports --- clover/src/selfcheck.py | 13 +++++-------- 1 file changed, 5 insertions(+), 8 deletions(-) diff --git a/clover/src/selfcheck.py b/clover/src/selfcheck.py index 64100ea9..03cd8663 100755 --- a/clover/src/selfcheck.py +++ b/clover/src/selfcheck.py @@ -582,12 +582,10 @@ def check_optical_flow(): 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', + info('LPE_FLW_QMIN = %s, LPE_FLW_R = %.4f, LPE_FLW_RR = %.4f', 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')) + get_param('LPE_FLW_RR')) elif est == 2: fuse = get_param('EKF2_AID_MASK') if not fuse & (1 << 1): @@ -595,12 +593,11 @@ def check_optical_flow(): delay = get_param('EKF2_OF_DELAY') 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', + info('EKF2_OF_QMIN = %s, EKF2_OF_N_MIN = %.4f, EKF2_OF_N_MAX = %.4f', 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')) + get_param('EKF2_OF_N_MAX')) + info('SENS_FLOW_MINHGT = %.3f, SENS_FLOW_MAXHGT = %.3f', get_param('SENS_FLOW_MINHGT'), get_param('SENS_FLOW_MAXHGT')) except rospy.ROSException: if rospy.get_param('optical_flow/disable_on_vpe', False): From 4b397670a18b36083597315a8876113dcc991e81 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Mon, 31 Oct 2022 03:08:30 +0600 Subject: [PATCH 02/40] selfcheck.py: make report more compact by removing severity levels The severity level is visible from the color --- clover/src/selfcheck.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/clover/src/selfcheck.py b/clover/src/selfcheck.py index 03cd8663..7bf274ac 100755 --- a/clover/src/selfcheck.py +++ b/clover/src/selfcheck.py @@ -45,7 +45,7 @@ import locale 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, '') From ccbd1cbad9a057a1c492680020b38246e4ce3bab Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Mon, 31 Oct 2022 04:24:13 +0600 Subject: [PATCH 03/40] selfcheck.py: make output colored --- clover/src/selfcheck.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/clover/src/selfcheck.py b/clover/src/selfcheck.py index 7bf274ac..d02543ca 100755 --- a/clover/src/selfcheck.py +++ b/clover/src/selfcheck.py @@ -84,9 +84,9 @@ def check(name): if 'failure' in report: rospy.logerr('%s: %s', name, report['failure']) elif 'info' in report: - rospy.loginfo('%s: %s', name, report['info']) + rospy.loginfo('\033[90m%s\033[0m: %s', name, report['info']) if not thread_local.reports: - rospy.loginfo('%s: OK', name) + rospy.loginfo('\033[90m%s\033[0m: \033[92mOK\033[0m', name) if rospy.get_param('~time', False): rospy.loginfo('%s: %.1f sec', name, rospy.get_time() - start) return wrapper From ce0b4eb428a59b033d9e5e738d16b75c0a3e0751 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Wed, 2 Nov 2022 06:38:36 +0600 Subject: [PATCH 04/40] Implement long_callback Python decorator addressing #218 Clover package Python library added. --- builder/test/tests.py | 1 + clover/CMakeLists.txt | 2 +- clover/setup.py | 11 +++++++++++ clover/src/clover/__init__.py | 35 +++++++++++++++++++++++++++++++++++ clover/test/basic.py | 16 ++++++++++++++++ 5 files changed, 64 insertions(+), 1 deletion(-) create mode 100644 clover/setup.py create mode 100644 clover/src/clover/__init__.py diff --git a/builder/test/tests.py b/builder/test/tests.py index 2af19852..d453d505 100755 --- a/builder/test/tests.py +++ b/builder/test/tests.py @@ -22,6 +22,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 diff --git a/clover/CMakeLists.txt b/clover/CMakeLists.txt index abfaa9c3..a2bfb3dc 100644 --- a/clover/CMakeLists.txt +++ b/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 ## diff --git a/clover/setup.py b/clover/setup.py new file mode 100644 index 00000000..afdfcd70 --- /dev/null +++ b/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) diff --git a/clover/src/clover/__init__.py b/clover/src/clover/__init__.py new file mode 100644 index 00000000..f61781be --- /dev/null +++ b/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) + """ + 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 diff --git a/clover/test/basic.py b/clover/test/basic.py index 21440bd2..088d6cfe 100755 --- a/clover/test/basic.py +++ b/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 From 4f00960db3d8448dcf2013a0959ed70be4c19493 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Wed, 2 Nov 2022 16:09:56 +0600 Subject: [PATCH 05/40] Minor fix to long_callback decorator example --- clover/src/clover/__init__.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/clover/src/clover/__init__.py b/clover/src/clover/__init__.py index f61781be..6d398c42 100644 --- a/clover/src/clover/__init__.py +++ b/clover/src/clover/__init__.py @@ -15,7 +15,7 @@ def long_callback(fn): # perform image processing # ... - rospy.Subscriber('main_camera/image_raw', Image, image_callback) + rospy.Subscriber('main_camera/image_raw', Image, image_callback, queue_size=1) """ e = Event() From dc5da00abdcbeea10afca47c68396e42e4292c52 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Thu, 3 Nov 2022 06:56:07 +0600 Subject: [PATCH 06/40] selfcheck.py: print reports when there was exception in check --- clover/src/selfcheck.py | 1 - 1 file changed, 1 deletion(-) diff --git a/clover/src/selfcheck.py b/clover/src/selfcheck.py index d02543ca..d848a766 100755 --- a/clover/src/selfcheck.py +++ b/clover/src/selfcheck.py @@ -78,7 +78,6 @@ 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: From 0d44ff399387c22a7ce99015389e8e30752c4331 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Thu, 3 Nov 2022 17:47:34 +0600 Subject: [PATCH 07/40] selfcheck.py: handle nicely when a PX4 parameter cannot be retrieved --- clover/src/selfcheck.py | 59 ++++++++++++++++++++++++++--------------- 1 file changed, 37 insertions(+), 22 deletions(-) diff --git a/clover/src/selfcheck.py b/clover/src/selfcheck.py index d848a766..aba795be 100755 --- a/clover/src/selfcheck.py +++ b/clover/src/selfcheck.py @@ -92,10 +92,20 @@ def check(name): return inner +def ff(value, precision=2): + # safely format float or int + if value is None: + return '\033[31m???\033[0m' + if isinstance(value, float): + return ('{:.' + str(precision + 1) + '}').format(value) + elif isinstance(value, int): + return str(value) + + param_get = rospy.ServiceProxy('mavros/param/get', ParamGet) -def get_param(name): +def get_param(name, default=None): try: res = param_get(param_id=name) except rospy.ServiceException as e: @@ -104,12 +114,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) @@ -436,14 +451,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): @@ -452,10 +467,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 @@ -556,7 +571,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') @@ -577,26 +592,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', - get_param('LPE_FLW_QMIN'), - get_param('LPE_FLW_R'), - get_param('LPE_FLW_RR')) + 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', - get_param('EKF2_OF_QMIN'), - get_param('EKF2_OF_N_MIN'), - get_param('EKF2_OF_N_MAX')) - info('SENS_FLOW_MINHGT = %.3f, SENS_FLOW_MAXHGT = %.3f', 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): @@ -630,7 +645,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: From 0fb101cc5935c8eedc0d88a59917d2b9d33fb695 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Thu, 3 Nov 2022 17:47:53 +0600 Subject: [PATCH 08/40] selfcheck.py: add time sync offset report --- clover/src/selfcheck.py | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/clover/src/selfcheck.py b/clover/src/selfcheck.py index aba795be..169a4cc0 100755 --- a/clover/src/selfcheck.py +++ b/clover/src/selfcheck.py @@ -28,6 +28,7 @@ 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 @@ -169,6 +170,15 @@ def mavlink_exec(cmd, timeout=3.0): return mavlink_recv +def read_diagnostics(name, key): + diagnostics = rospy.wait_for_message('/diagnostics', DiagnosticArray, timeout=3.0) + for status in diagnostics.status: + if status.name.lower() == name.lower(): + for value in status.values: + if value.key.lower() == key.lower(): + return value.value + + BOARD_ROTATIONS = { 0: 'no rotation', 1: 'yaw 45°', @@ -288,6 +298,12 @@ 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 rospy.ROSException: + failure('cannot read time sync offset') + except rospy.ROSException: failure('no MAVROS state (check wiring)') From d03cfe00cafddab31bc0d64793c86b74e7658cea Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Thu, 3 Nov 2022 17:53:59 +0600 Subject: [PATCH 09/40] selfcheck.py: handle inability to read time sync offset --- clover/src/selfcheck.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/clover/src/selfcheck.py b/clover/src/selfcheck.py index 169a4cc0..8368afb3 100755 --- a/clover/src/selfcheck.py +++ b/clover/src/selfcheck.py @@ -301,7 +301,7 @@ def check_fcu(): # time sync check try: info('time sync offset: %.2f s', float(read_diagnostics('mavros: Time Sync', 'Estimated time offset (s)'))) - except rospy.ROSException: + except: failure('cannot read time sync offset') except rospy.ROSException: From a6cee773abb69cb6282759f4dbd96b094aa190f6 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Thu, 3 Nov 2022 18:23:54 +0600 Subject: [PATCH 10/40] selfcheck.py: fix reading diagnostics considering there might be multiple nodes publishing them --- clover/src/selfcheck.py | 21 +++++++++++++++------ 1 file changed, 15 insertions(+), 6 deletions(-) diff --git a/clover/src/selfcheck.py b/clover/src/selfcheck.py index 8368afb3..7c802c37 100755 --- a/clover/src/selfcheck.py +++ b/clover/src/selfcheck.py @@ -171,12 +171,21 @@ def mavlink_exec(cmd, timeout=3.0): def read_diagnostics(name, key): - diagnostics = rospy.wait_for_message('/diagnostics', DiagnosticArray, timeout=3.0) - for status in diagnostics.status: - if status.name.lower() == name.lower(): - for value in status.values: - if value.key.lower() == key.lower(): - return value.value + 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 = { From c5399868cb4ff03eb7cc747ea9b814d6f949bbfe Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Thu, 3 Nov 2022 18:28:22 +0600 Subject: [PATCH 11/40] selfcheck.py: remove obsolete todos --- clover/src/selfcheck.py | 9 --------- 1 file changed, 9 deletions(-) diff --git a/clover/src/selfcheck.py b/clover/src/selfcheck.py index 7c802c37..c58b24ec 100755 --- a/clover/src/selfcheck.py +++ b/clover/src/selfcheck.py @@ -35,15 +35,6 @@ 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']='${message}' From ec9ddf5fd2867cda075c25cdf00c4a579e5a7558 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Thu, 3 Nov 2022 18:36:18 +0600 Subject: [PATCH 12/40] selfcheck.py: read VM image version from /etc/clover_vm_version --- clover/src/selfcheck.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/clover/src/selfcheck.py b/clover/src/selfcheck.py index c58b24ec..9cd58b85 100755 --- a/clover/src/selfcheck.py +++ b/clover/src/selfcheck.py @@ -764,7 +764,10 @@ def check_image(): try: info('version: %s', open('/etc/clover_version').read().strip()) except IOError: - info('no /etc/clover_version file, not the Clover image?') + try: + info('VM version: %s', open('/etc/clover_vm_version').read().strip()) + except IOError: + info('no /etc/clover_version file, not the Clover image?') @check('Preflight status') From 2b13aa02eb87dbb0db04c62c40c12331dd0341a4 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Fri, 4 Nov 2022 03:10:56 +0600 Subject: [PATCH 13/40] docs: make /camera redirect to English version of the article --- redirects.json | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/redirects.json b/redirects.json index 07d1d70c..d6a5ac81 100644 --- a/redirects.json +++ b/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" }, From c5d01c678abf95fa1a0b630057bc00b3f0b72b33 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Sat, 5 Nov 2022 17:26:19 +0600 Subject: [PATCH 14/40] image and vm image: validate `python` is python2 (for now) --- builder/test/tests.sh | 3 +++ 1 file changed, 3 insertions(+) diff --git a/builder/test/tests.sh b/builder/test/tests.sh index 8eaf9dec..eb2f0c04 100755 --- a/builder/test/tests.sh +++ b/builder/test/tests.sh @@ -12,6 +12,9 @@ python3 --version ipython --version ipython3 --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 From 8f09df6b340809bb9e504d3395df69b0cf0e91db Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Sat, 5 Nov 2022 17:45:10 +0600 Subject: [PATCH 15/40] Optimize shell tests for vm image --- builder/test/tests.sh | 28 +++++++++++++++++++--------- 1 file changed, 19 insertions(+), 9 deletions(-) diff --git a/builder/test/tests.sh b/builder/test/tests.sh index eb2f0c04..f5bfae39 100755 --- a/builder/test/tests.sh +++ b/builder/test/tests.sh @@ -6,15 +6,10 @@ set -ex # validate required software is installed -python --version python2 --version python3 --version -ipython --version ipython3 --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 @@ -28,17 +23,26 @@ 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" ]] + + pigpiod -v + i2cdetect -V +fi + # ros stuff roscore -h @@ -65,5 +69,11 @@ rosversion image_view [[ $(rosversion cv_camera) == "0.5.1" ]] # patched version with init fix [[ $(rosversion ws281x) == "0.0.13" ]] +if [ $VM ]; then + H="/home/clover" +else + H="/home/pi" +fi + # validate examples are present -[[ $(ls /home/pi/examples/*) ]] +[[ $(ls $H/examples/*) ]] From 6fafaf31840baa70ab40c9b91d4fd57b69f648ee Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Sat, 5 Nov 2022 20:48:38 +0600 Subject: [PATCH 16/40] Fix Python tests for VM --- builder/test/tests.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/builder/test/tests.py b/builder/test/tests.py index d453d505..6bce4823 100755 --- a/builder/test/tests.py +++ b/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 @@ -32,9 +33,11 @@ import tf2_geometry_msgs import VL53L1X import pymavlink from pymavlink import mavutil -import rpi_ws281x -import pigpio # from espeak import espeak from pyzbar import pyzbar print(cv2.getBuildInformation()) + +if not os.environ.get('VM'): + import rpi_ws281x + import pigpio From 19e0d725b0eb9266eea5656da54fb6b1c8bebea4 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Sat, 5 Nov 2022 22:47:39 +0600 Subject: [PATCH 17/40] image: test ptvsd on the RPi image only --- builder/test/tests.sh | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/builder/test/tests.sh b/builder/test/tests.sh index f5bfae39..0cb1ae8e 100755 --- a/builder/test/tests.sh +++ b/builder/test/tests.sh @@ -10,10 +10,6 @@ python2 --version python3 --version ipython3 --version -# ptvsd does not have a stand-alone binary -python -m ptvsd --version -python3 -m ptvsd --version - node -v npm -v @@ -39,6 +35,10 @@ if [ -z $VM ]; then # `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 fi From 9c34d7722c37730ad558bdbeaadc0324a6b2276a Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Sun, 6 Nov 2022 00:25:23 +0600 Subject: [PATCH 18/40] =?UTF-8?q?image:=20don=E2=80=99t=20test=20butterfly?= =?UTF-8?q?=20on=20vm=20image?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- builder/test/tests.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/builder/test/tests.sh b/builder/test/tests.sh index 0cb1ae8e..48274a82 100755 --- a/builder/test/tests.sh +++ b/builder/test/tests.sh @@ -22,7 +22,6 @@ pip --version pip3 --version tcpdump --version monkey --version -butterfly -h # espeak --version mjpg_streamer --version systemctl --version @@ -41,6 +40,7 @@ if [ -z $VM ]; then pigpiod -v i2cdetect -V + butterfly -h fi # ros stuff From 3f3d1cd2203855d2bb198d0d4ce244e880b3951f Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Sun, 6 Nov 2022 01:43:03 +0600 Subject: [PATCH 19/40] =?UTF-8?q?image:=20don=E2=80=99t=20test=20mjpg=5Fst?= =?UTF-8?q?reamer=20on=20vm=20image?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- builder/test/tests.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/builder/test/tests.sh b/builder/test/tests.sh index 48274a82..5e2e34cc 100755 --- a/builder/test/tests.sh +++ b/builder/test/tests.sh @@ -23,7 +23,6 @@ pip3 --version tcpdump --version monkey --version # espeak --version -mjpg_streamer --version systemctl --version if [ -z $VM ]; then @@ -41,6 +40,7 @@ if [ -z $VM ]; then pigpiod -v i2cdetect -V butterfly -h + mjpg_streamer --version fi # ros stuff From 9a8aa00bc715c5f1f3af2fe5f913daec3e6745e2 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Sun, 6 Nov 2022 02:02:43 +0600 Subject: [PATCH 20/40] Run map_flipped frame only when markers placement is ceiling --- clover/launch/aruco.launch | 4 ++++ clover/launch/clover.launch | 2 -- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/clover/launch/aruco.launch b/clover/launch/aruco.launch index 53766c05..4fbb5e44 100644 --- a/clover/launch/aruco.launch +++ b/clover/launch/aruco.launch @@ -53,4 +53,8 @@ + + + diff --git a/clover/launch/clover.launch b/clover/launch/clover.launch index 8c77a0f4..98954f7c 100644 --- a/clover/launch/clover.launch +++ b/clover/launch/clover.launch @@ -48,8 +48,6 @@ - - From 64f939d7ed93628d54689260e9c7e7295747a70a Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Sun, 6 Nov 2022 04:15:32 +0600 Subject: [PATCH 21/40] image: updates to tests for VM image --- builder/test/tests.sh | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/builder/test/tests.sh b/builder/test/tests.sh index 5e2e34cc..d5db2b9a 100755 --- a/builder/test/tests.sh +++ b/builder/test/tests.sh @@ -48,7 +48,6 @@ fi roscore -h rosversion clover rosversion aruco_pose -rosversion vl53l1x rosversion mavros rosversion mavros_extras rosversion ws281x @@ -65,15 +64,14 @@ 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 [ $VM ]; then - H="/home/clover" -else - H="/home/pi" +if [ -z $VM ]; then + rosversion vl53l1x + [[ $(rosversion cv_camera) == "0.5.1" ]] # patched version with init fix fi +[ $VM ] && H="/home/clover" || H="/home/pi" + # validate examples are present [[ $(ls $H/examples/*) ]] From 2cd77662df589b5de8ad60dafda0e1e782bba925 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Mon, 7 Nov 2022 18:21:32 +0600 Subject: [PATCH 22/40] image: check version of rosbridge_server which clover package depends on instead of rosbridge_suite --- builder/test/tests.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/builder/test/tests.sh b/builder/test/tests.sh index d5db2b9a..f370d8a2 100755 --- a/builder/test/tests.sh +++ b/builder/test/tests.sh @@ -55,7 +55,7 @@ rosversion led_msgs rosversion dynamic_reconfigure rosversion tf2_web_republisher rosversion compressed_image_transport -rosversion rosbridge_suite +rosversion rosbridge_server rosversion rosserial rosversion usb_cam rosversion cv_camera From 8b1b365e676f6161307644d2d3a388cd024c6b2e Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Mon, 7 Nov 2022 18:21:55 +0600 Subject: [PATCH 23/40] image: check compressed_image_transport on rpi image only --- builder/test/tests.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/builder/test/tests.sh b/builder/test/tests.sh index f370d8a2..0b9cb793 100755 --- a/builder/test/tests.sh +++ b/builder/test/tests.sh @@ -54,7 +54,6 @@ rosversion ws281x rosversion led_msgs rosversion dynamic_reconfigure rosversion tf2_web_republisher -rosversion compressed_image_transport rosversion rosbridge_server rosversion rosserial rosversion usb_cam @@ -67,6 +66,7 @@ rosversion image_view [[ $(rosversion ws281x) == "0.0.13" ]] if [ -z $VM ]; then + rosversion compressed_image_transport rosversion vl53l1x [[ $(rosversion cv_camera) == "0.5.1" ]] # patched version with init fix fi From 8512e8a045f5052c365e5145d3eda72fda19936e Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Mon, 7 Nov 2022 18:30:54 +0600 Subject: [PATCH 24/40] image: check rosshow version only on rpi image --- builder/test/tests.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/builder/test/tests.sh b/builder/test/tests.sh index 0b9cb793..941793ba 100755 --- a/builder/test/tests.sh +++ b/builder/test/tests.sh @@ -59,7 +59,6 @@ rosversion rosserial rosversion usb_cam rosversion cv_camera rosversion web_video_server -rosversion rosshow rosversion nodelet rosversion image_view @@ -67,6 +66,7 @@ rosversion image_view if [ -z $VM ]; then rosversion compressed_image_transport + rosversion rosshow rosversion vl53l1x [[ $(rosversion cv_camera) == "0.5.1" ]] # patched version with init fix fi From 92748a760be5cb86d39745cad6ae30c9b9d355e2 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Mon, 7 Nov 2022 19:00:02 +0600 Subject: [PATCH 25/40] simulation: remove redundant warning on creating a new LedController --- clover_simulation/src/sim_leds.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/clover_simulation/src/sim_leds.cpp b/clover_simulation/src/sim_leds.cpp index fb188af7..f281d99a 100644 --- a/clover_simulation/src/sim_leds.cpp +++ b/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 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]; } From 5e3b07ff5ee9ba65a0b52dd580d5601a16566f9a Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Mon, 7 Nov 2022 20:09:15 +0600 Subject: [PATCH 26/40] Add a basic example script on working with the camera --- clover/examples/camera.py | 64 +++++++++++++++++++++++++++++++++++++++ 1 file changed, 64 insertions(+) create mode 100644 clover/examples/camera.py diff --git a/clover/examples/camera.py b/clover/examples/camera.py new file mode 100644 index 00000000..67d805ee --- /dev/null +++ b/clover/examples/camera.py @@ -0,0 +1,64 @@ +# Information: https://clover.coex.tech/camera + +# Example on basic working with the camera and image processing: + +# - cuts out a central square from the camera image; +# - publishes this cropped image to the topic `/cv/center`; +# - computes the average color of it; +# - prints its name to the console. + +import rospy +import cv2 +from sensor_msgs.msg import Image +from cv_bridge import CvBridge +from clover import long_callback + +rospy.init_node('cv') +bridge = CvBridge() + +printed_color = None +center_pub = rospy.Publisher('~center', Image, queue_size=1) + +def get_color_name(h): + if h < 15: return 'red' + if h < 30: return 'orange' + elif h < 60: return 'yellow' + elif h < 90: return 'green' + elif h < 120: return 'cyan' + elif h < 150: return 'blue' + elif h < 170: return 'magenta' + else: return 'red' + + +@long_callback +def image_callback(msg): + img = bridge.imgmsg_to_cv2(msg, 'bgr8') + + # convert to HSV to work with color hue + img_hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV) + + # cut out a central square + w = img.shape[1] + h = img.shape[0] + r = 20 + center = img_hsv[h // 2 - r:h // 2 + r, w // 2 - r:w // 2 + r] + + # compute and print the average hue + mean_hue = center[:, :, 0].mean() + color = get_color_name(mean_hue) + global printed_color + if color != printed_color: + print(color) + printed_color = color + + # publish the cropped image + center = cv2.cvtColor(center, cv2.COLOR_HSV2BGR) + center_pub.publish(bridge.cv2_to_imgmsg(center, 'bgr8')) + +# process every frame: +image_sub = rospy.Subscriber('main_camera/image_raw', Image, image_callback, queue_size=1) + +# process 5 frames per second: +# image_sub = rospy.Subscriber('main_camera/image_raw_throttled', Image, image_callback, queue_size=1) + +rospy.spin() From 96ea78f141348df852ae5868b3728e7e1d4384b8 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Mon, 7 Nov 2022 20:47:18 +0600 Subject: [PATCH 27/40] image: check rosserial version only on rpi image --- builder/test/tests.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/builder/test/tests.sh b/builder/test/tests.sh index 941793ba..ae45873d 100755 --- a/builder/test/tests.sh +++ b/builder/test/tests.sh @@ -55,7 +55,6 @@ rosversion led_msgs rosversion dynamic_reconfigure rosversion tf2_web_republisher rosversion rosbridge_server -rosversion rosserial rosversion usb_cam rosversion cv_camera rosversion web_video_server @@ -68,6 +67,7 @@ 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 From 640ec1ee1aba18c57f3914278b9f557c3bea4084 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Tue, 8 Nov 2022 03:37:12 +0600 Subject: [PATCH 28/40] Add clover package dependency on pytest --- clover/package.xml | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/clover/package.xml b/clover/package.xml index 1c49e2b1..7ee4c683 100644 --- a/clover/package.xml +++ b/clover/package.xml @@ -43,8 +43,7 @@ python3-lxml dynamic_reconfigure python-pymavlink - - + ros_pytest From d24b6617a4bde26d9d9ce4df4e3e8f637c974f1c Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Tue, 8 Nov 2022 06:41:21 +0600 Subject: [PATCH 29/40] selfcheck.py: don't fall when aruco_map/known_tilt is not set --- clover/src/selfcheck.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/clover/src/selfcheck.py b/clover/src/selfcheck.py index 9cd58b85..3875cc2f 100755 --- a/clover/src/selfcheck.py +++ b/clover/src/selfcheck.py @@ -450,7 +450,7 @@ def check_vpe(): except rospy.ROSException: if not is_process_running('vpe_publisher', full=True): info('no vision position estimate, vpe_publisher is not running') - elif rospy.get_param('aruco_map/known_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') From 2cda68ae4a2eac2a35851d52e4d89e73ee99a3c8 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Tue, 8 Nov 2022 06:46:29 +0600 Subject: [PATCH 30/40] selfcheck.py: don't fall if aruco_detect/length is not set --- clover/src/selfcheck.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/clover/src/selfcheck.py b/clover/src/selfcheck.py index 3875cc2f..1810b683 100755 --- a/clover/src/selfcheck.py +++ b/clover/src/selfcheck.py @@ -386,7 +386,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', '') From 54ab5ab4b5fc9a90caab1276b783a8f7599d9872 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Tue, 8 Nov 2022 06:47:01 +0600 Subject: [PATCH 31/40] selfcheck.py: make output colored only in a tty --- clover/src/selfcheck.py | 18 ++++++++++++++---- 1 file changed, 14 insertions(+), 4 deletions(-) diff --git a/clover/src/selfcheck.py b/clover/src/selfcheck.py index 1810b683..4ac6fffb 100755 --- a/clover/src/selfcheck.py +++ b/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 @@ -50,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}] @@ -75,9 +85,9 @@ def check(name): if 'failure' in report: rospy.logerr('%s: %s', name, report['failure']) elif 'info' in report: - rospy.loginfo('\033[90m%s\033[0m: %s', name, report['info']) + rospy.loginfo(GREY + name + END + ': ' + report['info']) if not thread_local.reports: - rospy.loginfo('\033[90m%s\033[0m: \033[92mOK\033[0m', 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 @@ -87,7 +97,7 @@ def check(name): def ff(value, precision=2): # safely format float or int if value is None: - return '\033[31m???\033[0m' + return RED + '???' + END if isinstance(value, float): return ('{:.' + str(precision + 1) + '}').format(value) elif isinstance(value, int): From 8fe34e90e699e1b7561b8c1e3c522ece0c8c78bd Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Tue, 8 Nov 2022 16:07:36 +0600 Subject: [PATCH 32/40] Depend on docopt in package.xml instead of requirements.txt --- aruco_pose/package.xml | 4 +++- builder/test/tests.py | 1 + clover/requirements.txt | 1 - clover_simulation/package.xml | 4 +++- 4 files changed, 7 insertions(+), 3 deletions(-) diff --git a/aruco_pose/package.xml b/aruco_pose/package.xml index 03977753..a29c85b2 100644 --- a/aruco_pose/package.xml +++ b/aruco_pose/package.xml @@ -1,5 +1,5 @@ - + aruco_pose 0.23.0 Positioning with ArUco markers @@ -28,6 +28,8 @@ sensor_msgs rostest dynamic_reconfigure + python-docopt + python3-docopt image_publisher ros_pytest diff --git a/builder/test/tests.py b/builder/test/tests.py index 6bce4823..e550cf52 100755 --- a/builder/test/tests.py +++ b/builder/test/tests.py @@ -35,6 +35,7 @@ import pymavlink from pymavlink import mavutil # from espeak import espeak from pyzbar import pyzbar +import docopt print(cv2.getBuildInformation()) diff --git a/clover/requirements.txt b/clover/requirements.txt index 81d0fda6..64b5f719 100644 --- a/clover/requirements.txt +++ b/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 diff --git a/clover_simulation/package.xml b/clover_simulation/package.xml index a7d27462..7ee6983f 100644 --- a/clover_simulation/package.xml +++ b/clover_simulation/package.xml @@ -1,4 +1,4 @@ - + clover_simulation 0.23.0 The clover_simulation package provides worlds and launch files for Gazebo. @@ -22,6 +22,8 @@ gazebo_ros gazebo_plugins rospy + python-docopt + python3-docopt From e65d380b4bd6ba2b1e05fca677f39a5c4c005e30 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Tue, 8 Nov 2022 22:43:08 +0600 Subject: [PATCH 33/40] Minor camera example fix --- clover/examples/camera.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/clover/examples/camera.py b/clover/examples/camera.py index 67d805ee..9f69d6de 100644 --- a/clover/examples/camera.py +++ b/clover/examples/camera.py @@ -21,7 +21,7 @@ center_pub = rospy.Publisher('~center', Image, queue_size=1) def get_color_name(h): if h < 15: return 'red' - if h < 30: return 'orange' + elif h < 30: return 'orange' elif h < 60: return 'yellow' elif h < 90: return 'green' elif h < 120: return 'cyan' From e719b0f1e2c80d25f188f893e7743b6969110bed Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Wed, 9 Nov 2022 23:46:05 +0600 Subject: [PATCH 34/40] selfcheck.py: print fcu_url value if no connection to fcu --- clover/src/selfcheck.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/clover/src/selfcheck.py b/clover/src/selfcheck.py index 4ac6fffb..b7b3eb85 100755 --- a/clover/src/selfcheck.py +++ b/clover/src/selfcheck.py @@ -234,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 @@ -316,6 +317,7 @@ def check_fcu(): except rospy.ROSException: failure('no MAVROS state (check wiring)') + info('fcu_url = %s', rospy.get_param('mavros/fcu_url', '?')) def describe_direction(v): From 277eb7297f8e991025a1e674b27d2758e75e2b9d Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Thu, 10 Nov 2022 04:37:30 +0600 Subject: [PATCH 35/40] image: test basic ros tools work --- builder/test/tests.sh | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/builder/test/tests.sh b/builder/test/tests.sh index ae45873d..6445eed8 100755 --- a/builder/test/tests.sh +++ b/builder/test/tests.sh @@ -71,6 +71,18 @@ if [ -z $VM ]; then [[ $(rosversion cv_camera) == "0.5.1" ]] # patched version with init fix fi +# test basic ros tool work +roscd +rosrun +rosmsg +rossrv +rosnode +rostopic +rosservice +rosparam +roslaunch + +# determine user home directory [ $VM ] && H="/home/clover" || H="/home/pi" # validate examples are present From 3662f512a7ed9c7b73d21ad4ffd480541160995f Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Thu, 10 Nov 2022 05:06:46 +0600 Subject: [PATCH 36/40] docs: update in wall aruco article --- docs/en/wall_aruco.md | 6 +++--- docs/ru/wall_aruco.md | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/docs/en/wall_aruco.md b/docs/en/wall_aruco.md index 0fe2bc73..130e005a 100644 --- a/docs/en/wall_aruco.md +++ b/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 ``, 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 + ``` After all the settings, call `sudo systemctl restart clover` to restart the `clover` service. diff --git a/docs/ru/wall_aruco.md b/docs/ru/wall_aruco.md index c2621394..ca3bd97a 100644 --- a/docs/ru/wall_aruco.md +++ b/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/`. Измените в нем строку ``, где `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 + ``` После всех настроек вызовите `sudo systemctl restart clover` для перезагрузки сервиса `clover`. From a0322c55f23a7186442073d6988dc401aa701c28 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Thu, 10 Nov 2022 17:56:23 +0600 Subject: [PATCH 37/40] Fix ROS tools tests --- builder/test/tests.sh | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/builder/test/tests.sh b/builder/test/tests.sh index 6445eed8..d27c9597 100755 --- a/builder/test/tests.sh +++ b/builder/test/tests.sh @@ -71,7 +71,11 @@ if [ -z $VM ]; then [[ $(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 @@ -82,8 +86,5 @@ rosservice rosparam roslaunch -# determine user home directory -[ $VM ] && H="/home/clover" || H="/home/pi" - # validate examples are present [[ $(ls $H/examples/*) ]] From 393801b0234915c15c331d5e39767c776ccf6815 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Thu, 10 Nov 2022 19:57:02 +0600 Subject: [PATCH 38/40] Fix ROS tools tests considering some of them exit with 64 on usage --- builder/test/tests.sh | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/builder/test/tests.sh b/builder/test/tests.sh index d27c9597..669f4d1c 100755 --- a/builder/test/tests.sh +++ b/builder/test/tests.sh @@ -80,11 +80,11 @@ roscd rosrun rosmsg rossrv -rosnode -rostopic -rosservice +rosnode || [ $? -eq 64 ] # usage output code is 64 +rostopic || [ $? -eq 64 ] +rosservice || [ $? -eq 64 ] rosparam -roslaunch +roslaunch -h # validate examples are present [[ $(ls $H/examples/*) ]] From 72f8d901d59a7af1edb3573261bd91981babaa4d Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Thu, 10 Nov 2022 20:04:05 +0600 Subject: [PATCH 39/40] ci: set cancel-in-progress only for deploy job not the whole docs wf --- .github/workflows/docs.yml | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/.github/workflows/docs.yml b/.github/workflows/docs.yml index a9dc26a8..e819c4e3 100644 --- a/.github/workflows/docs.yml +++ b/.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 }} From f719406c8bad88c7ba466e1d436bf48a0c375e0e Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Thu, 10 Nov 2022 22:25:44 +0600 Subject: [PATCH 40/40] Remove www directory update from clover.launch and update it on demand only (#475) --- builder/image-ros.sh | 3 +++ builder/test/tests.sh | 5 +++++ clover/launch/clover.launch | 4 ---- clover/src/www | 4 ++++ docs/en/simulation_native.md | 10 ++++++++++ docs/ru/simulation_native.md | 10 ++++++++++ roswww_static/CMakeLists.txt | 4 +--- roswww_static/README.md | 10 ++++++---- roswww_static/launch/example.launch | 6 ------ roswww_static/{main.py => src/update} | 12 +++++------- 10 files changed, 44 insertions(+), 24 deletions(-) create mode 100755 clover/src/www delete mode 100644 roswww_static/launch/example.launch rename roswww_static/{main.py => src/update} (82%) diff --git a/builder/image-ros.sh b/builder/image-ros.sh index afa369b5..2dd192b6 100755 --- a/builder/image-ros.sh +++ b/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 diff --git a/builder/test/tests.sh b/builder/test/tests.sh index 669f4d1c..1d0ccc18 100755 --- a/builder/test/tests.sh +++ b/builder/test/tests.sh @@ -88,3 +88,8 @@ roslaunch -h # validate examples are present [[ $(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" ] diff --git a/clover/launch/clover.launch b/clover/launch/clover.launch index 98954f7c..68150d49 100644 --- a/clover/launch/clover.launch +++ b/clover/launch/clover.launch @@ -85,8 +85,4 @@ - - - - diff --git a/clover/src/www b/clover/src/www new file mode 100755 index 00000000..7a66c28e --- /dev/null +++ b/clover/src/www @@ -0,0 +1,4 @@ +#!/usr/bin/env bash + +export ROSWWW_DEFAULT=clover +rosrun roswww_static update diff --git a/docs/en/simulation_native.md b/docs/en/simulation_native.md index 35df908f..d28bc4b3 100644 --- a/docs/en/simulation_native.md +++ b/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. diff --git a/docs/ru/simulation_native.md b/docs/ru/simulation_native.md index a3f738ae..10dce379 100644 --- a/docs/ru/simulation_native.md +++ b/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`), также необходимо выполнение данной команды. diff --git a/roswww_static/CMakeLists.txt b/roswww_static/CMakeLists.txt index bb9e1fc7..f44f98ea 100644 --- a/roswww_static/CMakeLists.txt +++ b/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} ) diff --git a/roswww_static/README.md b/roswww_static/README.md index c2eea67f..92eb35f4 100644 --- a/roswww_static/README.md +++ b/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. diff --git a/roswww_static/launch/example.launch b/roswww_static/launch/example.launch deleted file mode 100644 index 3732081f..00000000 --- a/roswww_static/launch/example.launch +++ /dev/null @@ -1,6 +0,0 @@ - - - - - - diff --git a/roswww_static/main.py b/roswww_static/src/update similarity index 82% rename from roswww_static/main.py rename to roswww_static/src/update index 0b168509..6fbd0665 100755 --- a/roswww_static/main.py +++ b/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 = '

Packages list

\n
    \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 += '
  • {name}
    • '.format(name=name) @@ -42,7 +40,7 @@ if default_package is not None: redirect_html = ''.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)