From eb448ae0e79e3e192dafd85a625756a91b453b83 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Wed, 12 Oct 2022 00:25:12 +0600 Subject: [PATCH 001/144] main_camera.launch: run image_raw_throttled topic by default (#248) --- clover/launch/main_camera.launch | 6 ++++++ clover/test/basic.test | 3 +++ docs/en/camera.md | 2 ++ docs/ru/camera.md | 2 ++ 4 files changed, 13 insertions(+) diff --git a/clover/launch/main_camera.launch b/clover/launch/main_camera.launch index 312a0f15..8e932ce4 100644 --- a/clover/launch/main_camera.launch +++ b/clover/launch/main_camera.launch @@ -4,6 +4,8 @@ + + @@ -43,4 +45,8 @@ + + + diff --git a/clover/test/basic.test b/clover/test/basic.test index 3aac7f10..bd5366f0 100755 --- a/clover/test/basic.test +++ b/clover/test/basic.test @@ -37,6 +37,9 @@ + + diff --git a/docs/en/camera.md b/docs/en/camera.md index bbf68d87..ba0dbd3a 100644 --- a/docs/en/camera.md +++ b/docs/en/camera.md @@ -145,6 +145,8 @@ rospy.spin() The script will take up to 100% CPU capacity. To slow down the script artificially, you can use [throttling](http://wiki.ros.org/topic_tools/throttle) of frames from the camera, for example, at 5 Hz (`main_camera.launch`): +> **Note** Starting from [image](image.md) version **0.24** `image_raw_throttled` topic is available without addition configuration. + ```xml diff --git a/docs/ru/camera.md b/docs/ru/camera.md index 7d90084e..0f9b6a14 100644 --- a/docs/ru/camera.md +++ b/docs/ru/camera.md @@ -147,6 +147,8 @@ rospy.spin() Скрипт будет занимать 100% процессора. Для искусственного замедления работы скрипта можно запустить [throttling](http://wiki.ros.org/topic_tools/throttle) кадров с камеры, например, в 5 Гц (`main_camera.launch`): +> **Note** Начиная с версии [образа](image.md) **0.24** топик `image_raw_throttled` доступен без дополнительной конфигурации. + ```xml From 94171d51aceecd72cdc3d5ff7d3fa6b022142871 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Thu, 13 Oct 2022 00:07:27 +0600 Subject: [PATCH 002/144] simple_offboard: implement simple_offboard/release service --- clover/src/simple_offboard.cpp | 8 ++++++++ docs/en/simple_offboard.md | 10 ++++++++++ docs/ru/simple_offboard.md | 10 ++++++++++ 3 files changed, 28 insertions(+) diff --git a/clover/src/simple_offboard.cpp b/clover/src/simple_offboard.cpp index b8f57033..fc755d77 100644 --- a/clover/src/simple_offboard.cpp +++ b/clover/src/simple_offboard.cpp @@ -860,6 +860,13 @@ bool land(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) return false; } +bool release(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res) +{ + setpoint_timer.stop(); + res.success = true; + return true; +} + int main(int argc, char **argv) { ros::init(argc, argv, "simple_offboard"); @@ -933,6 +940,7 @@ int main(int argc, char **argv) auto sa_serv = nh.advertiseService("set_attitude", &setAttitude); auto sr_serv = nh.advertiseService("set_rates", &setRates); auto ld_serv = nh.advertiseService("land", &land); + auto rl_serv = nh_priv.advertiseService("release", &release); // Setpoint timer setpoint_timer = nh.createTimer(ros::Duration(1 / nh_priv.param("setpoint_rate", 30.0)), &publishSetpoint, false, false); diff --git a/docs/en/simple_offboard.md b/docs/en/simple_offboard.md index 832bd363..cff1c743 100644 --- a/docs/en/simple_offboard.md +++ b/docs/en/simple_offboard.md @@ -305,6 +305,16 @@ rosservice call /land "{}" > **Caution** In recent PX4 versions, the vehicle will be switched out of LAND mode to manual mode, if the remote control sticks are moved significantly. +### release + +If it's necessary to pause sending setpoint messages, use the `simple_offboard/release` service: + +```python +release = rospy.ServiceProxy('simple_offboard/release', Trigger) + +release() +``` + ## Additional materials * [ArUco-based position estimation and navigation](aruco.md). diff --git a/docs/ru/simple_offboard.md b/docs/ru/simple_offboard.md index 29938e25..c9d51474 100644 --- a/docs/ru/simple_offboard.md +++ b/docs/ru/simple_offboard.md @@ -305,6 +305,16 @@ rosservice call /land "{}" > **Caution** В более новых версиях PX4 коптер выйдет из режима LAND в ручной режим, если сильно перемещать стики. +### release + +В случае необходимости приостановки отправки setpoint-сообщений, используйте сервис `simple_offboard/release`: + +```python +release = rospy.ServiceProxy('simple_offboard/release', Trigger) + +release() +``` + ## Дополнительные материалы * [Полеты в поле ArUco-маркеров](aruco.md). From 4e9d8a64d0141eced651557670418faaace42ff7 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Thu, 13 Oct 2022 00:08:35 +0600 Subject: [PATCH 003/144] simple_offboard: test for simple_offboard/release service presence --- clover/test/basic.py | 1 + 1 file changed, 1 insertion(+) diff --git a/clover/test/basic.py b/clover/test/basic.py index 5d9d6c29..21440bd2 100755 --- a/clover/test/basic.py +++ b/clover/test/basic.py @@ -24,6 +24,7 @@ def test_simple_offboard_services_available(): rospy.wait_for_service('set_attitude', timeout=5) rospy.wait_for_service('set_rates', timeout=5) rospy.wait_for_service('land', timeout=5) + rospy.wait_for_service('simple_offboard/release', timeout=5) def test_web_video_server(node): try: From 0f5f111f46c5eeb8ed6aceab8ab6502516fce3d8 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Thu, 13 Oct 2022 02:05:56 +0600 Subject: [PATCH 004/144] docs: minor fix --- docs/en/simple_offboard.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/en/simple_offboard.md b/docs/en/simple_offboard.md index cff1c743..14985dfa 100644 --- a/docs/en/simple_offboard.md +++ b/docs/en/simple_offboard.md @@ -103,7 +103,7 @@ Parameters: * `yaw_rate` – angular yaw velocity (will be used if yaw is set to `NaN`) *(rad/s)*; * `speed` – flight speed (setpoint speed) *(m/s)*; * `auto_arm` – switch the drone to `OFFBOARD` mode and arm automatically (**the drone will take off**); -* `frame_id` – [coordinate system](frames.md) for values `x`, `y`, `z`, `vx`, `vy`, `vz`. Example: `map`, `body`, `aruco_map`. Default value: `map`. +* `frame_id` – [coordinate system](frames.md) for values `x`, `y`, `z` and `yaw`. Example: `map`, `body`, `aruco_map`. Default value: `map`. > **Note** If you don't want to change your current yaw set the `yaw` parameter to `NaN` (angular velocity by default is 0). From d6f9327ede7ee1c199cb2c88dbee6326feb07a05 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Tue, 25 Oct 2022 05:10:25 +0600 Subject: [PATCH 005/144] simulation: set distance sensor's fov to 27 deg As vl53l1x rangefinder specification stances --- clover_description/urdf/sensors/distance_sensor.urdf.xacro | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/clover_description/urdf/sensors/distance_sensor.urdf.xacro b/clover_description/urdf/sensors/distance_sensor.urdf.xacro index 4b539a29..591c2cfd 100644 --- a/clover_description/urdf/sensors/distance_sensor.urdf.xacro +++ b/clover_description/urdf/sensors/distance_sensor.urdf.xacro @@ -2,7 +2,7 @@ - + @@ -58,7 +58,7 @@ /rangefinder/range rangefinder infrared - 0.01 + ${fov} 0.001 ${rate} ${range_min} From 26245dfb42e8036609e08176bb9f1c64c25ee983 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Wed, 26 Oct 2022 03:31:00 +0600 Subject: [PATCH 006/144] docs: add snippet on how to check if code is running inside simulation --- docs/en/snippets.md | 8 ++++++++ docs/ru/snippets.md | 8 ++++++++ 2 files changed, 16 insertions(+) diff --git a/docs/en/snippets.md b/docs/en/snippets.md index af2e36c9..5ce93c97 100644 --- a/docs/en/snippets.md +++ b/docs/en/snippets.md @@ -480,3 +480,11 @@ param_set(param_id='COM_FLTMODE1', value=ParamValue(integer=8)) # Set parameter of type FLOAT: param_set(param_id='MPC_Z_P', value=ParamValue(real=1.5)) ``` + +### # {#is-simulation} + +Check, if the code is running inside a [Gazebo simulation](simulation.md): + +```python +is_simulation = rospy.get_param('/use_sim_time', False) +``` diff --git a/docs/ru/snippets.md b/docs/ru/snippets.md index 85af2531..0e5dc95d 100644 --- a/docs/ru/snippets.md +++ b/docs/ru/snippets.md @@ -491,3 +491,11 @@ param_set(param_id='COM_FLTMODE1', value=ParamValue(integer=8)) # Изменить параметр типа FLOAT: param_set(param_id='MPC_Z_P', value=ParamValue(real=1.5)) ``` + +### # {#is-simulation} + +Проверить, что код запущен в [симуляции Gazebo](simulation.md): + +```python +is_simulation = rospy.get_param('/use_sim_time', False) +``` From b855c4586a3228fb53e34c2f2e6ad5628f19fe0c Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Sun, 30 Oct 2022 04:05:53 +0600 Subject: [PATCH 007/144] led: allow to use namespaced mavros (from #439) rosout_agg cannot be namespaced: https://github.com/ros/ros_comm/blob/f5fa3a168760d62e9693f10dcb9adfffc6132d22/tools/rosout/rosout.cpp#L127 Co-Authored-By: Playergeek181 <93044104+Playergeek181@users.noreply.github.com> --- clover/src/led.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/clover/src/led.cpp b/clover/src/led.cpp index 8d437c05..91438df3 100644 --- a/clover/src/led.cpp +++ b/clover/src/led.cpp @@ -319,8 +319,8 @@ int main(int argc, char **argv) auto set_effect = nh.advertiseService("set_effect", &setEffect); - auto mavros_state_sub = nh.subscribe("/mavros/state", 1, &handleMavrosState); - auto battery_sub = nh.subscribe("/mavros/battery", 1, &handleBattery); + auto mavros_state_sub = nh.subscribe("mavros/state", 1, &handleMavrosState); + auto battery_sub = nh.subscribe("mavros/battery", 1, &handleBattery); auto rosout_sub = nh.subscribe("/rosout_agg", 1, &handleLog); timer = nh.createTimer(ros::Duration(0), &proceed, false, false); From b41cb6b581c739f5fe5b3bc588e9275aceea08d7 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Sun, 30 Oct 2022 05:10:54 +0600 Subject: [PATCH 008/144] selfcheck.py: minor fix --- 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 159fcad2..933155de 100755 --- a/clover/src/selfcheck.py +++ b/clover/src/selfcheck.py @@ -356,7 +356,7 @@ def check_aruco(): known_tilt += ' (ALL markers are on the floor)' elif known_tilt == 'map_flipped': known_tilt += ' (ALL markers are on the ceiling)' - info('aruco_detector/known_tilt = %s', known_tilt) + info('aruco_detect/known_tilt = %s', known_tilt) try: rospy.wait_for_message('aruco_detect/markers', MarkerArray, timeout=1) except rospy.ROSException: From f9450fe03db26f8537b4b4def1e12d51102d033f Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Sun, 30 Oct 2022 05:34:57 +0600 Subject: [PATCH 009/144] selfcheck.py: skip px4 version check in SITL --- clover/src/selfcheck.py | 37 +++++++++++++++++++------------------ 1 file changed, 19 insertions(+), 18 deletions(-) diff --git a/clover/src/selfcheck.py b/clover/src/selfcheck.py index 933155de..347d2785 100755 --- a/clover/src/selfcheck.py +++ b/clover/src/selfcheck.py @@ -198,27 +198,28 @@ def check_fcu(): failure('no connection to the FCU (check wiring)') return - clover_tag = re.compile(r'-cl[oe]ver\.\d+$') - clover_fw = False + if not is_process_running('px4', exact=True): # can't use px4 console in SITL + clover_tag = re.compile(r'-cl[oe]ver\.\d+$') + clover_fw = False - # Make sure the console is available to us - mavlink_exec('\n') - version_str = mavlink_exec('ver all') - if version_str == '': - info('no version data available from SITL') + # Make sure the console is available to us + mavlink_exec('\n') + version_str = mavlink_exec('ver all') + if version_str == '': + info('no version data available from SITL') - for line in version_str.split('\n'): - if line.startswith('FW version: '): - info(line[len('FW version: '):]) - elif line.startswith('FW git tag: '): # only Clover's firmware - tag = line[len('FW git tag: '):] - clover_fw = clover_tag.search(tag) - info(tag) - elif line.startswith('HW arch: '): - info(line[len('HW arch: '):]) + for line in version_str.split('\n'): + if line.startswith('FW version: '): + info(line[len('FW version: '):]) + elif line.startswith('FW git tag: '): # only Clover's firmware + tag = line[len('FW git tag: '):] + clover_fw = clover_tag.search(tag) + info(tag) + elif line.startswith('HW arch: '): + info(line[len('HW arch: '):]) - if not clover_fw: - info('not Clover PX4 firmware, check https://clover.coex.tech/firmware') + if not clover_fw: + info('not Clover PX4 firmware, check https://clover.coex.tech/firmware') est = get_param('SYS_MC_EST_GROUP') if est == 1: From 7f31fdd320d054cc7d0cb28909ff87e4e3ea0b32 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Sun, 30 Oct 2022 18:39:51 +0600 Subject: [PATCH 010/144] docs: minor cleanup in snippets --- docs/en/snippets.md | 8 ++++---- docs/ru/snippets.md | 8 ++++---- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/docs/en/snippets.md b/docs/en/snippets.md index 5ce93c97..315e1f55 100644 --- a/docs/en/snippets.md +++ b/docs/en/snippets.md @@ -207,9 +207,9 @@ def pose_update(pose): # Processing new data of copter's position pass -rospy.Subscriber('/mavros/local_position/pose', PoseStamped, pose_update) -rospy.Subscriber('/mavros/local_position/velocity', TwistStamped, velocity_update) -rospy.Subscriber('/mavros/battery', BatteryState, battery_update) +rospy.Subscriber('mavros/local_position/pose', PoseStamped, pose_update) +rospy.Subscriber('mavros/local_position/velocity', TwistStamped, velocity_update) +rospy.Subscriber('mavros/battery', BatteryState, battery_update) rospy.Subscriber('mavros/rc/in', RCIn, rc_callback) rospy.spin() @@ -349,7 +349,7 @@ from pymavlink import mavutil from mavros_msgs.srv import CommandLong from mavros_msgs.msg import State -send_command = rospy.ServiceProxy('/mavros/cmd/command', CommandLong) +send_command = rospy.ServiceProxy('mavros/cmd/command', CommandLong) def calibrate_gyro(): rospy.loginfo('Calibrate gyro') diff --git a/docs/ru/snippets.md b/docs/ru/snippets.md index 0e5dc95d..45b1be6f 100644 --- a/docs/ru/snippets.md +++ b/docs/ru/snippets.md @@ -217,9 +217,9 @@ def pose_update(pose): # Обработка новых данных о позиции коптера pass -rospy.Subscriber('/mavros/local_position/pose', PoseStamped, pose_update) -rospy.Subscriber('/mavros/local_position/velocity', TwistStamped, velocity_update) -rospy.Subscriber('/mavros/battery', BatteryState, battery_update) +rospy.Subscriber('mavros/local_position/pose', PoseStamped, pose_update) +rospy.Subscriber('mavros/local_position/velocity', TwistStamped, velocity_update) +rospy.Subscriber('mavros/battery', BatteryState, battery_update) rospy.Subscriber('mavros/rc/in', RCIn, rc_callback) rospy.spin() @@ -360,7 +360,7 @@ from pymavlink import mavutil from mavros_msgs.srv import CommandLong from mavros_msgs.msg import State -send_command = rospy.ServiceProxy('/mavros/cmd/command', CommandLong) +send_command = rospy.ServiceProxy('mavros/cmd/command', CommandLong) def calibrate_gyro(): rospy.loginfo('Calibrate gyro') From 63619847946ffe13855ea486c701136dd59d497b Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Sun, 30 Oct 2022 19:21:10 +0600 Subject: [PATCH 011/144] selfcheck.py: don't fail when marker's map not detected --- clover/src/selfcheck.py | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/clover/src/selfcheck.py b/clover/src/selfcheck.py index 347d2785..cc5cf13f 100755 --- a/clover/src/selfcheck.py +++ b/clover/src/selfcheck.py @@ -347,6 +347,8 @@ def is_process_running(binary, exact=False, full=False): @check('ArUco markers') def check_aruco(): + markers = None + if is_process_running('aruco_detect', full=True): try: info('aruco_detect/length = %g m', rospy.get_param('aruco_detect/length')) @@ -359,7 +361,7 @@ def check_aruco(): known_tilt += ' (ALL markers are on the ceiling)' info('aruco_detect/known_tilt = %s', known_tilt) try: - rospy.wait_for_message('aruco_detect/markers', MarkerArray, timeout=1) + markers = rospy.wait_for_message('aruco_detect/markers', MarkerArray, timeout=1) except rospy.ROSException: failure('no markers detection') return @@ -384,7 +386,12 @@ def check_aruco(): try: rospy.wait_for_message('aruco_map/pose', PoseWithCovarianceStamped, timeout=1) except rospy.ROSException: - failure('no map detection') + if not markers: + info('no map detection as no markers detection') + elif not markers.markers: + info('no map detection as no markers detected') + else: + failure('no map detection') else: info('aruco_map is not running') From 1700ad24df2b55ee68b6006871c84c23e1f07f93 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Sun, 30 Oct 2022 19:21:42 +0600 Subject: [PATCH 012/144] selfcheck.py: don't failure when no vpe and drone is on the floor --- clover/src/selfcheck.py | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/clover/src/selfcheck.py b/clover/src/selfcheck.py index cc5cf13f..efcf0b45 100755 --- a/clover/src/selfcheck.py +++ b/clover/src/selfcheck.py @@ -396,6 +396,14 @@ def check_aruco(): info('aruco_map is not running') +def is_on_the_floor(): + try: + dist = rospy.wait_for_message('rangefinder/range', Range, timeout=1) + return dist.range < 0.3 + except rospy.ROSException: + return False + + @check('Vision position estimate') def check_vpe(): vis = None @@ -405,7 +413,12 @@ def check_vpe(): try: vis = rospy.wait_for_message('mavros/mocap/pose', PoseStamped, timeout=1) except rospy.ROSException: - failure('no VPE or MoCap messages') + if rospy.get_param('aruco_map/known_tilt') == 'map_flipped': + failure('no vision position estimate, markers are on the ceiling') + elif is_on_the_floor(): + info('no vision position estimate, the drone is on the floor') + else: + failure('no vision position estimate') # check if vpe_publisher is running try: subprocess.check_output(['pgrep', '-x', 'vpe_publisher']) From 69d5d1e521e1488adfc9494a96772cf5b41bb992 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Sun, 30 Oct 2022 19:48:42 +0600 Subject: [PATCH 013/144] selfcheck.py: don't fail when there is no vpe and vpe_publisher is not running --- clover/src/selfcheck.py | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/clover/src/selfcheck.py b/clover/src/selfcheck.py index efcf0b45..2223b9d9 100755 --- a/clover/src/selfcheck.py +++ b/clover/src/selfcheck.py @@ -413,17 +413,14 @@ def check_vpe(): try: vis = rospy.wait_for_message('mavros/mocap/pose', PoseStamped, timeout=1) except rospy.ROSException: - if rospy.get_param('aruco_map/known_tilt') == 'map_flipped': + if not is_process_running('vpe_publisher', full=True): + info('no vision position estimate, vpe_publisher is not running') + elif rospy.get_param('aruco_map/known_tilt') == 'map_flipped': failure('no vision position estimate, markers are on the ceiling') elif is_on_the_floor(): info('no vision position estimate, the drone is on the floor') else: failure('no vision position estimate') - # check if vpe_publisher is running - try: - subprocess.check_output(['pgrep', '-x', 'vpe_publisher']) - except subprocess.CalledProcessError: - return # it's not running, skip following checks # check PX4 settings est = get_param('SYS_MC_EST_GROUP') From c907e6041ae716c06c578af90d8ac008da4dbefc Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Sun, 30 Oct 2022 20:44:13 +0600 Subject: [PATCH 014/144] selfcheck.py: skip battery check in simulation --- clover/src/selfcheck.py | 25 +++++++++++++------------ 1 file changed, 13 insertions(+), 12 deletions(-) diff --git a/clover/src/selfcheck.py b/clover/src/selfcheck.py index 2223b9d9..0de6d856 100755 --- a/clover/src/selfcheck.py +++ b/clover/src/selfcheck.py @@ -256,18 +256,19 @@ def check_fcu(): if cbrk_usb_chk != 197848: failure('set parameter CBRK_USB_CHK to 197848 for flying with USB connected') - try: - battery = rospy.wait_for_message('mavros/battery', BatteryState, timeout=3) - if not battery.cell_voltage: - failure('cell voltage is not available, https://clover.coex.tech/power') - else: - cell = battery.cell_voltage[0] - if cell > 4.3 or cell < 3.0: - failure('incorrect cell voltage: %.2f V, https://clover.coex.tech/power', cell) - elif cell < 3.7: - failure('critically low cell voltage: %.2f V, recharge battery', cell) - except rospy.ROSException: - failure('no battery state') + if not is_process_running('px4', exact=True): # skip battery check in SITL + try: + battery = rospy.wait_for_message('mavros/battery', BatteryState, timeout=3) + if not battery.cell_voltage: + failure('cell voltage is not available, https://clover.coex.tech/power') + else: + cell = battery.cell_voltage[0] + if cell > 4.3 or cell < 3.0: + failure('incorrect cell voltage: %.2f V, https://clover.coex.tech/power', cell) + elif cell < 3.7: + failure('critically low cell voltage: %.2f V, recharge battery', cell) + except rospy.ROSException: + failure('no battery state') except rospy.ROSException: failure('no MAVROS state (check wiring)') From 070c23be53a5c0eafef5188f7c58acc4b6c3dd79 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Sun, 30 Oct 2022 21:00:45 +0600 Subject: [PATCH 015/144] selfcheck.py: shorten some reports replacing 'is' with '=' --- clover/src/selfcheck.py | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/clover/src/selfcheck.py b/clover/src/selfcheck.py index 0de6d856..419a3ac6 100755 --- a/clover/src/selfcheck.py +++ b/clover/src/selfcheck.py @@ -440,7 +440,7 @@ def check_vpe(): delay = get_param('LPE_VIS_DELAY') if delay != 0: failure('LPE_VIS_DELAY parameter is %s, but it should be zero', delay) - info('LPE_VIS_XY is %.2f m, LPE_VIS_Z is %.2f m', get_param('LPE_VIS_XY'), get_param('LPE_VIS_Z')) + info('LPE_VIS_XY = %.2f m, LPE_VIS_Z = %.2f m', get_param('LPE_VIS_XY'), get_param('LPE_VIS_Z')) elif est == 2: fuse = get_param('EKF2_AID_MASK') if not fuse & (1 << 3): @@ -449,8 +449,8 @@ def check_vpe(): failure('vision yaw fusion is disabled, change EKF2_AID_MASK parameter') delay = get_param('EKF2_EV_DELAY') if delay != 0: - failure('EKF2_EV_DELAY is %.2f, but it should be zero', delay) - info('EKF2_EVA_NOISE is %.3f, EKF2_EVP_NOISE is %.3f', + 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')) @@ -566,7 +566,7 @@ def check_optical_flow(): # check PX4 settings rot = get_param('SENS_FLOW_ROT') if rot != 0: - failure('SENS_FLOW_ROT parameter is %s, but it should be zero', rot) + failure('SENS_FLOW_ROT = %s, but it should be zero', rot) est = get_param('SYS_MC_EST_GROUP') if est == 1: fuse = get_param('LPE_FUSION') @@ -576,9 +576,9 @@ def check_optical_flow(): failure('flow gyro compensation is disabled, change LPE_FUSION parameter') scale = get_param('LPE_FLW_SCALE') if not numpy.isclose(scale, 1.0): - failure('LPE_FLW_SCALE parameter is %.2f, but it should be 1.0', scale) + failure('LPE_FLW_SCALE = %.2f, but it should be 1.0', scale) - info('LPE_FLW_QMIN is %s, LPE_FLW_R is %.4f, LPE_FLW_RR is %.4f, SENS_FLOW_MINHGT is %.3f, SENS_FLOW_MAXHGT is %.3f', + info('LPE_FLW_QMIN = %s, LPE_FLW_R = %.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'), @@ -590,8 +590,8 @@ def check_optical_flow(): failure('optical flow fusion is disabled, change EKF2_AID_MASK parameter') delay = get_param('EKF2_OF_DELAY') if delay != 0: - failure('EKF2_OF_DELAY is %.2f, but it should be zero', delay) - info('EKF2_OF_QMIN is %s, EKF2_OF_N_MIN is %.4f, EKF2_OF_N_MAX is %.4f, SENS_FLOW_MINHGT is %.3f, SENS_FLOW_MAXHGT is %.3f', + 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'), From 0b62f677afceeb63bbd18f3967be7ebb4ad6a2c9 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Sun, 30 Oct 2022 22:28:33 +0600 Subject: [PATCH 016/144] aruco_detect: consider markers size from markers map `use_map_markers` parameter added --- aruco_pose/src/aruco_detect.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/aruco_pose/src/aruco_detect.cpp b/aruco_pose/src/aruco_detect.cpp index fe1ad5be..062caf80 100644 --- a/aruco_pose/src/aruco_detect.cpp +++ b/aruco_pose/src/aruco_detect.cpp @@ -71,7 +71,7 @@ private: ros::Publisher markers_pub_, vis_markers_pub_; ros::Subscriber map_markers_sub_; ros::ServiceServer set_markers_srv_; - bool estimate_poses_, send_tf_, auto_flip_; + bool estimate_poses_, send_tf_, auto_flip_, use_map_markers_; double length_; ros::Duration transform_timeout_; std::unordered_map length_override_; @@ -95,6 +95,7 @@ public: dictionary = nh_priv_.param("dictionary", 2); estimate_poses_ = nh_priv_.param("estimate_poses", true); send_tf_ = nh_priv_.param("send_tf", true); + use_map_markers_ = nh_priv_.param("use_map_markers", true); if (estimate_poses_ && !nh_priv_.getParam("length", length_)) { NODELET_FATAL("can't estimate marker's poses as ~length parameter is not defined"); ros::shutdown(); @@ -395,6 +396,11 @@ private: map_markers_ids_.clear(); for (auto const& marker : msg.markers) { map_markers_ids_.insert(marker.id); + if (use_map_markers_) { + if (length_override_.find(marker.id) == length_override_.end()) { + length_override_[marker.id] = marker.length; + } + } } } From 2c07bbffe38062d3768acb7ce8981a213b93dcc8 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Sun, 30 Oct 2022 23:16:03 +0600 Subject: [PATCH 017/144] aruco_map: fix visualization markers' orientation Was by mistake uninitialized --- aruco_pose/src/aruco_map.cpp | 2 +- aruco_pose/test/basic.py | 32 +++++++++++++++++++++++++++++++- 2 files changed, 32 insertions(+), 2 deletions(-) diff --git a/aruco_pose/src/aruco_map.cpp b/aruco_pose/src/aruco_map.cpp index 41250be8..3d59e875 100644 --- a/aruco_pose/src/aruco_map.cpp +++ b/aruco_pose/src/aruco_map.cpp @@ -503,7 +503,7 @@ publish_debug: vis_marker.pose.position.x = x; vis_marker.pose.position.y = y; vis_marker.pose.position.z = z; - tf::quaternionTFToMsg(q, marker.pose.orientation); + tf::quaternionTFToMsg(q, vis_marker.pose.orientation); vis_marker.frame_locked = true; vis_array_.markers.push_back(vis_marker); diff --git a/aruco_pose/test/basic.py b/aruco_pose/test/basic.py index 94203ef9..171f7090 100755 --- a/aruco_pose/test/basic.py +++ b/aruco_pose/test/basic.py @@ -6,7 +6,7 @@ import tf2_geometry_msgs from geometry_msgs.msg import PoseWithCovarianceStamped from sensor_msgs.msg import Image from aruco_pose.msg import MarkerArray -from visualization_msgs.msg import MarkerArray as VisMarkerArray +from visualization_msgs.msg import MarkerArray as VisMarkerArray, Marker as VisMarker @pytest.fixture @@ -199,6 +199,36 @@ def test_map_markers(node): def test_map_visualization(node): vis = rospy.wait_for_message('aruco_map/visualization', VisMarkerArray, timeout=5) + assert len(vis.markers) == 7 + assert vis.markers[0].header.frame_id == 'aruco_map' + assert vis.markers[0].type == VisMarker.CUBE + assert vis.markers[0].action == VisMarker.ADD + assert vis.markers[0].pose.position.x == 0 + assert vis.markers[0].pose.position.y == 0 + assert vis.markers[0].pose.position.z == 0 + assert vis.markers[0].pose.orientation.x == 0 + assert vis.markers[0].pose.orientation.y == 0 + assert vis.markers[0].pose.orientation.z == 0 + assert vis.markers[0].pose.orientation.w == 1 + assert vis.markers[0].scale.x == approx(0.33) + assert vis.markers[0].scale.y == approx(0.33) + assert vis.markers[0].scale.z == approx(0.001) + assert vis.markers[1].pose.position.x == 1 + assert vis.markers[1].pose.position.y == 0 + assert vis.markers[1].pose.position.z == 0 + assert vis.markers[1].pose.orientation.x == 0 + assert vis.markers[1].pose.orientation.y == 0 + assert vis.markers[1].pose.orientation.z == 0 + assert vis.markers[1].pose.orientation.w == 1 + # non-zero yaw marker: + assert vis.markers[4].scale.x == approx(0.5) + assert vis.markers[4].pose.position.x == approx(0.5) + assert vis.markers[4].pose.position.y == 2 + assert vis.markers[4].pose.position.z == 0 + assert vis.markers[4].pose.orientation.x == 0 + assert vis.markers[4].pose.orientation.y == 0 + assert vis.markers[4].pose.orientation.z == approx(0.5646424733950354) + assert vis.markers[4].pose.orientation.w == approx(0.8253356149096783) def test_map_debug(node): img = rospy.wait_for_message('aruco_map/debug', Image, timeout=5) From 1461dd22f475f6707ac5b2cdd7764d32e81c817c Mon Sep 17 00:00:00 2001 From: Elena Seliverstova <64311178+SeliverstovaE@users.noreply.github.com> Date: Sun, 30 Oct 2022 21:22:04 +0300 Subject: [PATCH 018/144] Update deadline --- docs/en/educational_contests.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/docs/en/educational_contests.md b/docs/en/educational_contests.md index ebd8192f..30a22beb 100644 --- a/docs/en/educational_contests.md +++ b/docs/en/educational_contests.md @@ -20,7 +20,7 @@ The main goal of the contest is aerial robotics popularization and community de * Third parties can provide technical support for recording a lecture. * The status of the participant is unlimited (student, representative of a general education institution, representative of the industry, amateur). -Applications deadline: September 1, 2022. +Applications deadline: November 30, 2022. ### How to apply? @@ -64,7 +64,7 @@ The main goal of the contest is aerial robotics popularization and community de The application to the contest is performed via the [Google Form](https://docs.google.com/forms/d/e/1FAIpQLSdelVy6yQ1iN6u88KeiEIKGj7gGaM0xccSt2tiYKB46ICmjkQ/viewform). -Applications deadline: September 1, 2022. +Applications deadline: November 30, 2022. ### Prizes @@ -105,7 +105,7 @@ The course is evaluated according to a separate, publicly available lesson submi The application to the contest is performed via the [Google Form](https://docs.google.com/forms/d/e/1FAIpQLSdf2Q68X4hPnFE9f3EP95AxPNnzHKqIsFHtTRT6EBKiH93wzg/viewform) where the link to the video course should be attached. -Applications deadline: September 1, 2022. +Applications deadline: November 30, 2022. ### Prizes From ee17a3badab07b2a50e8393a808e5b9426bbe221 Mon Sep 17 00:00:00 2001 From: Elena Seliverstova <64311178+SeliverstovaE@users.noreply.github.com> Date: Sun, 30 Oct 2022 21:38:40 +0300 Subject: [PATCH 019/144] docs: update course contest deadline --- docs/ru/educational_contests.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/docs/ru/educational_contests.md b/docs/ru/educational_contests.md index bbab6f34..48b0e12b 100644 --- a/docs/ru/educational_contests.md +++ b/docs/ru/educational_contests.md @@ -26,7 +26,7 @@ Прием заявок осуществляется через [Google Форму](https://docs.google.com/forms/d/e/1FAIpQLScE2kN5dO2OYNSM8hOYzOa5Qvh2uDdd9Fjx8OnL1W93bfEBgw/viewform). -Дедлайн подачи заявок: 1 сентября 2022 года. +Дедлайн подачи заявок: 30 ноября 2022 года. ### Призы @@ -64,7 +64,7 @@ Прием заявок осуществляется через [Google Форму](https://docs.google.com/forms/d/e/1FAIpQLSdelVy6yQ1iN6u88KeiEIKGj7gGaM0xccSt2tiYKB46ICmjkQ/viewform). -Дедлайн подачи заявок: 1 сентября 2022 года. +Дедлайн подачи заявок: 30 ноября 2022 года. ### Призы @@ -106,7 +106,7 @@ Прием заявок осуществляется через [Google Форму](https://docs.google.com/forms/d/e/1FAIpQLSdf2Q68X4hPnFE9f3EP95AxPNnzHKqIsFHtTRT6EBKiH93wzg/viewform). -Дедлайн подачи заявок: 1 сентября 2022 года +Дедлайн подачи заявок: 30 ноября 2022 года ### Призы From 99fad312c570ecfbf1c55a9c3036ff116e1cbfdf Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Mon, 31 Oct 2022 01:32:57 +0600 Subject: [PATCH 020/144] aruco_detect: wait util map is published to avoid race condition --- aruco_pose/src/aruco_detect.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/aruco_pose/src/aruco_detect.cpp b/aruco_pose/src/aruco_detect.cpp index 062caf80..0e7803a7 100644 --- a/aruco_pose/src/aruco_detect.cpp +++ b/aruco_pose/src/aruco_detect.cpp @@ -72,6 +72,7 @@ private: ros::Subscriber map_markers_sub_; ros::ServiceServer set_markers_srv_; bool estimate_poses_, send_tf_, auto_flip_, use_map_markers_; + bool waiting_for_map_; double length_; ros::Duration transform_timeout_; std::unordered_map length_override_; @@ -95,7 +96,8 @@ public: dictionary = nh_priv_.param("dictionary", 2); estimate_poses_ = nh_priv_.param("estimate_poses", true); send_tf_ = nh_priv_.param("send_tf", true); - use_map_markers_ = nh_priv_.param("use_map_markers", true); + use_map_markers_ = nh_priv_.param("use_map_markers", false); + waiting_for_map_ = use_map_markers_; if (estimate_poses_ && !nh_priv_.getParam("length", length_)) { NODELET_FATAL("can't estimate marker's poses as ~length parameter is not defined"); ros::shutdown(); @@ -134,6 +136,7 @@ private: void imageCallback(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr &cinfo) { if (!enabled_) return; + if (waiting_for_map_) return; Mat image = cv_bridge::toCvShare(msg, "bgr8")->image; @@ -402,6 +405,7 @@ private: } } } + waiting_for_map_ = false; } void paramCallback(aruco_pose::DetectorConfig &config, uint32_t level) From b165e154f58a49b3ed76e19df099ea56a9d5c761 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Mon, 31 Oct 2022 02:14:02 +0600 Subject: [PATCH 021/144] selfcheck.py: decrease some timeouts to speed up check --- clover/src/selfcheck.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/clover/src/selfcheck.py b/clover/src/selfcheck.py index 419a3ac6..a0af7ebf 100755 --- a/clover/src/selfcheck.py +++ b/clover/src/selfcheck.py @@ -362,7 +362,7 @@ def check_aruco(): known_tilt += ' (ALL markers are on the ceiling)' info('aruco_detect/known_tilt = %s', known_tilt) try: - markers = rospy.wait_for_message('aruco_detect/markers', MarkerArray, timeout=1) + markers = rospy.wait_for_message('aruco_detect/markers', MarkerArray, timeout=0.8) except rospy.ROSException: failure('no markers detection') return @@ -379,13 +379,13 @@ def check_aruco(): info('aruco_map/known_tilt = %s', known_tilt) try: - visualization = rospy.wait_for_message('aruco_map/visualization', VisualizationMarkerArray, timeout=1) + visualization = rospy.wait_for_message('aruco_map/visualization', VisualizationMarkerArray, timeout=0.8) info('map has %s markers', len(visualization.markers)) except: failure('cannot read aruco_map/visualization topic') try: - rospy.wait_for_message('aruco_map/pose', PoseWithCovarianceStamped, timeout=1) + rospy.wait_for_message('aruco_map/pose', PoseWithCovarianceStamped, timeout=0.8) except rospy.ROSException: if not markers: info('no map detection as no markers detection') @@ -409,10 +409,10 @@ def is_on_the_floor(): def check_vpe(): vis = None try: - vis = rospy.wait_for_message('mavros/vision_pose/pose', PoseStamped, timeout=1) + vis = rospy.wait_for_message('mavros/vision_pose/pose', PoseStamped, timeout=0.8) except rospy.ROSException: try: - vis = rospy.wait_for_message('mavros/mocap/pose', PoseStamped, timeout=1) + vis = rospy.wait_for_message('mavros/mocap/pose', PoseStamped, timeout=0.8) except rospy.ROSException: if not is_process_running('vpe_publisher', full=True): info('no vision position estimate, vpe_publisher is not running') @@ -550,7 +550,7 @@ def check_velocity(): @check('Global position (GPS)') def check_global_position(): try: - rospy.wait_for_message('mavros/global_position/global', NavSatFix, timeout=1) + rospy.wait_for_message('mavros/global_position/global', NavSatFix, timeout=0.8) except rospy.ROSException: info('no global position') if get_param('SYS_MC_EST_GROUP') == 2 and (get_param('EKF2_AID_MASK') & (1 << 0)): From 2e7997941106dd74c7d25ffcbdbf9b30995194f1 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Mon, 31 Oct 2022 02:25:53 +0600 Subject: [PATCH 022/144] selfcheck.py: implement experimental parameter for parallel checks run Run: rosrun clover selfcheck.py _parallel:=1 --- clover/src/selfcheck.py | 90 +++++++++++++++++++++++++---------------- 1 file changed, 56 insertions(+), 34 deletions(-) diff --git a/clover/src/selfcheck.py b/clover/src/selfcheck.py index a0af7ebf..c597eafc 100755 --- a/clover/src/selfcheck.py +++ b/clover/src/selfcheck.py @@ -15,7 +15,8 @@ import subprocess import re from collections import OrderedDict import traceback -from threading import Event +import threading +from threading import Event, Thread, Lock import numpy import rospy import tf2_ros @@ -53,38 +54,38 @@ tf_buffer = tf2_ros.Buffer() tf_listener = tf2_ros.TransformListener(tf_buffer) -failures = [] -infos = [] -current_check = None +thread_local = threading.local() +reports_lock = Lock() def failure(text, *args): msg = text % args - rospy.logwarn('%s: %s', current_check, msg) - failures.append(msg) + thread_local.reports += [{'failure': msg}] def info(text, *args): msg = text % args - rospy.loginfo('%s: %s', current_check, msg) - infos.append(msg) + thread_local.reports += [{'info': msg}] def check(name): def inner(fn): def wrapper(*args, **kwargs): - failures[:] = [] - infos[:] = [] - global current_check - current_check = name + thread_local.reports = [] try: fn(*args, **kwargs) except Exception as e: traceback.print_exc() rospy.logerr('%s: exception occurred', name) return - if not failures and not infos: - rospy.loginfo('%s: OK', name) + with reports_lock: + for report in thread_local.reports: + if 'failure' in report: + rospy.logerr('%s: %s', name, report['failure']) + elif 'info' in report: + rospy.loginfo('%s: %s', name, report['info']) + if not thread_local.reports: + rospy.loginfo('%s: OK', name) return wrapper return inner @@ -657,7 +658,7 @@ def check_boot_duration(): @check('CPU usage') def check_cpu_usage(): - WHITELIST = 'nodelet', 'gzclient', 'gzserver' + WHITELIST = 'nodelet', 'gzclient', 'gzserver', 'selfcheck.py' CMD = "top -n 1 -b -i | tail -n +8 | awk '{ printf(\"%-8s\\t%-8s\\t%-8s\\n\", $1, $9, $12); }'" output = subprocess.check_output(CMD, shell=True).decode() processes = output.split('\n') @@ -837,26 +838,47 @@ def check_board(): info('could not open /proc/device-tree/model, not a Raspberry Pi?') +def parallel_for(fns): + threads = [] + for fn in fns: + thread = Thread(target=fn) + thread.start() + threads.append(thread) + for thread in threads: + thread.join() + + +def consequentially_for(fns): + for fn in fns: + fn() + + def selfcheck(): - check_image() - check_board() - check_clover_service() - check_network() - check_fcu() - check_imu() - check_local_position() - check_velocity() - check_global_position() - check_preflight_status() - check_main_camera() - check_aruco() - check_simpleoffboard() - check_optical_flow() - check_vpe() - check_rangefinder() - check_rpi_health() - check_cpu_usage() - check_boot_duration() + checks = [ + check_image, + check_board, + check_clover_service, + check_network, + check_fcu, + check_imu, + check_local_position, + check_velocity, + check_global_position, + check_preflight_status, + check_main_camera, + check_aruco, + check_simpleoffboard, + check_optical_flow, + check_vpe, + check_rangefinder, + check_rpi_health, + check_cpu_usage, + check_boot_duration, + ] + if rospy.get_param('~parallel', False): + parallel_for(checks) + else: + consequentially_for(checks) if __name__ == '__main__': From 38a3f656abeadc2bf31212bdb167f32e172ec214 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Mon, 31 Oct 2022 02:28:02 +0600 Subject: [PATCH 023/144] selfcheck.py: add parameter to print the time spent on each check Usage: rosrun clover selfcheck.py _time:=1 --- clover/src/selfcheck.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/clover/src/selfcheck.py b/clover/src/selfcheck.py index c597eafc..2159b0f6 100755 --- a/clover/src/selfcheck.py +++ b/clover/src/selfcheck.py @@ -71,6 +71,7 @@ def info(text, *args): def check(name): def inner(fn): def wrapper(*args, **kwargs): + start = rospy.get_time() thread_local.reports = [] try: fn(*args, **kwargs) @@ -86,6 +87,8 @@ def check(name): rospy.loginfo('%s: %s', name, report['info']) if not thread_local.reports: rospy.loginfo('%s: OK', name) + if rospy.get_param('~time', False): + rospy.loginfo('%s: %.1f sec', name, rospy.get_time() - start) return wrapper return inner From 22293c2220042f6d8ed58dff00f8e594e952f336 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Mon, 31 Oct 2022 02:30:19 +0600 Subject: [PATCH 024/144] aruco.launch: set use_map_markers parameter to true --- clover/launch/aruco.launch | 1 + 1 file changed, 1 insertion(+) diff --git a/clover/launch/aruco.launch b/clover/launch/aruco.launch index 61328f94..53766c05 100644 --- a/clover/launch/aruco.launch +++ b/clover/launch/aruco.launch @@ -18,6 +18,7 @@ + From 6b05cb34e596d6c63f95ccb1c4308d9910d5b861 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Mon, 31 Oct 2022 02:31:47 +0600 Subject: [PATCH 025/144] optical_flow: add disable_on_vpe parameter (#461) --- clover/launch/clover.launch | 1 + clover/src/optical_flow.cpp | 35 +++++++++++++++++++++++++++-------- clover/src/selfcheck.py | 9 ++++++++- 3 files changed, 36 insertions(+), 9 deletions(-) diff --git a/clover/launch/clover.launch b/clover/launch/clover.launch index 1efe1b89..8c77a0f4 100644 --- a/clover/launch/clover.launch +++ b/clover/launch/clover.launch @@ -45,6 +45,7 @@ + diff --git a/clover/src/optical_flow.cpp b/clover/src/optical_flow.cpp index 7207f516..36a98b6d 100644 --- a/clover/src/optical_flow.cpp +++ b/clover/src/optical_flow.cpp @@ -25,6 +25,7 @@ #include #include #include +#include #include #include #include @@ -57,6 +58,9 @@ private: std::unique_ptr tf_listener_; bool calc_flow_gyro_; float flow_gyro_default_; + bool disable_on_vpe_; + ros::Subscriber vpe_sub_; + ros::Time last_vpe_time_; std::shared_ptr> dyn_srv_; void onInit() @@ -87,6 +91,11 @@ private: img_sub_ = it.subscribeCamera("image_raw", 1, &OpticalFlow::flow, this); + disable_on_vpe_ = nh_priv.param("disable_on_vpe", false); + if (disable_on_vpe_) { + vpe_sub_ = nh.subscribe("mavros/vision_pose/pose", 1, &OpticalFlow::vpeCallback, this); + } + dyn_srv_ = std::make_shared>(nh_priv); dynamic_reconfigure::Server::CallbackType cb; @@ -121,6 +130,12 @@ private: { if (!enabled_) return; + if (disable_on_vpe_ && + !last_vpe_time_.isZero() && + (msg->header.stamp - last_vpe_time_).toSec() < 0.1) { + return; + } + parseCameraInfo(cinfo); auto img = cv_bridge::toCvShare(msg, "mono8")->image; @@ -236,6 +251,14 @@ private: prev_ = curr_.clone(); prev_stamp_ = msg->header.stamp; + // Publish estimated angular velocity + geometry_msgs::TwistStamped velo; + velo.header.stamp = msg->header.stamp; + velo.header.frame_id = fcu_frame_id_; + velo.twist.angular.x = flow_fcu.vector.x / integration_time.toSec(); + velo.twist.angular.y = flow_fcu.vector.y / integration_time.toSec(); + velo_pub_.publish(velo); + publish_debug: // Publish debug image if (img_pub_.getNumSubscribers() > 0) { @@ -248,14 +271,6 @@ publish_debug: out_msg.image = img; img_pub_.publish(out_msg.toImageMsg()); } - - // Publish estimated angular velocity - geometry_msgs::TwistStamped velo; - velo.header.stamp = msg->header.stamp; - velo.header.frame_id = fcu_frame_id_; - velo.twist.angular.x = flow_fcu.vector.x / integration_time.toSec(); - velo.twist.angular.y = flow_fcu.vector.y / integration_time.toSec(); - velo_pub_.publish(velo); } } @@ -284,6 +299,10 @@ publish_debug: prev_ = Mat(); // clear previous frame } } + + void vpeCallback(const geometry_msgs::PoseStamped& vpe) { + last_vpe_time_ = vpe.header.stamp; + } }; PLUGINLIB_EXPORT_CLASS(OpticalFlow, nodelet::Nodelet) diff --git a/clover/src/selfcheck.py b/clover/src/selfcheck.py index 2159b0f6..64100ea9 100755 --- a/clover/src/selfcheck.py +++ b/clover/src/selfcheck.py @@ -603,7 +603,14 @@ def check_optical_flow(): get_param('SENS_FLOW_MAXHGT')) except rospy.ROSException: - failure('no optical flow data (from Raspberry)') + if rospy.get_param('optical_flow/disable_on_vpe', False): + try: + rospy.wait_for_message('mavros/vision_pose/pose', PoseStamped, timeout=1) + info('no optical flow as disable_on_vpe is true') + except: + failure('no optical flow on RPi, disable_on_vpe is true, but no vision pose also') + else: + failure('no optical flow on RPi') @check('Rangefinder') From 2dda726d3ec63968b233698b965b710b07ba6947 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Mon, 31 Oct 2022 02:32:45 +0600 Subject: [PATCH 026/144] clover.launch: turn on disable_on_vpe by default --- clover/launch/clover.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/clover/launch/clover.launch b/clover/launch/clover.launch index 8c77a0f4..d438bad5 100644 --- a/clover/launch/clover.launch +++ b/clover/launch/clover.launch @@ -45,7 +45,7 @@ - + From 89bfc150f3d0be8cfc47b935ccf34ed60a68c6ad Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Mon, 31 Oct 2022 02:41:21 +0600 Subject: [PATCH 027/144] 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 028/144] 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 029/144] 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 030/144] 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 031/144] 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 032/144] 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 033/144] 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 034/144] 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 035/144] 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 036/144] 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 037/144] 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 038/144] 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 45042cd6f51e60805847cf8f2222c9db00169ba7 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Fri, 4 Nov 2022 03:05:59 +0600 Subject: [PATCH 039/144] docs: updates to camera and computer vision article --- docs/en/camera.md | 60 +++++++++++++++++++++++++---------------------- docs/ru/camera.md | 60 +++++++++++++++++++++++++---------------------- 2 files changed, 64 insertions(+), 56 deletions(-) diff --git a/docs/en/camera.md b/docs/en/camera.md index ba0dbd3a..a4079a45 100644 --- a/docs/en/camera.md +++ b/docs/en/camera.md @@ -14,7 +14,7 @@ The `clover` service must be restarted after the launch-file has been edited: sudo systemctl restart clover ``` -You may use rqt or [web_video_server](web_video_server.md) to view the camera stream. +You may use [rqt](rviz.md) or [web_video_server](web_video_server.md) to view the camera stream. ## Troubleshooting @@ -52,8 +52,6 @@ The [SD card image](image.md) comes with a preinstalled [OpenCV](https://opencv. ### Python -Main article: http://wiki.ros.org/cv_bridge/Tutorials/ConvertingBetweenROSImagesAndOpenCVImagesPython. - An example of creating a subscriber for a topic with an image from the main camera for processing with OpenCV: ```python @@ -61,12 +59,14 @@ import rospy import cv2 from sensor_msgs.msg import Image from cv_bridge import CvBridge +from clover import long_callback -rospy.init_node('computer_vision_sample') +rospy.init_node('cv') bridge = CvBridge() +@long_callback def image_callback(data): - cv_image = bridge.imgmsg_to_cv2(data, 'bgr8') # OpenCV image + img = bridge.imgmsg_to_cv2(data, 'bgr8') # OpenCV image # Do any image processing with cv2... image_sub = rospy.Subscriber('main_camera/image_raw', Image, image_callback) @@ -74,19 +74,31 @@ image_sub = rospy.Subscriber('main_camera/image_raw', Image, image_callback) rospy.spin() ``` +> **Note** Image processing may take significant time to finish. This can cause an [issue](https://github.com/ros/ros_comm/issues/1901) in rospy library, which would lead to processing stale camera frames. To solve this problem you need to use `long_callback` decorator from `clover` library, as in the example above. + +#### Limiting CPU usage + +When using the `main_camera/image_raw` topic, the script will process the maximum number of frames from the camera, actively utilizing the CPU (up to 100%). In tasks, where processing each camera frame is not critical, you can use the topic, where the frames are published at rate 5 Hz: `main_camera/image_raw_throttled`: + +```python +image_sub = rospy.Subscriber('main_camera/image_raw_throttled', Image, image_callback, queue_size=1) +``` + +#### Publishing images + To debug image processing, you can publish a separate topic with the processed image: ```python image_pub = rospy.Publisher('~debug', Image) ``` -Publishing the processed image (at the end of the image_callback function): +Publishing the processed image: ```python -image_pub.publish(bridge.cv2_to_imgmsg(cv_image, 'bgr8')) +image_pub.publish(bridge.cv2_to_imgmsg(img, 'bgr8')) ``` -The obtained images can be viewed using [web_video_server](web_video_server.md). +The published images can be viewed using [web_video_server](web_video_server.md) or [rqt](rviz.md). #### Retrieving one frame @@ -97,7 +109,7 @@ import rospy from sensor_msgs.msg import Image from cv_bridge import CvBridge -rospy.init_node('computer_vision_sample') +rospy.init_node('cv') bridge = CvBridge() # ... @@ -119,40 +131,32 @@ QR codes recognition in Python: ```python import rospy from pyzbar import pyzbar +import cv2 from cv_bridge import CvBridge from sensor_msgs.msg import Image +from clover import long_callback +rospy.init_node('cv') bridge = CvBridge() -rospy.init_node('barcode_test') - -# Image subscriber callback function -def image_callback(data): - cv_image = bridge.imgmsg_to_cv2(data, 'bgr8') # OpenCV image - barcodes = pyzbar.decode(cv_image) +@long_callback +def image_callback(msg): + img = bridge.imgmsg_to_cv2(msg, 'bgr8') + barcodes = pyzbar.decode(img) for barcode in barcodes: - b_data = barcode.data.decode("utf-8") + b_data = barcode.data.decode('utf-8') b_type = barcode.type (x, y, w, h) = barcode.rect xc = x + w/2 yc = y + h/2 - print("Found {} with data {} with center at x={}, y={}".format(b_type, b_data, xc, yc)) + print('Found {} with data {} with center at x={}, y={}'.format(b_type, b_data, xc, yc)) -image_sub = rospy.Subscriber('main_camera/image_raw', Image, image_callback, queue_size=1) +image_sub = rospy.Subscriber('main_camera/image_raw_throttled', Image, image_callback, queue_size=1) rospy.spin() ``` -The script will take up to 100% CPU capacity. To slow down the script artificially, you can use [throttling](http://wiki.ros.org/topic_tools/throttle) of frames from the camera, for example, at 5 Hz (`main_camera.launch`): - -> **Note** Starting from [image](image.md) version **0.24** `image_raw_throttled` topic is available without addition configuration. - -```xml - -``` - -The topic for the subscriber in this case should be changed for `main_camera/image_raw_throttled`. +> **Hint** See other computer vision examples in the `~/examples` directory of the [RPi image](image.md). ## Video recording diff --git a/docs/ru/camera.md b/docs/ru/camera.md index 0f9b6a14..ea4082c6 100644 --- a/docs/ru/camera.md +++ b/docs/ru/camera.md @@ -54,8 +54,6 @@ raspistill -o test.jpg ### Python -Основная статья: http://wiki.ros.org/cv_bridge/Tutorials/ConvertingBetweenROSImagesAndOpenCVImagesPython. - Пример создания подписчика на топик с изображением с основной камеры для обработки с использованием OpenCV: ```python @@ -63,12 +61,14 @@ import rospy import cv2 from sensor_msgs.msg import Image from cv_bridge import CvBridge +from clover import long_callback -rospy.init_node('computer_vision_sample') +rospy.init_node('cv') bridge = CvBridge() +@long_callback def image_callback(data): - cv_image = bridge.imgmsg_to_cv2(data, 'bgr8') # OpenCV image + img = bridge.imgmsg_to_cv2(data, 'bgr8') # OpenCV image # Do any image processing with cv2... image_sub = rospy.Subscriber('main_camera/image_raw', Image, image_callback) @@ -76,19 +76,31 @@ image_sub = rospy.Subscriber('main_camera/image_raw', Image, image_callback) rospy.spin() ``` +> **Note** Обработка изображения может занимать значительное время. Это может вызвать [проблему](https://github.com/ros/ros_comm/issues/1901) в библиотеке rospy, которая приведет к обработке устаревших кадров с камеры. Для решения этой проблемы необходимо использовать декоратор `long_callback` из библиотеки `clover`, как в примере выше. + +#### Ограничение использования CPU + +При использовании топика `main_camera/image_raw` скрипт будет обрабатывать максимальное количество кадров с камеры, активно используя CPU (вплоть до 100%). В задачах, где обработка каждого кадра не критична, можно использовать топик, где кадры публикуются с частотой 5 Гц: `main_camera/image_raw_throttled`: + +```python +image_sub = rospy.Subscriber('main_camera/image_raw_throttled', Image, image_callback, queue_size=1) +``` + +#### Публикация изображений + Для отладки обработки изображения можно публиковать отдельный топик с обработанным изображением: ```python image_pub = rospy.Publisher('~debug', Image) ``` -Публикация обработанного изображения (в конце функции image_callback): +Публикация обработанного изображения: ```python -image_pub.publish(bridge.cv2_to_imgmsg(cv_image, 'bgr8')) +image_pub.publish(bridge.cv2_to_imgmsg(img, 'bgr8')) ``` -Получаемые изображения можно просматривать используя [web_video_server](web_video_server.md). +Получаемые изображения можно просматривать используя [web_video_server](web_video_server.md) или [rqt](rviz.md). #### Получение одного кадра @@ -99,12 +111,12 @@ import rospy from sensor_msgs.msg import Image from cv_bridge import CvBridge -rospy.init_node('computer_vision_sample') +rospy.init_node('cv') bridge = CvBridge() # ... -# Получение кадра: +# Retrieve a frame: img = bridge.imgmsg_to_cv2(rospy.wait_for_message('main_camera/image_raw', Image), 'bgr8') ``` @@ -121,40 +133,32 @@ img = bridge.imgmsg_to_cv2(rospy.wait_for_message('main_camera/image_raw', Image ```python import rospy from pyzbar import pyzbar +import cv2 from cv_bridge import CvBridge from sensor_msgs.msg import Image +from clover import long_callback +rospy.init_node('cv') bridge = CvBridge() -rospy.init_node('barcode_test') - -# Image subscriber callback function -def image_callback(data): - cv_image = bridge.imgmsg_to_cv2(data, 'bgr8') # OpenCV image - barcodes = pyzbar.decode(cv_image) +@long_callback +def image_callback(msg): + img = bridge.imgmsg_to_cv2(msg, 'bgr8') + barcodes = pyzbar.decode(img) for barcode in barcodes: - b_data = barcode.data.decode("utf-8") + b_data = barcode.data.decode('utf-8') b_type = barcode.type (x, y, w, h) = barcode.rect xc = x + w/2 yc = y + h/2 - print("Found {} with data {} with center at x={}, y={}".format(b_type, b_data, xc, yc)) + print('Found {} with data {} with center at x={}, y={}'.format(b_type, b_data, xc, yc)) -image_sub = rospy.Subscriber('main_camera/image_raw', Image, image_callback, queue_size=1) +image_sub = rospy.Subscriber('main_camera/image_raw_throttled', Image, image_callback, queue_size=1) rospy.spin() ``` -Скрипт будет занимать 100% процессора. Для искусственного замедления работы скрипта можно запустить [throttling](http://wiki.ros.org/topic_tools/throttle) кадров с камеры, например, в 5 Гц (`main_camera.launch`): - -> **Note** Начиная с версии [образа](image.md) **0.24** топик `image_raw_throttled` доступен без дополнительной конфигурации. - -```xml - -``` - -Топик для подписчика в этом случае необходимо поменять на `main_camera/image_raw_throttled`. +> **Hint** Смотрите другие примеры по работе с компьютерным зрением в каталоге `~/examples` [образа для RPi](image.md). ## Запись видео From 2b13aa02eb87dbb0db04c62c40c12331dd0341a4 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Fri, 4 Nov 2022 03:10:56 +0600 Subject: [PATCH 040/144] 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 041/144] 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 042/144] 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 043/144] 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 044/144] 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 045/144] =?UTF-8?q?image:=20don=E2=80=99t=20test=20butterf?= =?UTF-8?q?ly=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 046/144] =?UTF-8?q?image:=20don=E2=80=99t=20test=20mjpg=5F?= =?UTF-8?q?streamer=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 047/144] 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 048/144] 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 049/144] 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 050/144] 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 051/144] 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 052/144] 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 053/144] 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 054/144] 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 055/144] 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 056/144] 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 057/144] 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 058/144] 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 059/144] 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 060/144] 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 061/144] 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 062/144] 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 063/144] 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 064/144] 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 065/144] 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 066/144] 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 067/144] 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