From e2dc628c2a3585dab1cdd4f7f033f69de546bd9f Mon Sep 17 00:00:00 2001 From: Arkadiy Telegin Date: Fri, 22 Dec 2023 14:29:30 +0100 Subject: [PATCH 01/25] Add proximity-warning package --- .gitmodules | 4 ++++ docker/proximity-warning | 1 + 2 files changed, 5 insertions(+) create mode 160000 docker/proximity-warning diff --git a/.gitmodules b/.gitmodules index bb4df47..e7310ec 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,3 +1,7 @@ [submodule "docker/realsense-ros"] path = docker/realsense-ros url = https://github.com/juandelos/realsense-ros.git + +[submodule "docker/proximity-warning"] + path = docker/proximity-warning + url = https://github.com/arkadiy-telegin/proximity-warning.git diff --git a/docker/proximity-warning b/docker/proximity-warning new file mode 160000 index 0000000..185193b --- /dev/null +++ b/docker/proximity-warning @@ -0,0 +1 @@ +Subproject commit 185193b1b341d7daaed0ca10e3d269cb9ea85093 From 54755fb7e232838e98ba0432d32cf595739245b0 Mon Sep 17 00:00:00 2001 From: Arkadiy Telegin Date: Fri, 22 Dec 2023 14:45:23 +0100 Subject: [PATCH 02/25] Update .gitmodules --- .gitmodules | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.gitmodules b/.gitmodules index e7310ec..db3377b 100644 --- a/.gitmodules +++ b/.gitmodules @@ -2,6 +2,6 @@ path = docker/realsense-ros url = https://github.com/juandelos/realsense-ros.git -[submodule "docker/proximity-warning"] - path = docker/proximity-warning - url = https://github.com/arkadiy-telegin/proximity-warning.git +[submodule "docker/proximity_monitor"] + path = docker/proximity_monitor + url = https://github.com/arkadiy-telegin/proximity_monitor.git From a018c1925928c7dd3b2ef87de474eeaad7536df3 Mon Sep 17 00:00:00 2001 From: Arkadiy Telegin Date: Fri, 22 Dec 2023 14:50:50 +0100 Subject: [PATCH 03/25] Update submodule --- docker/{proximity-warning => proximity_monitor} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename docker/{proximity-warning => proximity_monitor} (100%) diff --git a/docker/proximity-warning b/docker/proximity_monitor similarity index 100% rename from docker/proximity-warning rename to docker/proximity_monitor From 2355503158f8bce24eca4a0559d0c39eecfd72cc Mon Sep 17 00:00:00 2001 From: Arkadiy Telegin Date: Fri, 22 Dec 2023 15:05:04 +0100 Subject: [PATCH 04/25] Update launch file --- docker/multi_camera_launch.py | 24 ++++++++++++++++-------- 1 file changed, 16 insertions(+), 8 deletions(-) diff --git a/docker/multi_camera_launch.py b/docker/multi_camera_launch.py index 930a548..2ea747e 100644 --- a/docker/multi_camera_launch.py +++ b/docker/multi_camera_launch.py @@ -64,14 +64,22 @@ def generate_launch_description(): PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/rs_launch.py']), launch_arguments={'camera_name': 'camera_back', "serial_no":"_829212071844", "pointcloud.enable":"false"}.items(), ), + # launch_ros.actions.Node( + # package = "depth_handler", + # executable = "depth_subscriber" + # ), + # launch_ros.actions.Node( + # package = "depth_handler", + # executable = "pcd_subscriber", + # name = 'pointcloud_shoulder_left', + # parameters = [{'camera_side': 'left'}] + # ), launch_ros.actions.Node( - package = "depth_handler", - executable = "depth_subscriber" - ), - launch_ros.actions.Node( - package = "depth_handler", - executable = "pcd_subscriber", - name = 'pointcloud_shoulder_left', - parameters = [{'camera_side': 'left'}] + package = "proximity_monitor", + executable = "proximity_monitor", + name = 'proximity_monitor', + parameters = [{'first_camera_name': 'camera_shoulder_left'}, + {'second_camera_name': 'camera_back'}, + {'threshold': 1.0}] ), ]) \ No newline at end of file From 0423f51763d6667d599229a177b86afe2fb010b8 Mon Sep 17 00:00:00 2001 From: Arkadiy Telegin Date: Fri, 22 Dec 2023 15:12:17 +0100 Subject: [PATCH 05/25] Update dockerfile --- docker/Dockerfile | 1 + 1 file changed, 1 insertion(+) diff --git a/docker/Dockerfile b/docker/Dockerfile index 06dc605..ddce7a8 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -84,6 +84,7 @@ RUN apt-get update -y \ COPY docker/realsense-ros ./src COPY docker/multi_camera_launch.py /ros2_ws/src/realsense2_camera/launch COPY docker/depth_handler /ros2_ws/src/depth_handler +COPY docker/proximity_monitor /ros2_ws/src/proximity_monitor RUN apt-get install -y python3-rosdep \ && apt-get update \ && source /opt/ros/${ROS_DISTRO}/setup.bash \ From fcd2896be39a03849e21422061d48e3f88cbd3db Mon Sep 17 00:00:00 2001 From: Arkadiy Telegin Date: Fri, 22 Dec 2023 15:21:23 +0100 Subject: [PATCH 06/25] Update proximity_monitor --- docker/proximity_monitor | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docker/proximity_monitor b/docker/proximity_monitor index 185193b..d690782 160000 --- a/docker/proximity_monitor +++ b/docker/proximity_monitor @@ -1 +1 @@ -Subproject commit 185193b1b341d7daaed0ca10e3d269cb9ea85093 +Subproject commit d690782edaa06d390a3f3f22e8f9f30f868f9519 From 9cd8a98681b7ae02a0491048ccf7ba507e1123e4 Mon Sep 17 00:00:00 2001 From: Arkadiy Telegin Date: Sat, 23 Dec 2023 17:08:35 +0100 Subject: [PATCH 07/25] Fix bugs --- docker/multi_camera_launch.py | 2 +- docker/proximity_monitor | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/docker/multi_camera_launch.py b/docker/multi_camera_launch.py index 2ea747e..a92cf03 100644 --- a/docker/multi_camera_launch.py +++ b/docker/multi_camera_launch.py @@ -80,6 +80,6 @@ def generate_launch_description(): name = 'proximity_monitor', parameters = [{'first_camera_name': 'camera_shoulder_left'}, {'second_camera_name': 'camera_back'}, - {'threshold': 1.0}] + {'threshold': 0.2}] ), ]) \ No newline at end of file diff --git a/docker/proximity_monitor b/docker/proximity_monitor index d690782..d2db1bd 160000 --- a/docker/proximity_monitor +++ b/docker/proximity_monitor @@ -1 +1 @@ -Subproject commit d690782edaa06d390a3f3f22e8f9f30f868f9519 +Subproject commit d2db1bd22b88801b422dac5551a0b7a39ceef665 From 0a5559678c2e4115d31d106f506ebd77ce300842 Mon Sep 17 00:00:00 2001 From: Arkadiy Telegin Date: Sat, 23 Dec 2023 17:10:18 +0100 Subject: [PATCH 08/25] Update proximity_monitor to the latest version --- docker/proximity_monitor | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docker/proximity_monitor b/docker/proximity_monitor index d2db1bd..19e2794 160000 --- a/docker/proximity_monitor +++ b/docker/proximity_monitor @@ -1 +1 @@ -Subproject commit d2db1bd22b88801b422dac5551a0b7a39ceef665 +Subproject commit 19e27942d7d264d13f28874f66b9635ce8bf9fde From ba7a99b37e9f831074656a37be39d13c5f9b2c28 Mon Sep 17 00:00:00 2001 From: Arkadiy Telegin Date: Thu, 4 Jan 2024 18:03:01 +0100 Subject: [PATCH 09/25] Publish at 40Hz --- .gitmodules | 4 - docker/multi_camera_launch.py | 35 ++++-- docker/proximity_monitor | 1 - docker/proximity_monitor/CMakeLists.txt | 14 +++ .../launch/proximity_monitor_launch.py | 25 +++++ .../msg/ProximityWarning.msg | 12 ++ docker/proximity_monitor/package.xml | 31 ++++++ .../proximity_monitor/__init__.py | 0 .../__pycache__/__init__.cpython-310.pyc | Bin 0 -> 180 bytes .../proximity_monitor.cpython-310.pyc | Bin 0 -> 3614 bytes .../proximity_monitor/proximity_monitor.py | 104 ++++++++++++++++++ .../resource/proximity_monitor | 0 docker/proximity_monitor/setup.cfg | 4 + docker/proximity_monitor/setup.py | 26 +++++ .../proximity_monitor/test/test_copyright.py | 25 +++++ docker/proximity_monitor/test/test_flake8.py | 25 +++++ docker/proximity_monitor/test/test_pep257.py | 23 ++++ 17 files changed, 312 insertions(+), 17 deletions(-) delete mode 160000 docker/proximity_monitor create mode 100644 docker/proximity_monitor/CMakeLists.txt create mode 100644 docker/proximity_monitor/launch/proximity_monitor_launch.py create mode 100644 docker/proximity_monitor/msg/ProximityWarning.msg create mode 100644 docker/proximity_monitor/package.xml create mode 100644 docker/proximity_monitor/proximity_monitor/__init__.py create mode 100644 docker/proximity_monitor/proximity_monitor/__pycache__/__init__.cpython-310.pyc create mode 100644 docker/proximity_monitor/proximity_monitor/__pycache__/proximity_monitor.cpython-310.pyc create mode 100644 docker/proximity_monitor/proximity_monitor/proximity_monitor.py create mode 100644 docker/proximity_monitor/resource/proximity_monitor create mode 100644 docker/proximity_monitor/setup.cfg create mode 100644 docker/proximity_monitor/setup.py create mode 100644 docker/proximity_monitor/test/test_copyright.py create mode 100644 docker/proximity_monitor/test/test_flake8.py create mode 100644 docker/proximity_monitor/test/test_pep257.py diff --git a/.gitmodules b/.gitmodules index db3377b..bb4df47 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,7 +1,3 @@ [submodule "docker/realsense-ros"] path = docker/realsense-ros url = https://github.com/juandelos/realsense-ros.git - -[submodule "docker/proximity_monitor"] - path = docker/proximity_monitor - url = https://github.com/arkadiy-telegin/proximity_monitor.git diff --git a/docker/multi_camera_launch.py b/docker/multi_camera_launch.py index a92cf03..4afde9b 100644 --- a/docker/multi_camera_launch.py +++ b/docker/multi_camera_launch.py @@ -33,8 +33,8 @@ sys.path.append(str(pathlib.Path(__file__).parent.absolute())) import rs_launch -local_parameters = [{'name': 'camera_name1', 'default': 'camera_back', 'description': 'camera unique name'}, - {'name': 'camera_name2', 'default': 'camera2', 'description': 'camera unique name'}, +local_parameters = [{'name': 'camera_name1', 'default': 'camera_left', 'description': 'camera unique name'}, + # {'name': 'camera_name2', 'default': 'camera2', 'description': 'camera unique name'}, ] def set_configurable_parameters(local_params): @@ -50,20 +50,25 @@ def duplicate_params(general_params, posix): def generate_launch_description(): params1 = duplicate_params(rs_launch.configurable_parameters, '1') - params2 = duplicate_params(rs_launch.configurable_parameters, '2') + # params2 = duplicate_params(rs_launch.configurable_parameters, '2') return LaunchDescription( rs_launch.declare_configurable_parameters(local_parameters) + rs_launch.declare_configurable_parameters(params1) + - rs_launch.declare_configurable_parameters(params2) + + # rs_launch.declare_configurable_parameters(params2) + [ IncludeLaunchDescription( PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/rs_launch.py']), - launch_arguments={'camera_name': 'camera_shoulder_left', "serial_no":"_829212071824"}.items(), - ), - IncludeLaunchDescription( - PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/rs_launch.py']), - launch_arguments={'camera_name': 'camera_back', "serial_no":"_829212071844", "pointcloud.enable":"false"}.items(), + launch_arguments={ + 'camera_name': 'camera_left', + "serial_no":"_829212071824", + 'depth_module.profile': '424,240,5', + 'rgb_camera.profile': '424,240,5', + }.items(), ), + # IncludeLaunchDescription( + # PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/rs_launch.py']), + # launch_arguments={'camera_name': 'camera_back', "serial_no":"_829212071844", "pointcloud.enable":"false"}.items(), + # ), # launch_ros.actions.Node( # package = "depth_handler", # executable = "depth_subscriber" @@ -77,9 +82,15 @@ def generate_launch_description(): launch_ros.actions.Node( package = "proximity_monitor", executable = "proximity_monitor", - name = 'proximity_monitor', - parameters = [{'first_camera_name': 'camera_shoulder_left'}, - {'second_camera_name': 'camera_back'}, + name = 'proximity_monitor_left', + parameters = [{'camera_name': 'camera_left'}, {'threshold': 0.2}] ), + # launch_ros.actions.Node ( + # package = "proximity_monitor", + # executable = "proximity_monitor", + # name = 'proximity_monitor_back', + # parameters = [{'camera_name': 'camera_back'}, + # {'threshold': 0.2}] + # ), ]) \ No newline at end of file diff --git a/docker/proximity_monitor b/docker/proximity_monitor deleted file mode 160000 index 19e2794..0000000 --- a/docker/proximity_monitor +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 19e27942d7d264d13f28874f66b9635ce8bf9fde diff --git a/docker/proximity_monitor/CMakeLists.txt b/docker/proximity_monitor/CMakeLists.txt new file mode 100644 index 0000000..d9d0c1b --- /dev/null +++ b/docker/proximity_monitor/CMakeLists.txt @@ -0,0 +1,14 @@ +cmake_minimum_required(VERSION 3.5) +project(your_package_name) + +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(rclpy REQUIRED) +find_package(std_msgs REQUIRED) + +# rosidl_generate_interfaces(${PROJECT_NAME} +# "msg/ProximityWarning.msg" +# ) +# We use built-in messages to avoid the need to compile for Unity + +ament_package() diff --git a/docker/proximity_monitor/launch/proximity_monitor_launch.py b/docker/proximity_monitor/launch/proximity_monitor_launch.py new file mode 100644 index 0000000..de6add5 --- /dev/null +++ b/docker/proximity_monitor/launch/proximity_monitor_launch.py @@ -0,0 +1,25 @@ +from launch import LaunchDescription +from launch.actions import Node, DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration + +configurable_parameters = [ + {'name': 'first_camera_name', 'default': 'camera1', 'description': 'name of the first camera'}, + {'name': 'second_camera_name', 'default': 'camera2', 'description': 'name of the second camera'}, + {'name': 'threshold', 'default': 1.0, 'description': 'distance threshold for proximity warning in meters'}, +] + +def declare_configurable_parameters(parameters): + return [DeclareLaunchArgument(param['name'], default_value=param['default'], description=param['description']) for param in parameters] + +def set_configurable_parameters(parameters): + return dict([(param['name'], LaunchConfiguration(param['name'])) for param in parameters]) + +def generate_launch_description(): + return LaunchDescription( + declare_configurable_parameters(configurable_parameters) + [ + Node(package='proximity_monitor', + executable='proximity_monitor', + name='proximity_monitor', + parameters=[set_configurable_parameters(configurable_parameters)], + ), + ]) \ No newline at end of file diff --git a/docker/proximity_monitor/msg/ProximityWarning.msg b/docker/proximity_monitor/msg/ProximityWarning.msg new file mode 100644 index 0000000..d4dcf90 --- /dev/null +++ b/docker/proximity_monitor/msg/ProximityWarning.msg @@ -0,0 +1,12 @@ +# ProximityWarning.msg + +# Flag indicating if an object is closer than the threshold +bool warning + +# Proximity (0 means the object is right next to the camera, 1 means the object is at the threshold) +float32 proximity + +# Camera side (left or right) +bool side +bool left_side=false +bool right_side=true \ No newline at end of file diff --git a/docker/proximity_monitor/package.xml b/docker/proximity_monitor/package.xml new file mode 100644 index 0000000..f1bbb88 --- /dev/null +++ b/docker/proximity_monitor/package.xml @@ -0,0 +1,31 @@ + + + + proximity_monitor + 0.0.0 + TODO: Package description + ros + TODO: License declaration + + rclpy + sensor_msgs + std_msgs + + ament_cmake + rosidl_default_generators + rosidl_default_runtime + + realsense2_camera + realsense2_description + + cv_bridge + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/docker/proximity_monitor/proximity_monitor/__init__.py b/docker/proximity_monitor/proximity_monitor/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/docker/proximity_monitor/proximity_monitor/__pycache__/__init__.cpython-310.pyc b/docker/proximity_monitor/proximity_monitor/__pycache__/__init__.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..6a1eb176dbad0df6c38515e633e8242b16447af6 GIT binary patch literal 180 zcmd1j<>g`kfUu+Z)7uHWI&CE&BFDS~d$jr?wsf^Ff&&w>yFTx@cAD;=7ijUVT YsJz8tlbfGXnv-e=vapy5NU$&f0HQQ6jsO4v literal 0 HcmV?d00001 diff --git a/docker/proximity_monitor/proximity_monitor/__pycache__/proximity_monitor.cpython-310.pyc b/docker/proximity_monitor/proximity_monitor/__pycache__/proximity_monitor.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..1fec5492b4a4075c411a676af7bcde98b1e17eaf GIT binary patch literal 3614 zcma)9&2JmW6`$EJE-6x?C|R|W*e2SzEt|qDoT48=P{)pypsmfQR??tEi|vZDqLy0j zGP6smBB&yO4CLe<3#35+>F7TtKu2UBM{Ji1 zE{rE`*A%9(p0KVZY~kQ(=lAJ4`W~!X5=){AzWWigShu{*S~a!4C6hmd!!SE~l*D0{ zNV-XnFMO702ZO$z{uNWkPS`2FdB!=LvQtAC!aRc~G>vZ;v?+`;erlbufjwoOgI#D> za7y5~0=$9q39SC9iFFwgm23&TvT)CcQw67%Eeq^uxeCb*RyWu3aL!=k%mDWrSYO5JS}sQx=&04v%mm&rw}D#f!G+TIVmy?SFVu_Y za)-W*!?^EddooD(l1TLb@-h8ByF0nkmdQ?X)Ey8UOS|K@MO@-Jn0`!u0IUZETHc+cN5vn_JU@vNi&I??c$3c-)+61>~uT|q;fqk zgbzJWt-^Wg zrQTeTwYeS7nUwZv5bZwWs_c0%cbaR{|D8lal;oKe_N#G>Af|0&}smiNnl{d@=ubU0yhI!R+42Rc^x;eSNH4E&8+`c^v zlAeLLXFqZlCHR{d>C2dqC~S%pKQ^XFoB`iwvMP8CpgA_D=G53VkUVCm4^jO7dAA@K z1E6ih0b&nM86{zyc>-bV$Gt#qK(b}YZ{wjX;b*=opF65lcu=;moloB*!8M4$l4N(C zlJwpkY@~!q72YshKKbsY7ro@g3wNgb&+kmS&F=hLf(njd2)50h@lMN>ZS2dNG&M1` z41EjvePUcxSiT6}PcTwCe}hk6zErvALj3~b(u?E}?F<2e5ce1g4Y+JTNdu^73R+vA zuup-qoW(#?&9@1435y4ObTe~=d7|Y;x8`)IZ%(=ALZW zj@O!#D+}DtJF^XCcKl9jNvC;knCs9~-k_<8N!cRU8TXaFOSMn?J~!7j+AI}`SQt_* z&2VCv_MbQCH}S&j7-<8O!yV%)Up6S{E$;lcZhwt|pUdb?@=LN1N|%KK35G z^WOc3o663CnqpU;Tc2)+h}CK4Xy8hft)I_Z_7)^N80j)5mv3+y%iQ>|^0hfNt0(6; zc)_wdDRG##YzAU-Geam&b&K-UI5sk40QA6Y2@`8Gvoqt=0iwW$`{p%v#!vXMHANe- z&fbSTEtZvze#+Ug&Dmrb9&q7-^3DA(dcD~*E_hQjCiDM zpL#b{(jon$AIewpsxpUREK8uY3zUu0m&lfr+rJFt_jFxXOS;HkT;%6fal#xW6%U?w z(Vops+%lQ1Q_5h%w5$r>dU!!Pt-T z9bIB_%p*9Hl!B6?MUpP6R@A#*5JjH1pze1_gkW2`nAS1!&_b1Rue*+GxsAH3_r3yn z6ty~PegQ(q3^MU4yqp;t{P?K}&s*5B2hO0RK`!UHIpk#&7aGn$RnDkUCs;Fef9}hE zI=L~oQ?n?DP3m@=C{HpAL~BXevKNhxsIdq$t!D1iGW7*fa1cbQM7;;%N3A@}iEXeY z+Gk}y*u4MV+Zq7T7@+-AZn`(lL~L3a@h6rDNbgE#b)@VA)!izanl$>wa4wNEJjiz(7NY|3r4F7r9N)N>-*ePk316 zEUGkWg&NgFo^uHvm8k>2cc99>120btNg1xgUD@$?h;HOQG2Wnwe6BT=4~aUji<;6` jb6?K`?q`&B=}VYg>R|MrYt{ikj!}0jzjWOSx4!%z`qbVP literal 0 HcmV?d00001 diff --git a/docker/proximity_monitor/proximity_monitor/proximity_monitor.py b/docker/proximity_monitor/proximity_monitor/proximity_monitor.py new file mode 100644 index 0000000..8427937 --- /dev/null +++ b/docker/proximity_monitor/proximity_monitor/proximity_monitor.py @@ -0,0 +1,104 @@ +import rclpy +from rclpy.node import Node +from rcl_interfaces.msg import SetParametersResult +from std_msgs.msg import Float32MultiArray +from sensor_msgs.msg import Image +# from proximity_monitor.msg import ProximityWarning + +import cv2 as cv +from cv_bridge import CvBridge, CvBridgeError +import numpy as np + +bridge = CvBridge() + +# ProximityWarning message +# Float32MultiArray: [warning, proximity, threshold] +# warning: 1 if the distance is less than the threshold +# proximity: distance to the nearest point (meters) +# threshold: distance threshold (meters) + +class ProximityMonitorNode(Node): + def __init__(self): + super().__init__('proximity_monitor') + + # Declare parameters and initialize them + self.declare_parameter('camera_name', 'camera') + self.declare_parameter('proximity_warning_threshold', 0.2) + + # Get parameters + self.camera_name = self.get_parameter('camera_name').get_parameter_value().string_value + self.threshold = self.get_parameter('proximity_warning_threshold').get_parameter_value().double_value + + # Publishers and Subscribers + self.warning_topic = f'/roboy/pinky/sensing/{self.camera_name}/proximity_warning' + + self.warning_publisher = self.create_publisher(Float32MultiArray, self.warning_topic, 10) + # self.second_warning_publisher = self.create_publisher(Float32MultiArray, self.second_warning_topic, 10) + + self.camera_subscriber = self.create_subscription(Image, f'/{self.camera_name}/depth/image_rect_raw', self.camera_callback, 10) + + self.get_logger().info(f'Listening for the camera on /{self.camera_name}/depth/image_rect_raw') + + self.last_warning = None + self.last_proximity = None + + self.timer = self.create_timer(0.025, self.publish_warning) # 40 Hz + + # Dynamic reconfiguration callback + self.add_on_set_parameters_callback(self.parameters_callback) + + def process_camera_data(self, data): + proximity = self.nearest_point_distance(data) + # relative_proximity = proximity / self.threshold + warning = 1.0 if proximity < self.threshold else 0.0 + return warning, proximity + + def camera_callback(self, data): + self.last_warning, self.last_proximity = self.process_camera_data(data) + # self.get_logger().debug(f'Proximity: {self.last_proximity}, Warning: {self.last_warning}') + + def publish_warning(self): + warning_msg = Float32MultiArray() + + if self.last_warning: + self.get_logger().info(f'Publishing warning: {self.last_proximity}') + else: + self.get_logger().debug(f'Publishing NO warning') + + warning_msg.data = [ + self.last_warning, + self.last_proximity or float('NaN'), + self.threshold + ] + self.warning_publisher.publish(warning_msg) + + # self.last_warning = None + # self.last_proximity = None + + def parameters_callback(self, params): + for param in params: + if param.name == 'proximity_warning_threshold' and param.type_ == param.PARAMETER_DOUBLE: + self.threshold = param.value + self.get_logger().info(f'New proximity_warning_threshold set: {self.threshold}') + return SetParametersResult(successful=True) + return SetParametersResult(successful=False) + + def nearest_point_distance(self, ros_image): + try: + depth_image = bridge.imgmsg_to_cv2(ros_image, desired_encoding='passthrough') + depth_array = np.array(depth_image, dtype=np.float64) + min_distance = float(np.min(depth_array[depth_array!=0])) + return min_distance * 0.001 # Convert to meters + except CvBridgeError as e: + self.get_logger().error(e) + +def main(args=None): + rclpy.init(args=args) + proximity_monitor_node = ProximityMonitorNode() + proximity_monitor_node.get_logger().set_level(rclpy.logging.LoggingSeverity.DEBUG) + proximity_monitor_node.get_logger().info('Proximity Monitor Node started') + rclpy.spin(proximity_monitor_node) + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/docker/proximity_monitor/resource/proximity_monitor b/docker/proximity_monitor/resource/proximity_monitor new file mode 100644 index 0000000..e69de29 diff --git a/docker/proximity_monitor/setup.cfg b/docker/proximity_monitor/setup.cfg new file mode 100644 index 0000000..48ed68f --- /dev/null +++ b/docker/proximity_monitor/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/proximity_monitor +[install] +install_scripts=$base/lib/proximity_monitor diff --git a/docker/proximity_monitor/setup.py b/docker/proximity_monitor/setup.py new file mode 100644 index 0000000..7098b48 --- /dev/null +++ b/docker/proximity_monitor/setup.py @@ -0,0 +1,26 @@ +from setuptools import find_packages, setup + +package_name = 'proximity_monitor' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='ros', + maintainer_email='ros@todo.todo', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'proximity_monitor = proximity_monitor.proximity_monitor:main' + ], + }, +) diff --git a/docker/proximity_monitor/test/test_copyright.py b/docker/proximity_monitor/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/docker/proximity_monitor/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/docker/proximity_monitor/test/test_flake8.py b/docker/proximity_monitor/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/docker/proximity_monitor/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/docker/proximity_monitor/test/test_pep257.py b/docker/proximity_monitor/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/docker/proximity_monitor/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' From a6fec5ff04da4a457a358a81987c5d4e3cb54788 Mon Sep 17 00:00:00 2001 From: Arkadiy Telegin Date: Thu, 4 Jan 2024 18:08:06 +0100 Subject: [PATCH 10/25] Set initial warning to 0 --- docker/proximity_monitor/msg/ProximityWarning.msg | 12 ------------ .../proximity_monitor/proximity_monitor.py | 5 +---- 2 files changed, 1 insertion(+), 16 deletions(-) delete mode 100644 docker/proximity_monitor/msg/ProximityWarning.msg diff --git a/docker/proximity_monitor/msg/ProximityWarning.msg b/docker/proximity_monitor/msg/ProximityWarning.msg deleted file mode 100644 index d4dcf90..0000000 --- a/docker/proximity_monitor/msg/ProximityWarning.msg +++ /dev/null @@ -1,12 +0,0 @@ -# ProximityWarning.msg - -# Flag indicating if an object is closer than the threshold -bool warning - -# Proximity (0 means the object is right next to the camera, 1 means the object is at the threshold) -float32 proximity - -# Camera side (left or right) -bool side -bool left_side=false -bool right_side=true \ No newline at end of file diff --git a/docker/proximity_monitor/proximity_monitor/proximity_monitor.py b/docker/proximity_monitor/proximity_monitor/proximity_monitor.py index 8427937..873f37b 100644 --- a/docker/proximity_monitor/proximity_monitor/proximity_monitor.py +++ b/docker/proximity_monitor/proximity_monitor/proximity_monitor.py @@ -39,7 +39,7 @@ def __init__(self): self.get_logger().info(f'Listening for the camera on /{self.camera_name}/depth/image_rect_raw') - self.last_warning = None + self.last_warning = 0.0 self.last_proximity = None self.timer = self.create_timer(0.025, self.publish_warning) # 40 Hz @@ -72,9 +72,6 @@ def publish_warning(self): ] self.warning_publisher.publish(warning_msg) - # self.last_warning = None - # self.last_proximity = None - def parameters_callback(self, params): for param in params: if param.name == 'proximity_warning_threshold' and param.type_ == param.PARAMETER_DOUBLE: From cceb267e6c75216a87c8ec532eff8c2da62e6add Mon Sep 17 00:00:00 2001 From: Arkadiy Telegin Date: Fri, 19 Jan 2024 01:19:36 +0100 Subject: [PATCH 11/25] Version 1 --- docker/multi_camera_launch.py | 36 ++++++++++--------- .../proximity_monitor/proximity_monitor.py | 12 +++++-- 2 files changed, 30 insertions(+), 18 deletions(-) diff --git a/docker/multi_camera_launch.py b/docker/multi_camera_launch.py index 4afde9b..d883140 100644 --- a/docker/multi_camera_launch.py +++ b/docker/multi_camera_launch.py @@ -59,16 +59,20 @@ def generate_launch_description(): IncludeLaunchDescription( PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/rs_launch.py']), launch_arguments={ - 'camera_name': 'camera_left', + 'camera_name': 'camera_back_top', "serial_no":"_829212071824", - 'depth_module.profile': '424,240,5', - 'rgb_camera.profile': '424,240,5', + 'depth_module.profile': '424x240x6', + 'rgb_camera.profile': '320x180x6', }.items(), ), - # IncludeLaunchDescription( - # PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/rs_launch.py']), - # launch_arguments={'camera_name': 'camera_back', "serial_no":"_829212071844", "pointcloud.enable":"false"}.items(), - # ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/rs_launch.py']), + launch_arguments={ + 'camera_name': 'camera_back_bottom', + "serial_no":"_829212071844", + 'depth_module.profile': '424x240x6 ', + 'rgb_camera.profile': '320x180x6',}.items(), + ), # launch_ros.actions.Node( # package = "depth_handler", # executable = "depth_subscriber" @@ -82,15 +86,15 @@ def generate_launch_description(): launch_ros.actions.Node( package = "proximity_monitor", executable = "proximity_monitor", - name = 'proximity_monitor_left', - parameters = [{'camera_name': 'camera_left'}, + name = 'proximity_monitor_top', + parameters = [{'camera_name': 'camera_back_top'}, + {'threshold': 0.2}] + ), + launch_ros.actions.Node ( + package = "proximity_monitor", + executable = "proximity_monitor", + name = 'proximity_monitor_bottom', + parameters = [{'camera_name': 'camera_back_bottom'}, {'threshold': 0.2}] ), - # launch_ros.actions.Node ( - # package = "proximity_monitor", - # executable = "proximity_monitor", - # name = 'proximity_monitor_back', - # parameters = [{'camera_name': 'camera_back'}, - # {'threshold': 0.2}] - # ), ]) \ No newline at end of file diff --git a/docker/proximity_monitor/proximity_monitor/proximity_monitor.py b/docker/proximity_monitor/proximity_monitor/proximity_monitor.py index 873f37b..1c15463 100644 --- a/docker/proximity_monitor/proximity_monitor/proximity_monitor.py +++ b/docker/proximity_monitor/proximity_monitor/proximity_monitor.py @@ -2,10 +2,10 @@ from rclpy.node import Node from rcl_interfaces.msg import SetParametersResult from std_msgs.msg import Float32MultiArray -from sensor_msgs.msg import Image +from sensor_msgs.msg import Image, CompressedImage # from proximity_monitor.msg import ProximityWarning -import cv2 as cv +import cv2 from cv_bridge import CvBridge, CvBridgeError import numpy as np @@ -56,6 +56,14 @@ def process_camera_data(self, data): def camera_callback(self, data): self.last_warning, self.last_proximity = self.process_camera_data(data) # self.get_logger().debug(f'Proximity: {self.last_proximity}, Warning: {self.last_warning}') + # cv_img = bridge.imgmsg_to_cv2(data, desired_encoding='passthrough') + # compressed_img = cv2.imencode('.jpg', cv_img)[1].tostring() + # compressed_msg = CompressedImage() + # compressed_msg.header = data.header + # compressed_msg.format = "jpeg" + # compressed_msg.data = compressed_img + # self.get_logger().info(f'Publishing image') + # self.image_publisher.publish(compressed_msg) def publish_warning(self): warning_msg = Float32MultiArray() From 33badbf9b4f698d3b53dde258e5dadcb2284fa86 Mon Sep 17 00:00:00 2001 From: missxa Date: Thu, 7 Mar 2024 13:15:45 +0100 Subject: [PATCH 12/25] Log throttling --- docker/Dockerfile | 7 ++- docker/multi_camera_launch.py | 6 +-- .../proximity_monitor/proximity_monitor.py | 50 +++++++++++++------ 3 files changed, 43 insertions(+), 20 deletions(-) diff --git a/docker/Dockerfile b/docker/Dockerfile index ddce7a8..589f272 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -81,6 +81,11 @@ RUN apt-get update -y \ && apt-get install -y \ ros-${ROS_DISTRO}-rviz2 \ && mkdir src + +RUN sudo apt-get install ros-humble-rmw-cyclonedds-cpp -y +ENV RMW_IMPLEMENTATION=rmw_cyclonedds_cpp +RUN sudo apt-get install ros-humble-image-transport-plugins -y + COPY docker/realsense-ros ./src COPY docker/multi_camera_launch.py /ros2_ws/src/realsense2_camera/launch COPY docker/depth_handler /ros2_ws/src/depth_handler @@ -98,4 +103,4 @@ RUN echo "source /ros2_ws/install/setup.bash" >> ~/.bashrc COPY docker/launch.sh . RUN chmod +x /ros2_ws/launch.sh ENV DEBIAN_FRONTEND=dialog -ENTRYPOINT ["/ros2_ws/launch.sh"] \ No newline at end of file +ENTRYPOINT ["/ros2_ws/launch.sh"] diff --git a/docker/multi_camera_launch.py b/docker/multi_camera_launch.py index d883140..b03005a 100644 --- a/docker/multi_camera_launch.py +++ b/docker/multi_camera_launch.py @@ -62,7 +62,7 @@ def generate_launch_description(): 'camera_name': 'camera_back_top', "serial_no":"_829212071824", 'depth_module.profile': '424x240x6', - 'rgb_camera.profile': '320x180x6', + 'rgb_camera.profile': '320x240x6', }.items(), ), IncludeLaunchDescription( @@ -71,7 +71,7 @@ def generate_launch_description(): 'camera_name': 'camera_back_bottom', "serial_no":"_829212071844", 'depth_module.profile': '424x240x6 ', - 'rgb_camera.profile': '320x180x6',}.items(), + 'rgb_camera.profile': '320x240x6',}.items(), ), # launch_ros.actions.Node( # package = "depth_handler", @@ -97,4 +97,4 @@ def generate_launch_description(): parameters = [{'camera_name': 'camera_back_bottom'}, {'threshold': 0.2}] ), - ]) \ No newline at end of file + ]) diff --git a/docker/proximity_monitor/proximity_monitor/proximity_monitor.py b/docker/proximity_monitor/proximity_monitor/proximity_monitor.py index 1c15463..81b74b9 100644 --- a/docker/proximity_monitor/proximity_monitor/proximity_monitor.py +++ b/docker/proximity_monitor/proximity_monitor/proximity_monitor.py @@ -1,14 +1,18 @@ import rclpy from rclpy.node import Node from rcl_interfaces.msg import SetParametersResult -from std_msgs.msg import Float32MultiArray +from std_msgs.msg import Float32MultiArray, Bool from sensor_msgs.msg import Image, CompressedImage -# from proximity_monitor.msg import ProximityWarning + +from rcl_interfaces.srv import SetParameters +from rclpy.parameter import Parameter import cv2 from cv_bridge import CvBridge, CvBridgeError import numpy as np +from enum import Enum + bridge = CvBridge() # ProximityWarning message @@ -17,6 +21,14 @@ # proximity: distance to the nearest point (meters) # threshold: distance threshold (meters) +class BackCameraMode(Enum): + HI_RES = True + LOW_RES = False + +HI_RES_PROFILE = '424x240x6' +LOW_RES_PROFILE = '320x240x6' + + class ProximityMonitorNode(Node): def __init__(self): super().__init__('proximity_monitor') @@ -31,11 +43,16 @@ def __init__(self): # Publishers and Subscribers self.warning_topic = f'/roboy/pinky/sensing/{self.camera_name}/proximity_warning' - self.warning_publisher = self.create_publisher(Float32MultiArray, self.warning_topic, 10) - # self.second_warning_publisher = self.create_publisher(Float32MultiArray, self.second_warning_topic, 10) self.camera_subscriber = self.create_subscription(Image, f'/{self.camera_name}/depth/image_rect_raw', self.camera_callback, 10) + self.parameter_client = self.create_client(SetParameters, + f'/{self.camera_name}/set_parameters') + + # Disable mode change for now. + # self.camera_mode_subscriber = self.create_subscription(Bool, + # f'/roboy/pinky/sensing/{self.camera_name}/mode', + # self.change_camera_mode, 10) self.get_logger().info(f'Listening for the camera on /{self.camera_name}/depth/image_rect_raw') @@ -55,23 +72,14 @@ def process_camera_data(self, data): def camera_callback(self, data): self.last_warning, self.last_proximity = self.process_camera_data(data) - # self.get_logger().debug(f'Proximity: {self.last_proximity}, Warning: {self.last_warning}') - # cv_img = bridge.imgmsg_to_cv2(data, desired_encoding='passthrough') - # compressed_img = cv2.imencode('.jpg', cv_img)[1].tostring() - # compressed_msg = CompressedImage() - # compressed_msg.header = data.header - # compressed_msg.format = "jpeg" - # compressed_msg.data = compressed_img - # self.get_logger().info(f'Publishing image') - # self.image_publisher.publish(compressed_msg) def publish_warning(self): warning_msg = Float32MultiArray() if self.last_warning: - self.get_logger().info(f'Publishing warning: {self.last_proximity}') + self.get_logger().info(f'Publishing warning: {self.last_proximity}', throttle_duration_sec=1.0) else: - self.get_logger().debug(f'Publishing NO warning') + self.get_logger().debug(f'Publishing NO warning', throttle_duration_sec=1.0) warning_msg.data = [ self.last_warning, @@ -87,7 +95,7 @@ def parameters_callback(self, params): self.get_logger().info(f'New proximity_warning_threshold set: {self.threshold}') return SetParametersResult(successful=True) return SetParametersResult(successful=False) - + def nearest_point_distance(self, ros_image): try: depth_image = bridge.imgmsg_to_cv2(ros_image, desired_encoding='passthrough') @@ -97,6 +105,16 @@ def nearest_point_distance(self, ros_image): except CvBridgeError as e: self.get_logger().error(e) + def change_camera_mode(self, mode_bool: Bool): + mode = BackCameraMode(mode_bool.data) + self.get_logger().info(f'Changing {self.camera_name} mode to {mode.name}') + request = SetParameters.Request() + request.parameters = [ + Parameter(name='rgb_camera.profile', value=LOW_RES_PROFILE if mode == BackCameraMode.LOW_RES else HI_RES_PROFILE).to_parameter_msg(), + ] + self.parameter_client.call(request) + + def main(args=None): rclpy.init(args=args) proximity_monitor_node = ProximityMonitorNode() From 6c59182c9955a203b678601b46441a001fb964af Mon Sep 17 00:00:00 2001 From: Arkadiy Telegin Date: Thu, 28 Mar 2024 01:18:31 +0100 Subject: [PATCH 13/25] Remove unused code + --- .../proximity_monitor/proximity_monitor.py | 23 ++++++------------- 1 file changed, 7 insertions(+), 16 deletions(-) diff --git a/docker/proximity_monitor/proximity_monitor/proximity_monitor.py b/docker/proximity_monitor/proximity_monitor/proximity_monitor.py index 1c15463..0b71888 100644 --- a/docker/proximity_monitor/proximity_monitor/proximity_monitor.py +++ b/docker/proximity_monitor/proximity_monitor/proximity_monitor.py @@ -2,8 +2,9 @@ from rclpy.node import Node from rcl_interfaces.msg import SetParametersResult from std_msgs.msg import Float32MultiArray -from sensor_msgs.msg import Image, CompressedImage -# from proximity_monitor.msg import ProximityWarning +from sensor_msgs.msg import Image + +from rcl_interfaces.srv import SetParameters import cv2 from cv_bridge import CvBridge, CvBridgeError @@ -31,11 +32,11 @@ def __init__(self): # Publishers and Subscribers self.warning_topic = f'/roboy/pinky/sensing/{self.camera_name}/proximity_warning' - self.warning_publisher = self.create_publisher(Float32MultiArray, self.warning_topic, 10) - # self.second_warning_publisher = self.create_publisher(Float32MultiArray, self.second_warning_topic, 10) self.camera_subscriber = self.create_subscription(Image, f'/{self.camera_name}/depth/image_rect_raw', self.camera_callback, 10) + self.parameter_client = self.create_client(SetParameters, + f'/{self.camera_name}/set_parameters') self.get_logger().info(f'Listening for the camera on /{self.camera_name}/depth/image_rect_raw') @@ -49,29 +50,19 @@ def __init__(self): def process_camera_data(self, data): proximity = self.nearest_point_distance(data) - # relative_proximity = proximity / self.threshold warning = 1.0 if proximity < self.threshold else 0.0 return warning, proximity def camera_callback(self, data): self.last_warning, self.last_proximity = self.process_camera_data(data) - # self.get_logger().debug(f'Proximity: {self.last_proximity}, Warning: {self.last_warning}') - # cv_img = bridge.imgmsg_to_cv2(data, desired_encoding='passthrough') - # compressed_img = cv2.imencode('.jpg', cv_img)[1].tostring() - # compressed_msg = CompressedImage() - # compressed_msg.header = data.header - # compressed_msg.format = "jpeg" - # compressed_msg.data = compressed_img - # self.get_logger().info(f'Publishing image') - # self.image_publisher.publish(compressed_msg) def publish_warning(self): warning_msg = Float32MultiArray() if self.last_warning: - self.get_logger().info(f'Publishing warning: {self.last_proximity}') + self.get_logger().info(f'Publishing warning: {self.last_proximity}', throttle_duration_sec=1.0) else: - self.get_logger().debug(f'Publishing NO warning') + self.get_logger().debug(f'Publishing NO warning', throttle_duration_sec=1.0) warning_msg.data = [ self.last_warning, From eae0a94b9bc5b2ae93a30db6cf1b2600e096f034 Mon Sep 17 00:00:00 2001 From: Arkadiy Telegin Date: Thu, 28 Mar 2024 01:19:02 +0100 Subject: [PATCH 14/25] Add cyclone DDS to dockerfile --- docker/Dockerfile | 3 +++ 1 file changed, 3 insertions(+) diff --git a/docker/Dockerfile b/docker/Dockerfile index ddce7a8..f9e88d9 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -81,6 +81,9 @@ RUN apt-get update -y \ && apt-get install -y \ ros-${ROS_DISTRO}-rviz2 \ && mkdir src +RUN sudo apt-get install ros-humble-rmw-cyclonedds-cpp -y +ENV RMW_IMPLEMENTATION=rmw_cyclonedds_cpp +RUN sudo apt-get install ros-humble-image-transport-plugins -y COPY docker/realsense-ros ./src COPY docker/multi_camera_launch.py /ros2_ws/src/realsense2_camera/launch COPY docker/depth_handler /ros2_ws/src/depth_handler From 0a4c3a5ca988e0c5f8cf06f24288519e54d5264b Mon Sep 17 00:00:00 2001 From: Arkadiy Telegin Date: Thu, 28 Mar 2024 01:24:50 +0100 Subject: [PATCH 15/25] Cleanup --- .gitignore | 1 + .../__pycache__/__init__.cpython-310.pyc | Bin 180 -> 0 bytes .../proximity_monitor.cpython-310.pyc | Bin 3614 -> 0 bytes 3 files changed, 1 insertion(+) create mode 100644 .gitignore delete mode 100644 docker/proximity_monitor/proximity_monitor/__pycache__/__init__.cpython-310.pyc delete mode 100644 docker/proximity_monitor/proximity_monitor/__pycache__/proximity_monitor.cpython-310.pyc diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..c1d3e32 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +*/__pycache__ \ No newline at end of file diff --git a/docker/proximity_monitor/proximity_monitor/__pycache__/__init__.cpython-310.pyc b/docker/proximity_monitor/proximity_monitor/__pycache__/__init__.cpython-310.pyc deleted file mode 100644 index 6a1eb176dbad0df6c38515e633e8242b16447af6..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 180 zcmd1j<>g`kfUu+Z)7uHWI&CE&BFDS~d$jr?wsf^Ff&&w>yFTx@cAD;=7ijUVT YsJz8tlbfGXnv-e=vapy5NU$&f0HQQ6jsO4v diff --git a/docker/proximity_monitor/proximity_monitor/__pycache__/proximity_monitor.cpython-310.pyc b/docker/proximity_monitor/proximity_monitor/__pycache__/proximity_monitor.cpython-310.pyc deleted file mode 100644 index 1fec5492b4a4075c411a676af7bcde98b1e17eaf..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 3614 zcma)9&2JmW6`$EJE-6x?C|R|W*e2SzEt|qDoT48=P{)pypsmfQR??tEi|vZDqLy0j zGP6smBB&yO4CLe<3#35+>F7TtKu2UBM{Ji1 zE{rE`*A%9(p0KVZY~kQ(=lAJ4`W~!X5=){AzWWigShu{*S~a!4C6hmd!!SE~l*D0{ zNV-XnFMO702ZO$z{uNWkPS`2FdB!=LvQtAC!aRc~G>vZ;v?+`;erlbufjwoOgI#D> za7y5~0=$9q39SC9iFFwgm23&TvT)CcQw67%Eeq^uxeCb*RyWu3aL!=k%mDWrSYO5JS}sQx=&04v%mm&rw}D#f!G+TIVmy?SFVu_Y za)-W*!?^EddooD(l1TLb@-h8ByF0nkmdQ?X)Ey8UOS|K@MO@-Jn0`!u0IUZETHc+cN5vn_JU@vNi&I??c$3c-)+61>~uT|q;fqk zgbzJWt-^Wg zrQTeTwYeS7nUwZv5bZwWs_c0%cbaR{|D8lal;oKe_N#G>Af|0&}smiNnl{d@=ubU0yhI!R+42Rc^x;eSNH4E&8+`c^v zlAeLLXFqZlCHR{d>C2dqC~S%pKQ^XFoB`iwvMP8CpgA_D=G53VkUVCm4^jO7dAA@K z1E6ih0b&nM86{zyc>-bV$Gt#qK(b}YZ{wjX;b*=opF65lcu=;moloB*!8M4$l4N(C zlJwpkY@~!q72YshKKbsY7ro@g3wNgb&+kmS&F=hLf(njd2)50h@lMN>ZS2dNG&M1` z41EjvePUcxSiT6}PcTwCe}hk6zErvALj3~b(u?E}?F<2e5ce1g4Y+JTNdu^73R+vA zuup-qoW(#?&9@1435y4ObTe~=d7|Y;x8`)IZ%(=ALZW zj@O!#D+}DtJF^XCcKl9jNvC;knCs9~-k_<8N!cRU8TXaFOSMn?J~!7j+AI}`SQt_* z&2VCv_MbQCH}S&j7-<8O!yV%)Up6S{E$;lcZhwt|pUdb?@=LN1N|%KK35G z^WOc3o663CnqpU;Tc2)+h}CK4Xy8hft)I_Z_7)^N80j)5mv3+y%iQ>|^0hfNt0(6; zc)_wdDRG##YzAU-Geam&b&K-UI5sk40QA6Y2@`8Gvoqt=0iwW$`{p%v#!vXMHANe- z&fbSTEtZvze#+Ug&Dmrb9&q7-^3DA(dcD~*E_hQjCiDM zpL#b{(jon$AIewpsxpUREK8uY3zUu0m&lfr+rJFt_jFxXOS;HkT;%6fal#xW6%U?w z(Vops+%lQ1Q_5h%w5$r>dU!!Pt-T z9bIB_%p*9Hl!B6?MUpP6R@A#*5JjH1pze1_gkW2`nAS1!&_b1Rue*+GxsAH3_r3yn z6ty~PegQ(q3^MU4yqp;t{P?K}&s*5B2hO0RK`!UHIpk#&7aGn$RnDkUCs;Fef9}hE zI=L~oQ?n?DP3m@=C{HpAL~BXevKNhxsIdq$t!D1iGW7*fa1cbQM7;;%N3A@}iEXeY z+Gk}y*u4MV+Zq7T7@+-AZn`(lL~L3a@h6rDNbgE#b)@VA)!izanl$>wa4wNEJjiz(7NY|3r4F7r9N)N>-*ePk316 zEUGkWg&NgFo^uHvm8k>2cc99>120btNg1xgUD@$?h;HOQG2Wnwe6BT=4~aUji<;6` jb6?K`?q`&B=}VYg>R|MrYt{ikj!}0jzjWOSx4!%z`qbVP From 854f8078bae7cc945890de62b3161ef11ee15678 Mon Sep 17 00:00:00 2001 From: Arkadiy Telegin Date: Thu, 28 Mar 2024 01:29:21 +0100 Subject: [PATCH 16/25] Rename threshold parameter --- docker/multi_camera_launch.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/docker/multi_camera_launch.py b/docker/multi_camera_launch.py index b03005a..a93857e 100644 --- a/docker/multi_camera_launch.py +++ b/docker/multi_camera_launch.py @@ -88,13 +88,13 @@ def generate_launch_description(): executable = "proximity_monitor", name = 'proximity_monitor_top', parameters = [{'camera_name': 'camera_back_top'}, - {'threshold': 0.2}] + {'proximity_warning_threshold': 0.2}] ), launch_ros.actions.Node ( package = "proximity_monitor", executable = "proximity_monitor", name = 'proximity_monitor_bottom', parameters = [{'camera_name': 'camera_back_bottom'}, - {'threshold': 0.2}] + {'proximity_warning_threshold': 0.2}] ), ]) From 7f42fb6dcfa9ce4319583489558ab69d8820169d Mon Sep 17 00:00:00 2001 From: Arkadiy Telegin Date: Thu, 28 Mar 2024 16:37:59 +0100 Subject: [PATCH 17/25] Add .devcontainer config --- .devcontainer/Dockerfile | 129 +++++++++++++++++++++++++++ .devcontainer/devcontainer.json | 59 ++++++++++-- .devcontainer/repos_to_submodules.py | 38 ++++++++ 3 files changed, 217 insertions(+), 9 deletions(-) create mode 100644 .devcontainer/Dockerfile create mode 100755 .devcontainer/repos_to_submodules.py diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile new file mode 100644 index 0000000..ccbbe08 --- /dev/null +++ b/.devcontainer/Dockerfile @@ -0,0 +1,129 @@ +# FROM althack/ros2:humble-dev + +# # ** [Optional] Uncomment this section to install additional packages. ** +# # +# # ENV DEBIAN_FRONTEND=noninteractive +# # RUN apt-get update \ +# # && apt-get -y install --no-install-recommends \ +# # # +# # # Clean up +# # && apt-get autoremove -y \ +# # && apt-get clean -y \ +# # && rm -rf /var/lib/apt/lists/* +# # ENV DEBIAN_FRONTEND=dialog + +# # Set up auto-source of workspace for ros user +# ARG WORKSPACE +# RUN echo "if [ -f ${WORKSPACE}/install/setup.bash ]; then source ${WORKSPACE}/install/setup.bash; fi" >> /home/ros/.bashrc + +# ARG BASE_IMAGE=ros:humble-perception +ARG BASE_IMAGE=althack/ros2:humble-dev + +# The following steps are based on the offical multi-stage build: https://github.com/IntelRealSense/librealsense/blob/master/scripts/Docker/Dockerfile +################################# +# Librealsense Builder Stage # +################################# +FROM $BASE_IMAGE as librealsense-builder + +SHELL ["/bin/bash", "-c"] + +ENV DEBIAN_FRONTEND=noninteractive + +RUN apt-get update \ + && apt-get install -qq -y --no-install-recommends \ + build-essential \ + cmake \ + git \ + libssl-dev \ + libusb-1.0-0-dev \ + pkg-config \ + libgtk-3-dev \ + libglfw3-dev \ + libgl1-mesa-dev \ + libglu1-mesa-dev \ + curl \ + python3 \ + python3-dev \ + ca-certificates \ + && rm -rf /var/lib/apt/lists/* + +WORKDIR /usr/src +# Get the latest tag of remote repository: https://stackoverflow.com/a/12704727 +# Needs to be a single command as ENV can't be set from Bash command: https://stackoverflow.com/questions/34911622/dockerfile-set-env-to-result-of-command +RUN export LIBRS_VERSION=2.53.1; \ + curl https://codeload.github.com/IntelRealSense/librealsense/tar.gz/refs/tags/v${LIBRS_VERSION} -o librealsense.tar.gz; \ + tar -zxf librealsense.tar.gz; \ + rm librealsense.tar.gz; \ + ln -s /usr/src/librealsense-${LIBRS_VERSION} /usr/src/librealsense + +RUN cd /usr/src/librealsense \ + && mkdir build && cd build \ + && cmake \ + -DCMAKE_C_FLAGS_RELEASE="${CMAKE_C_FLAGS_RELEASE} -s" \ + -DCMAKE_CXX_FLAGS_RELEASE="${CMAKE_CXX_FLAGS_RELEASE} -s" \ + -DCMAKE_INSTALL_PREFIX=/opt/librealsense \ + -DBUILD_GRAPHICAL_EXAMPLES=OFF \ + -DBUILD_PYTHON_BINDINGS:bool=true \ + -DCMAKE_BUILD_TYPE=Release ../ \ + && make -j$(($(nproc)-1)) all \ + && make install + + ENV DEBIAN_FRONTEND=dialog + +###################################### +# librealsense Base Image Stage # +###################################### +FROM ${BASE_IMAGE} as librealsense + +SHELL ["/bin/bash", "-c"] + +COPY --from=librealsense-builder /opt/librealsense /usr/local/ +COPY --from=librealsense-builder /usr/lib/python3/dist-packages/pyrealsense2 /usr/lib/python3/dist-packages/pyrealsense2 +COPY --from=librealsense-builder /usr/src/librealsense/config/99-realsense-libusb.rules /etc/udev/rules.d/ +ENV PYTHONPATH=${PYTHONPATH}:/usr/local/lib + +ENV DEBIAN_FRONTEND=noninteractive + +RUN apt-get update \ + && apt-get install -y --no-install-recommends \ + libusb-1.0-0 \ + udev \ + apt-transport-https \ + ca-certificates \ + curl \ + software-properties-common \ + && rm -rf /var/lib/apt/lists/* +# The following steps are based on: https://github.com/IntelRealSense/realsense-ros/tree/ros2-development +# ENV WS_DIR="/ros2_ws" +# WORKDIR ${WS_DIR} +RUN apt-get update -y \ + && apt-get install -y \ + ros-${ROS_DISTRO}-rviz2 #\ +# && mkdir src +# COPY docker/realsense-ros ./src +# COPY docker/multi_camera_launch.py /ros2_ws/src/realsense2_camera/launch +# COPY docker/depth_handler /ros2_ws/src/depth_handler +# COPY docker/proximity_monitor /ros2_ws/src/proximity_monitor +RUN apt-get install -y python3-rosdep \ + && apt-get update \ + && source /opt/ros/${ROS_DISTRO}/setup.bash \ + && rm /etc/ros/rosdep/sources.list.d/20-default.list \ + && rosdep init \ + && rosdep update \ + && rosdep install -i --from-path src --rosdistro ${ROS_DISTRO} --skip-keys=librealsense2 -y \ + && colcon build +RUN sudo apt-get install python3-opencv -y + +RUN sudo apt-get install ros-humble-rmw-cyclonedds-cpp -y +ENV RMW_IMPLEMENTATION=rmw_cyclonedds_cpp +RUN sudo apt-get install ros-humble-image-transport-plugins -y +RUN sudo apt-get install ros-humble-rqt-image-view -y +# RUN echo "source /ros2_ws/install/setup.bash" >> ~/.bashrc +# COPY docker/launch.sh . +# RUN chmod +x /ros2_ws/launch.sh +ENV DEBIAN_FRONTEND=dialog +# ENTRYPOINT ["/ros2_ws/launch.sh"] + +# # Set up auto-source of workspace for ros user +ARG WORKSPACE +RUN echo "if [ -f ${WORKSPACE}/install/setup.bash ]; then source ${WORKSPACE}/install/setup.bash; fi" >> /home/ros/.bashrc \ No newline at end of file diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index f36b7b6..7f12ab0 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -1,11 +1,52 @@ +// See https://aka.ms/vscode-remote/devcontainer.json for format details. { - "name": "Docker for Intel Realsense with ROS 2", - "dockerComposeFile": [ - "../docker/docker-compose-gui.yml" // Alternatives: "../docker/docker-compose-gui.yml", "../docker/docker-compose-gui-nvidia.yml" - ], - "service": "realsense_ros2", - "workspaceFolder": "/ros2_ws", - "shutdownAction": "stopCompose", - "extensions": [ - ] + "dockerFile": "Dockerfile", + "build": { + "args": { + "WORKSPACE": "${containerWorkspaceFolder}/docker" + } + }, + "remoteUser": "ros", + "runArgs": [ + "--network=host", + "--cap-add=SYS_PTRACE", + "--security-opt=seccomp:unconfined", + "--security-opt=apparmor:unconfined", + "--volume=/tmp/.X11-unix:/tmp/.X11-unix", + "--volume=/mnt/wslg:/mnt/wslg", + "--ipc=host", + "--volume=/dev:/dev", + "--device-cgroup-rule=c 81:* rmw", + "--device-cgroup-rule=c 189:* rmw" + // uncomment to use intel iGPU + // "--device=/dev/dri" + ], + "containerEnv": { + "DISPLAY": "${localEnv:DISPLAY}", // Needed for GUI try ":0" for windows + "WAYLAND_DISPLAY": "${localEnv:WAYLAND_DISPLAY}", + "XDG_RUNTIME_DIR": "${localEnv:XDG_RUNTIME_DIR}", + "PULSE_SERVER": "${localEnv:PULSE_SERVER}", + "LIBGL_ALWAYS_SOFTWARE": "1", // Needed for software rendering of opengl + "RMW_IMPLEMENTATION": "rmw_cyclonedds_cpp" + }, + // Set *default* container specific settings.json values on container create. + "customizations": { + "vscode": { + "extensions": [ + "althack.ament-task-provider", + "betwo.b2-catkin-tools", + "DotJoshJohnson.xml", + "ms-azuretools.vscode-docker", + "ms-iot.vscode-ros", + "ms-python.python", + "ms-vscode.cpptools", + "redhat.vscode-yaml", + "smilerobotics.urdf", + "streetsidesoftware.code-spell-checker", + "twxs.cmake", + "yzhang.markdown-all-in-one", + "zachflower.uncrustify" + ] + } + } } diff --git a/.devcontainer/repos_to_submodules.py b/.devcontainer/repos_to_submodules.py new file mode 100755 index 0000000..70713f7 --- /dev/null +++ b/.devcontainer/repos_to_submodules.py @@ -0,0 +1,38 @@ +import glob +import os +import subprocess +import yaml + +prefix="src" + +def add_git_submodule(repo_name, repo_url, repo_version): + subprocess.call(['git', 'submodule', 'add', '-b', repo_version, repo_url, repo_name]) + +def is_submodule(repo_name): + try: + subprocess.check_output(['git', 'submodule', 'status', repo_name], stderr=subprocess.DEVNULL) + return True + except subprocess.CalledProcessError: + return False + +def parse_repos_file(file_path): + with open(file_path, 'r') as file: + repos_data = yaml.safe_load(file) + repositories = repos_data['repositories'] + + for repo_name, repo_info in repositories.items(): + if 'type' in repo_info and repo_info['type'] == 'git': + repo_url = repo_info['url'] + repo_version = repo_info['version'] + submodule_name = os.path.join(prefix, repo_name) + + if not is_submodule(submodule_name): + add_git_submodule(submodule_name, repo_url, repo_version) + print(f"Added {repo_name} as a submodule.") + +# Find .repos files within the src directory +repos_files = glob.glob('src/**/*.repos', recursive=True) + +# Process each .repos file +for repos_file in repos_files: + parse_repos_file(repos_file) From c0613c80d90a133775d1b3d0172a3233ca45db0a Mon Sep 17 00:00:00 2001 From: Arkadiy Telegin Date: Thu, 28 Mar 2024 16:49:05 +0100 Subject: [PATCH 18/25] Fix .devcontainer build target --- .devcontainer/Dockerfile | 129 --------------------------- .devcontainer/devcontainer.json | 4 +- .devcontainer/repos_to_submodules.py | 38 -------- 3 files changed, 2 insertions(+), 169 deletions(-) delete mode 100644 .devcontainer/Dockerfile delete mode 100755 .devcontainer/repos_to_submodules.py diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile deleted file mode 100644 index ccbbe08..0000000 --- a/.devcontainer/Dockerfile +++ /dev/null @@ -1,129 +0,0 @@ -# FROM althack/ros2:humble-dev - -# # ** [Optional] Uncomment this section to install additional packages. ** -# # -# # ENV DEBIAN_FRONTEND=noninteractive -# # RUN apt-get update \ -# # && apt-get -y install --no-install-recommends \ -# # # -# # # Clean up -# # && apt-get autoremove -y \ -# # && apt-get clean -y \ -# # && rm -rf /var/lib/apt/lists/* -# # ENV DEBIAN_FRONTEND=dialog - -# # Set up auto-source of workspace for ros user -# ARG WORKSPACE -# RUN echo "if [ -f ${WORKSPACE}/install/setup.bash ]; then source ${WORKSPACE}/install/setup.bash; fi" >> /home/ros/.bashrc - -# ARG BASE_IMAGE=ros:humble-perception -ARG BASE_IMAGE=althack/ros2:humble-dev - -# The following steps are based on the offical multi-stage build: https://github.com/IntelRealSense/librealsense/blob/master/scripts/Docker/Dockerfile -################################# -# Librealsense Builder Stage # -################################# -FROM $BASE_IMAGE as librealsense-builder - -SHELL ["/bin/bash", "-c"] - -ENV DEBIAN_FRONTEND=noninteractive - -RUN apt-get update \ - && apt-get install -qq -y --no-install-recommends \ - build-essential \ - cmake \ - git \ - libssl-dev \ - libusb-1.0-0-dev \ - pkg-config \ - libgtk-3-dev \ - libglfw3-dev \ - libgl1-mesa-dev \ - libglu1-mesa-dev \ - curl \ - python3 \ - python3-dev \ - ca-certificates \ - && rm -rf /var/lib/apt/lists/* - -WORKDIR /usr/src -# Get the latest tag of remote repository: https://stackoverflow.com/a/12704727 -# Needs to be a single command as ENV can't be set from Bash command: https://stackoverflow.com/questions/34911622/dockerfile-set-env-to-result-of-command -RUN export LIBRS_VERSION=2.53.1; \ - curl https://codeload.github.com/IntelRealSense/librealsense/tar.gz/refs/tags/v${LIBRS_VERSION} -o librealsense.tar.gz; \ - tar -zxf librealsense.tar.gz; \ - rm librealsense.tar.gz; \ - ln -s /usr/src/librealsense-${LIBRS_VERSION} /usr/src/librealsense - -RUN cd /usr/src/librealsense \ - && mkdir build && cd build \ - && cmake \ - -DCMAKE_C_FLAGS_RELEASE="${CMAKE_C_FLAGS_RELEASE} -s" \ - -DCMAKE_CXX_FLAGS_RELEASE="${CMAKE_CXX_FLAGS_RELEASE} -s" \ - -DCMAKE_INSTALL_PREFIX=/opt/librealsense \ - -DBUILD_GRAPHICAL_EXAMPLES=OFF \ - -DBUILD_PYTHON_BINDINGS:bool=true \ - -DCMAKE_BUILD_TYPE=Release ../ \ - && make -j$(($(nproc)-1)) all \ - && make install - - ENV DEBIAN_FRONTEND=dialog - -###################################### -# librealsense Base Image Stage # -###################################### -FROM ${BASE_IMAGE} as librealsense - -SHELL ["/bin/bash", "-c"] - -COPY --from=librealsense-builder /opt/librealsense /usr/local/ -COPY --from=librealsense-builder /usr/lib/python3/dist-packages/pyrealsense2 /usr/lib/python3/dist-packages/pyrealsense2 -COPY --from=librealsense-builder /usr/src/librealsense/config/99-realsense-libusb.rules /etc/udev/rules.d/ -ENV PYTHONPATH=${PYTHONPATH}:/usr/local/lib - -ENV DEBIAN_FRONTEND=noninteractive - -RUN apt-get update \ - && apt-get install -y --no-install-recommends \ - libusb-1.0-0 \ - udev \ - apt-transport-https \ - ca-certificates \ - curl \ - software-properties-common \ - && rm -rf /var/lib/apt/lists/* -# The following steps are based on: https://github.com/IntelRealSense/realsense-ros/tree/ros2-development -# ENV WS_DIR="/ros2_ws" -# WORKDIR ${WS_DIR} -RUN apt-get update -y \ - && apt-get install -y \ - ros-${ROS_DISTRO}-rviz2 #\ -# && mkdir src -# COPY docker/realsense-ros ./src -# COPY docker/multi_camera_launch.py /ros2_ws/src/realsense2_camera/launch -# COPY docker/depth_handler /ros2_ws/src/depth_handler -# COPY docker/proximity_monitor /ros2_ws/src/proximity_monitor -RUN apt-get install -y python3-rosdep \ - && apt-get update \ - && source /opt/ros/${ROS_DISTRO}/setup.bash \ - && rm /etc/ros/rosdep/sources.list.d/20-default.list \ - && rosdep init \ - && rosdep update \ - && rosdep install -i --from-path src --rosdistro ${ROS_DISTRO} --skip-keys=librealsense2 -y \ - && colcon build -RUN sudo apt-get install python3-opencv -y - -RUN sudo apt-get install ros-humble-rmw-cyclonedds-cpp -y -ENV RMW_IMPLEMENTATION=rmw_cyclonedds_cpp -RUN sudo apt-get install ros-humble-image-transport-plugins -y -RUN sudo apt-get install ros-humble-rqt-image-view -y -# RUN echo "source /ros2_ws/install/setup.bash" >> ~/.bashrc -# COPY docker/launch.sh . -# RUN chmod +x /ros2_ws/launch.sh -ENV DEBIAN_FRONTEND=dialog -# ENTRYPOINT ["/ros2_ws/launch.sh"] - -# # Set up auto-source of workspace for ros user -ARG WORKSPACE -RUN echo "if [ -f ${WORKSPACE}/install/setup.bash ]; then source ${WORKSPACE}/install/setup.bash; fi" >> /home/ros/.bashrc \ No newline at end of file diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index 7f12ab0..3539a1d 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -1,9 +1,9 @@ // See https://aka.ms/vscode-remote/devcontainer.json for format details. { - "dockerFile": "Dockerfile", + "dockerFile": "../docker/Dockerfile", "build": { "args": { - "WORKSPACE": "${containerWorkspaceFolder}/docker" + "WORKSPACE": "${containerWorkspaceFolder}" } }, "remoteUser": "ros", diff --git a/.devcontainer/repos_to_submodules.py b/.devcontainer/repos_to_submodules.py deleted file mode 100755 index 70713f7..0000000 --- a/.devcontainer/repos_to_submodules.py +++ /dev/null @@ -1,38 +0,0 @@ -import glob -import os -import subprocess -import yaml - -prefix="src" - -def add_git_submodule(repo_name, repo_url, repo_version): - subprocess.call(['git', 'submodule', 'add', '-b', repo_version, repo_url, repo_name]) - -def is_submodule(repo_name): - try: - subprocess.check_output(['git', 'submodule', 'status', repo_name], stderr=subprocess.DEVNULL) - return True - except subprocess.CalledProcessError: - return False - -def parse_repos_file(file_path): - with open(file_path, 'r') as file: - repos_data = yaml.safe_load(file) - repositories = repos_data['repositories'] - - for repo_name, repo_info in repositories.items(): - if 'type' in repo_info and repo_info['type'] == 'git': - repo_url = repo_info['url'] - repo_version = repo_info['version'] - submodule_name = os.path.join(prefix, repo_name) - - if not is_submodule(submodule_name): - add_git_submodule(submodule_name, repo_url, repo_version) - print(f"Added {repo_name} as a submodule.") - -# Find .repos files within the src directory -repos_files = glob.glob('src/**/*.repos', recursive=True) - -# Process each .repos file -for repos_file in repos_files: - parse_repos_file(repos_file) From 7d92739d42420fb05f34a112284e6e1ad2b8b834 Mon Sep 17 00:00:00 2001 From: Arkadiy Telegin Date: Thu, 28 Mar 2024 16:52:32 +0100 Subject: [PATCH 19/25] Add QoL ROS vscode scripts --- .gitignore | 6 +- .vscode/c_cpp_properties.json | 18 +++ .vscode/launch.json | 90 +++++++++++++ .vscode/settings.json | 74 ++++++++++ .vscode/tasks.json | 247 ++++++++++++++++++++++++++++++++++ build.sh | 10 ++ setup.sh | 7 + test.sh | 6 + 8 files changed, 457 insertions(+), 1 deletion(-) create mode 100644 .vscode/c_cpp_properties.json create mode 100644 .vscode/launch.json create mode 100644 .vscode/settings.json create mode 100644 .vscode/tasks.json create mode 100755 build.sh create mode 100755 setup.sh create mode 100755 test.sh diff --git a/.gitignore b/.gitignore index c1d3e32..d02872e 100644 --- a/.gitignore +++ b/.gitignore @@ -1 +1,5 @@ -*/__pycache__ \ No newline at end of file +*/__pycache__ +build/* +install/* +log/* +site/* \ No newline at end of file diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json new file mode 100644 index 0000000..40d2c8e --- /dev/null +++ b/.vscode/c_cpp_properties.json @@ -0,0 +1,18 @@ +{ + "configurations": [ + { + "name": "Linux", + "includePath": [ + "${workspaceFolder}/**", + "/opt/ros/humble/include/**" + ], + "defines": [], + "compilerPath": "/usr/bin/gcc", + "compileCommands": "${workspaceFolder}/build/compile_commands.json", + "cStandard": "c99", + "cppStandard": "c++17", + "intelliSenseMode": "linux-gcc-x64" + } + ], + "version": 4 +} diff --git a/.vscode/launch.json b/.vscode/launch.json new file mode 100644 index 0000000..6dc8dd6 --- /dev/null +++ b/.vscode/launch.json @@ -0,0 +1,90 @@ +{ + // Use IntelliSense to learn about possible attributes. + // Hover to view descriptions of existing attributes. + // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 + "version": "0.2.0", + "configurations": [ + // Example launch of a python file + { + "name": "Python: Current File", + "type": "python", + "request": "launch", + "program": "${file}", + "console": "integratedTerminal", + }, + // Example gdb launch of a ros executable + { + "name": "(gdb) Launch (merge-install)", + "type": "cppdbg", + "request": "launch", + "program": "${workspaceFolder}/install/lib/${input:package}/${input:program}", + "args": [], + "preLaunchTask": "build", + "stopAtEntry": true, + "cwd": "${workspaceFolder}", + "externalConsole": false, + "MIMode": "gdb", + "setupCommands": [ + { + "description": "Enable pretty-printing for gdb", + "text": "-enable-pretty-printing", + "ignoreFailures": true + } + ] + }, + { + "name": "(gdb) Launch (isolated-install)", + "type": "cppdbg", + "request": "launch", + "program": "${workspaceFolder}/install/${input:package}/lib/${input:package}/${input:program}", + "args": [], + "preLaunchTask": "build", + "stopAtEntry": true, + "cwd": "${workspaceFolder}", + "externalConsole": false, + "MIMode": "gdb", + "setupCommands": [ + { + "description": "Enable pretty-printing for gdb", + "text": "-enable-pretty-printing", + "ignoreFailures": true + } + ] + }, + //Example of a ROS Launch file + { + "name": "ROS: Launch File (merge-install)", + "type": "ros", + "request": "launch", + "preLaunchTask": "build", + "target": "${workspaceFolder}/install/share/${input:package}/launch/${input:ros_launch}", + }, + { + "name": "ROS: Launch File (isolated-install)", + "type": "ros", + "request": "launch", + "preLaunchTask": "build", + "target": "${workspaceFolder}/install/${input:package}/share/${input:package}/launch/${input:ros_launch}", + }, + ], + "inputs": [ + { + "id": "package", + "type": "promptString", + "description": "Package name", + "default": "examples_rclcpp_minimal_publisher" + }, + { + "id": "program", + "type": "promptString", + "description": "Program name", + "default": "publisher_member_function" + }, + { + "id": "ros_launch", + "type": "promptString", + "description": "ROS launch name", + "default": "file_name_launch.py" + } + ] +} diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..a132292 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,74 @@ +{ + "editor.tabSize": 8, + "editor.rulers": [ + 100 + ], + "files.associations": { + "*.repos": "yaml", + "*.world": "xml", + "*.xacro": "xml" + }, + "python.analysis.extraPaths": [ + "/opt/ros/humble/lib/python3.10/site-packages/" + ], + // Autocomplete from ros python packages + "python.autoComplete.extraPaths": [ + "/opt/ros/humble/lib/python3.10/site-packages/" + ], + // Environment file lets vscode find python files within workspace + "python.envFile": "${workspaceFolder}/.env", + // Use the system installed version of autopep8 + "python.formatting.autopep8Path": "/usr/bin/autopep8", + "python.formatting.autopep8Args": [ + "--max-line-length=100" + ], + "C_Cpp.default.intelliSenseMode": "linux-gcc-x86", + "C_Cpp.formatting": "disabled", + "uncrustify.configPath.linux": "/opt/ros/humble/lib/python3.10/site-packages/ament_uncrustify/configuration/ament_code_style.cfg", + "[cpp]": { + "editor.defaultFormatter": "zachflower.uncrustify" + }, + "search.exclude": { + "**/node_modules": true, + "**/bower_components": true, + "**/*.code-search": true, + "**/build": true, + "**/install": true, + "**/log": true + }, + "cSpell.words": [ + "RTPS", + "athackst", + "autopep", + "cmake", + "cppcheck", + "cpplint", + "DCMAKE", + "deque", + "devcontainer", + "ints", + "noqa", + "pytest", + "rclcpp", + "rclpy", + "repos", + "rosdep", + "rosdistro", + "rosidl", + "RTPS", + "uncrustify", + "Wextra", + "Wpedantic", + "xmllint" + ], + "cSpell.allowCompoundWords": true, + "cSpell.ignorePaths": [ + "**/package-lock.json", + "**/node_modules/**", + "**/vscode-extension/**", + "**/.git/objects/**", + ".vscode", + ".vscode-insiders", + ".devcontainer/devcontainer.json" + ] +} diff --git a/.vscode/tasks.json b/.vscode/tasks.json new file mode 100644 index 0000000..d1ed3ee --- /dev/null +++ b/.vscode/tasks.json @@ -0,0 +1,247 @@ +{ + // See https://go.microsoft.com/fwlink/?LinkId=733558 + // for the documentation about the tasks.json format + "version": "2.0.0", + "tasks": [ + // Build tasks + { + "label": "build", + "detail": "Build workspace (default)", + "type": "shell", + "command": "./build.sh", + "group": { + "kind": "build", + "isDefault": true + }, + "problemMatcher": "$gcc" + }, + { + "label": "debug", + "detail": "Build workspace (debug)", + "type": "shell", + "command": "./build.sh", + "options": { + "env": { + "BUILD_TYPE": "Debug" + } + }, + "group": "build", + "problemMatcher": "$gcc" + }, + // Test tasks + { + "label": "test", + "detail": "Run all unit tests and show results.", + "type": "shell", + "command": "./test.sh", + "group": { + "kind": "test", + "isDefault": true + } + }, + // Clean + { + "label": "clean", + "detail": "Run the clean target", + "type": "shell", + "command": "colcon build --cmake-target clean", + "problemMatcher": "$gcc" + }, + { + "label": "purge", + "detail": "Purge workspace by deleting all generated files.", + "type": "shell", + "command": "sudo rm -fr build install log; sudo py3clean .", + "problemMatcher": [] + }, + // Linting and static code analysis tasks + { + "label": "fix", + "detail": "Reformat files with uncrustify.", + "type": "shell", + "command": "ament_uncrustify --reformat src/", + "problemMatcher": [] + }, + { + "label": "uncrustify", + "detail": "Lint files with uncrustify.", + "type": "shell", + "command": "ament_uncrustify src/", + "presentation": { + "panel": "dedicated", + "reveal": "silent", + "clear": true + }, + "problemMatcher": [ + { + "owner": "uncrustify", + "source": "uncrustify", + "fileLocation": "relative", + "pattern": [ + // just the file name message + { + "regexp": "^(.*)'(.*)':", + "kind": "file", + "file": 2, + "message": 1 + } + ] + } + ] + }, + { + "label": "cpplint", + "detail": "Lint files with cpplint.", + "type": "ament", + "task": "cpplint", + "path": "src/", + "problemMatcher": "$ament_cpplint", + "presentation": { + "panel": "dedicated", + "reveal": "silent", + "clear": true + }, + }, + { + "label": "cppcheck", + "detail": "Run static code checker cppcheck.", + "type": "ament", + "task": "cppcheck", + "path": "src/", + "problemMatcher": "$ament_cppcheck", + "presentation": { + "panel": "dedicated", + "reveal": "silent", + "clear": true + }, + "options": { + "env": { + "AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS": "1" + } + } + }, + { + "label": "lint_cmake", + "detail": "Run lint on cmake files.", + "type": "ament", + "task": "lint_cmake", + "path": "src/", + "problemMatcher": "$ament_lint_cmake", + "presentation": { + "panel": "dedicated", + "reveal": "silent", + "clear": true + } + }, + { + "label": "flake8", + "detail": "Run flake8 on python files.", + "type": "ament", + "task": "flake8", + "path": "src/", + "problemMatcher": "$ament_flake8", + "presentation": { + "panel": "dedicated", + "reveal": "silent", + "clear": true + } + }, + { + "label": "pep257", + "detail": "Run pep257 on python files.", + "type": "ament", + "task": "pep257", + "path": "src/", + "problemMatcher": "$ament_pep257", + "presentation": { + "panel": "dedicated", + "reveal": "silent", + "clear": true + } + }, + { + "label": "xmllint", + "detail": "Run xmllint on xml files.", + "type": "ament", + "task": "xmllint", + "path": "src/", + "problemMatcher": "$ament_xmllint", + "presentation": { + "panel": "dedicated", + "reveal": "silent", + "clear": true + } + }, + { + "label": "lint all", + "detail": "Run all linters.", + "dependsOn": [ + "cppcheck", + "cpplint", + "flake8", + "lint_cmake", + "pep257", + "xmllint", + "uncrustify" + ], + "problemMatcher": [] + }, + // Workspace editing tasks + { + "label": "new ament_cmake package", + "detail": "Create a new ROS cpp package from a template.", + "type": "shell", + "command": "ros2 pkg create --destination-directory src --build-type ament_cmake ${input:package}", + "problemMatcher": [] + }, + { + "label": "new ament_python package", + "detail": "Create a new ROS python package from a template.", + "type": "shell", + "command": "ros2 pkg create --destination-directory src --build-type ament_python ${input:package}", + "problemMatcher": [] + }, + { + "label": "import from workspace file", + "detail": "Use vcs to import modules specified by a workspace/rosinstall file.", + "type": "shell", + "command": "vcs import < src/ros2.repos src", + "problemMatcher": [] + }, + { + "label": "update workspace file", + "detail": "Use vcs to update repositories in src to workspace file.", + "type": "shell", + "command": "vcs export src > src/ros2.repos", + "problemMatcher": [] + }, + { + "label": "install dependencies", + "detail": "Install all dependencies specified in the workspaces package.xml files.", + "type": "shell", + "command": "sudo apt-get update && rosdep update && rosdep install --from-paths src --ignore-src -y", + "problemMatcher": [] + }, + { + "label": "setup", + "detail": "Set up the workspace", + "type": "shell", + "command": "./setup.sh", + "problemMatcher": [] + }, + { + "label": "add submodules from .repos", + "detail": "Create a git submodule for all repositories in your .repos file", + "type": "shell", + "command": "python3 .devcontainer/repos_to_submodules.py", + "problemMatcher": [] + } + ], + "inputs": [ + { + "id": "package", + "type": "promptString", + "description": "Package name" + } + ] +} diff --git a/build.sh b/build.sh new file mode 100755 index 0000000..7bf6bd8 --- /dev/null +++ b/build.sh @@ -0,0 +1,10 @@ +#!/bin/bash +set -e + +# Set the default build type +BUILD_TYPE=RelWithDebInfo +colcon build \ + --merge-install \ + --symlink-install \ + --cmake-args "-DCMAKE_BUILD_TYPE=$BUILD_TYPE" "-DCMAKE_EXPORT_COMPILE_COMMANDS=On" \ + -Wall -Wextra -Wpedantic diff --git a/setup.sh b/setup.sh new file mode 100755 index 0000000..beab00e --- /dev/null +++ b/setup.sh @@ -0,0 +1,7 @@ +#!/bin/bash +set -e + +vcs import < src/ros2.repos src +sudo apt-get update +rosdep update --rosdistro=$ROS_DISTRO +rosdep install --from-paths src --ignore-src -y --rosdistro=$ROS_DISTRO diff --git a/test.sh b/test.sh new file mode 100755 index 0000000..793de56 --- /dev/null +++ b/test.sh @@ -0,0 +1,6 @@ +#!/bin/bash +set -e + +if [ -f install/setup.bash ]; then source install/setup.bash; fi +colcon test --merge-install +colcon test-result --verbose From 6993c06754bff63e8d4f0e836084e4b0ac08e999 Mon Sep 17 00:00:00 2001 From: Arkadiy Telegin Date: Thu, 28 Mar 2024 18:10:23 +0000 Subject: [PATCH 20/25] .devcontainer dockerfile --- .devcontainer/Dockerfile | 121 ++++++++++++++++++++++++++++++++ .devcontainer/devcontainer.json | 3 +- 2 files changed, 123 insertions(+), 1 deletion(-) create mode 100644 .devcontainer/Dockerfile diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile new file mode 100644 index 0000000..ebe8113 --- /dev/null +++ b/.devcontainer/Dockerfile @@ -0,0 +1,121 @@ +ARG BASE_IMAGE=ros:humble-perception + +# The following steps are based on the offical multi-stage build: https://github.com/IntelRealSense/librealsense/blob/master/scripts/Docker/Dockerfile +################################# +# Librealsense Builder Stage # +################################# +FROM $BASE_IMAGE as librealsense-builder + +SHELL ["/bin/bash", "-c"] + +ENV DEBIAN_FRONTEND=noninteractive + +RUN apt-get update \ + && apt-get install -qq -y --no-install-recommends \ + build-essential \ + cmake \ + git \ + libssl-dev \ + libusb-1.0-0-dev \ + pkg-config \ + libgtk-3-dev \ + libglfw3-dev \ + libgl1-mesa-dev \ + libglu1-mesa-dev \ + curl \ + python3 \ + python3-dev \ + ca-certificates \ + && rm -rf /var/lib/apt/lists/* + +WORKDIR /usr/src +# Get the latest tag of remote repository: https://stackoverflow.com/a/12704727 +# Needs to be a single command as ENV can't be set from Bash command: https://stackoverflow.com/questions/34911622/dockerfile-set-env-to-result-of-command +RUN export LIBRS_VERSION=2.53.1; \ + curl https://codeload.github.com/IntelRealSense/librealsense/tar.gz/refs/tags/v${LIBRS_VERSION} -o librealsense.tar.gz; \ + tar -zxf librealsense.tar.gz; \ + rm librealsense.tar.gz; \ + ln -s /usr/src/librealsense-${LIBRS_VERSION} /usr/src/librealsense + +RUN cd /usr/src/librealsense \ + && mkdir build && cd build \ + && cmake \ + -DCMAKE_C_FLAGS_RELEASE="${CMAKE_C_FLAGS_RELEASE} -s" \ + -DCMAKE_CXX_FLAGS_RELEASE="${CMAKE_CXX_FLAGS_RELEASE} -s" \ + -DCMAKE_INSTALL_PREFIX=/opt/librealsense \ + -DBUILD_GRAPHICAL_EXAMPLES=OFF \ + -DBUILD_PYTHON_BINDINGS:bool=true \ + -DCMAKE_BUILD_TYPE=Release ../ \ + && make -j$(($(nproc)-1)) all \ + && make install + + ENV DEBIAN_FRONTEND=dialog + +###################################### +# librealsense Base Image Stage # +###################################### +FROM ${BASE_IMAGE} as librealsense + +SHELL ["/bin/bash", "-c"] + +COPY --from=librealsense-builder /opt/librealsense /usr/local/ +COPY --from=librealsense-builder /usr/lib/python3/dist-packages/pyrealsense2 /usr/lib/python3/dist-packages/pyrealsense2 +COPY --from=librealsense-builder /usr/src/librealsense/config/99-realsense-libusb.rules /etc/udev/rules.d/ +ENV PYTHONPATH=${PYTHONPATH}:/usr/local/lib + +ENV DEBIAN_FRONTEND=noninteractive + +RUN apt-get update \ + && apt-get install -y --no-install-recommends \ + libusb-1.0-0 \ + udev \ + apt-transport-https \ + ca-certificates \ + curl \ + software-properties-common \ + && rm -rf /var/lib/apt/lists/* +# The following steps are based on: https://github.com/IntelRealSense/realsense-ros/tree/ros2-development +# ENV WS_DIR="/ros2_ws" +# WORKDIR ${WS_DIR} +RUN apt-get update -y \ + && apt-get install -y \ + ros-${ROS_DISTRO}-rviz2 +# && mkdir src + +RUN sudo apt-get install ros-humble-rmw-cyclonedds-cpp -y +ENV RMW_IMPLEMENTATION=rmw_cyclonedds_cpp +RUN sudo apt-get install ros-humble-image-transport-plugins -y + +# COPY docker/realsense-ros ./src +# COPY docker/multi_camera_launch.py /ros2_ws/src/realsense2_camera/launch +# COPY docker/depth_handler /ros2_ws/src/depth_handler +# COPY docker/proximity_monitor /ros2_ws/src/proximity_monitor +RUN apt-get install -y python3-rosdep \ + && apt-get update \ + && source /opt/ros/${ROS_DISTRO}/setup.bash \ + && rm /etc/ros/rosdep/sources.list.d/20-default.list \ + && rosdep init \ + && rosdep update \ + && rosdep install -i --from-path src --rosdistro ${ROS_DISTRO} --skip-keys=librealsense2 -y \ + && colcon build +RUN sudo apt-get install python3-opencv +# RUN echo "source /ros2_ws/install/setup.bash" >> ~/.bashrc +# COPY docker/launch.sh . + +ARG USERNAME=ros +ARG USER_UID=1000 +ARG USER_GID=$USER_UID + +# Create a non-root user +RUN groupadd --gid $USER_GID $USERNAME \ + && useradd -s /bin/bash --uid $USER_UID --gid $USER_GID -m $USERNAME \ + # Add sudo support for the non-root user + && apt-get update \ + && apt-get install -y sudo \ + && echo $USERNAME ALL=\(root\) NOPASSWD:ALL > /etc/sudoers.d/$USERNAME\ + && chmod 0440 /etc/sudoers.d/$USERNAME \ + && rm -rf /var/lib/apt/lists/* + +# RUN chmod +x /ros2_ws/launch.sh +ENV DEBIAN_FRONTEND=dialog +# ENTRYPOINT ["/ros2_ws/launch.sh"] diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index 3539a1d..47ae70b 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -1,6 +1,7 @@ // See https://aka.ms/vscode-remote/devcontainer.json for format details. { - "dockerFile": "../docker/Dockerfile", + "dockerFile": "Dockerfile", + "context": "..", "build": { "args": { "WORKSPACE": "${containerWorkspaceFolder}" From 23508b0fed89ae78ef201f53a76a2dec1ddde92f Mon Sep 17 00:00:00 2001 From: Arkadiy Telegin Date: Thu, 28 Mar 2024 19:33:33 +0100 Subject: [PATCH 21/25] Rename docker -> src --- build.sh | 10 -- docker/launch.sh | 3 - docker/multi_camera_launch.py | 100 ------------------ docker/proximity_monitor/CMakeLists.txt | 14 --- .../launch/proximity_monitor_launch.py | 25 ----- docker/proximity_monitor/package.xml | 31 ------ .../proximity_monitor/__init__.py | 0 .../proximity_monitor/proximity_monitor.py | 100 ------------------ .../resource/proximity_monitor | 0 docker/proximity_monitor/setup.cfg | 4 - docker/proximity_monitor/setup.py | 26 ----- .../proximity_monitor/test/test_copyright.py | 25 ----- docker/proximity_monitor/test/test_flake8.py | 25 ----- docker/proximity_monitor/test/test_pep257.py | 23 ---- {docker => src}/Dockerfile | 10 +- .../depth_handler/depth_handler/__init__.py | 0 .../depth_handler/depth_subscriber.py | 0 .../depth_handler/joint_plotter.py | 0 .../depth_handler/pcd_subscriber.py | 0 {docker => src}/depth_handler/package.xml | 0 .../depth_handler/resource/depth_handler | 0 {docker => src}/depth_handler/setup.cfg | 0 {docker => src}/depth_handler/setup.py | 0 .../depth_handler/test/test_copyright.py | 0 .../depth_handler/test/test_flake8.py | 0 .../depth_handler/test/test_pep257.py | 0 {docker => src}/docker-compose-gui-nvidia.yml | 0 {docker => src}/docker-compose-gui.yml | 0 {docker => src}/docker-compose-nvidia.yml | 0 {docker => src}/docker-compose-unity.yml | 0 {docker => src}/docker-compose.yml | 2 +- src/launch.sh | 3 + src/proximity_monitor | 1 + src/realsense-ros | 1 + test.sh | 6 -- 35 files changed, 11 insertions(+), 398 deletions(-) delete mode 100755 build.sh delete mode 100644 docker/launch.sh delete mode 100644 docker/multi_camera_launch.py delete mode 100644 docker/proximity_monitor/CMakeLists.txt delete mode 100644 docker/proximity_monitor/launch/proximity_monitor_launch.py delete mode 100644 docker/proximity_monitor/package.xml delete mode 100644 docker/proximity_monitor/proximity_monitor/__init__.py delete mode 100644 docker/proximity_monitor/proximity_monitor/proximity_monitor.py delete mode 100644 docker/proximity_monitor/resource/proximity_monitor delete mode 100644 docker/proximity_monitor/setup.cfg delete mode 100644 docker/proximity_monitor/setup.py delete mode 100644 docker/proximity_monitor/test/test_copyright.py delete mode 100644 docker/proximity_monitor/test/test_flake8.py delete mode 100644 docker/proximity_monitor/test/test_pep257.py rename {docker => src}/Dockerfile (93%) rename {docker => src}/depth_handler/depth_handler/__init__.py (100%) rename {docker => src}/depth_handler/depth_handler/depth_subscriber.py (100%) rename {docker => src}/depth_handler/depth_handler/joint_plotter.py (100%) rename {docker => src}/depth_handler/depth_handler/pcd_subscriber.py (100%) rename {docker => src}/depth_handler/package.xml (100%) rename {docker => src}/depth_handler/resource/depth_handler (100%) rename {docker => src}/depth_handler/setup.cfg (100%) rename {docker => src}/depth_handler/setup.py (100%) rename {docker => src}/depth_handler/test/test_copyright.py (100%) rename {docker => src}/depth_handler/test/test_flake8.py (100%) rename {docker => src}/depth_handler/test/test_pep257.py (100%) rename {docker => src}/docker-compose-gui-nvidia.yml (100%) rename {docker => src}/docker-compose-gui.yml (100%) rename {docker => src}/docker-compose-nvidia.yml (100%) rename {docker => src}/docker-compose-unity.yml (100%) rename {docker => src}/docker-compose.yml (86%) create mode 100644 src/launch.sh create mode 160000 src/proximity_monitor create mode 160000 src/realsense-ros delete mode 100755 test.sh diff --git a/build.sh b/build.sh deleted file mode 100755 index 7bf6bd8..0000000 --- a/build.sh +++ /dev/null @@ -1,10 +0,0 @@ -#!/bin/bash -set -e - -# Set the default build type -BUILD_TYPE=RelWithDebInfo -colcon build \ - --merge-install \ - --symlink-install \ - --cmake-args "-DCMAKE_BUILD_TYPE=$BUILD_TYPE" "-DCMAKE_EXPORT_COMPILE_COMMANDS=On" \ - -Wall -Wextra -Wpedantic diff --git a/docker/launch.sh b/docker/launch.sh deleted file mode 100644 index 6319698..0000000 --- a/docker/launch.sh +++ /dev/null @@ -1,3 +0,0 @@ -#!/bin/bash -source install/setup.bash -ros2 launch realsense2_camera multi_camera_launch.py diff --git a/docker/multi_camera_launch.py b/docker/multi_camera_launch.py deleted file mode 100644 index a93857e..0000000 --- a/docker/multi_camera_launch.py +++ /dev/null @@ -1,100 +0,0 @@ -# Copyright 2023 Intel Corporation. All Rights Reserved. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -# DESCRIPTION # -# ----------- # -# Use this launch file to launch 2 devices. -# The Parameters available for definition in the command line for each camera are described in rs_launch.configurable_parameters -# For each device, the parameter name was changed to include an index. -# For example: to set camera_name for device1 set parameter camera_name1. -# command line example: -# ros2 launch realsense2_camera rs_multi_camera_launch.py camera_name1:=D400 device_type2:=l5. device_type1:=d4.. - -"""Launch realsense2_camera node.""" -import copy -from launch import LaunchDescription -import launch_ros.actions -from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir -from launch.launch_description_sources import PythonLaunchDescriptionSource -import sys -import pathlib -sys.path.append(str(pathlib.Path(__file__).parent.absolute())) -import rs_launch - -local_parameters = [{'name': 'camera_name1', 'default': 'camera_left', 'description': 'camera unique name'}, - # {'name': 'camera_name2', 'default': 'camera2', 'description': 'camera unique name'}, - ] - -def set_configurable_parameters(local_params): - return dict([(param['original_name'], LaunchConfiguration(param['name'])) for param in local_params]) - -def duplicate_params(general_params, posix): - local_params = copy.deepcopy(general_params) - for param in local_params: - param['original_name'] = param['name'] - param['name'] += posix - return local_params - - -def generate_launch_description(): - params1 = duplicate_params(rs_launch.configurable_parameters, '1') - # params2 = duplicate_params(rs_launch.configurable_parameters, '2') - return LaunchDescription( - rs_launch.declare_configurable_parameters(local_parameters) + - rs_launch.declare_configurable_parameters(params1) + - # rs_launch.declare_configurable_parameters(params2) + - [ - IncludeLaunchDescription( - PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/rs_launch.py']), - launch_arguments={ - 'camera_name': 'camera_back_top', - "serial_no":"_829212071824", - 'depth_module.profile': '424x240x6', - 'rgb_camera.profile': '320x240x6', - }.items(), - ), - IncludeLaunchDescription( - PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/rs_launch.py']), - launch_arguments={ - 'camera_name': 'camera_back_bottom', - "serial_no":"_829212071844", - 'depth_module.profile': '424x240x6 ', - 'rgb_camera.profile': '320x240x6',}.items(), - ), - # launch_ros.actions.Node( - # package = "depth_handler", - # executable = "depth_subscriber" - # ), - # launch_ros.actions.Node( - # package = "depth_handler", - # executable = "pcd_subscriber", - # name = 'pointcloud_shoulder_left', - # parameters = [{'camera_side': 'left'}] - # ), - launch_ros.actions.Node( - package = "proximity_monitor", - executable = "proximity_monitor", - name = 'proximity_monitor_top', - parameters = [{'camera_name': 'camera_back_top'}, - {'proximity_warning_threshold': 0.2}] - ), - launch_ros.actions.Node ( - package = "proximity_monitor", - executable = "proximity_monitor", - name = 'proximity_monitor_bottom', - parameters = [{'camera_name': 'camera_back_bottom'}, - {'proximity_warning_threshold': 0.2}] - ), - ]) diff --git a/docker/proximity_monitor/CMakeLists.txt b/docker/proximity_monitor/CMakeLists.txt deleted file mode 100644 index d9d0c1b..0000000 --- a/docker/proximity_monitor/CMakeLists.txt +++ /dev/null @@ -1,14 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(your_package_name) - -find_package(ament_cmake REQUIRED) -find_package(rosidl_default_generators REQUIRED) -find_package(rclpy REQUIRED) -find_package(std_msgs REQUIRED) - -# rosidl_generate_interfaces(${PROJECT_NAME} -# "msg/ProximityWarning.msg" -# ) -# We use built-in messages to avoid the need to compile for Unity - -ament_package() diff --git a/docker/proximity_monitor/launch/proximity_monitor_launch.py b/docker/proximity_monitor/launch/proximity_monitor_launch.py deleted file mode 100644 index de6add5..0000000 --- a/docker/proximity_monitor/launch/proximity_monitor_launch.py +++ /dev/null @@ -1,25 +0,0 @@ -from launch import LaunchDescription -from launch.actions import Node, DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration - -configurable_parameters = [ - {'name': 'first_camera_name', 'default': 'camera1', 'description': 'name of the first camera'}, - {'name': 'second_camera_name', 'default': 'camera2', 'description': 'name of the second camera'}, - {'name': 'threshold', 'default': 1.0, 'description': 'distance threshold for proximity warning in meters'}, -] - -def declare_configurable_parameters(parameters): - return [DeclareLaunchArgument(param['name'], default_value=param['default'], description=param['description']) for param in parameters] - -def set_configurable_parameters(parameters): - return dict([(param['name'], LaunchConfiguration(param['name'])) for param in parameters]) - -def generate_launch_description(): - return LaunchDescription( - declare_configurable_parameters(configurable_parameters) + [ - Node(package='proximity_monitor', - executable='proximity_monitor', - name='proximity_monitor', - parameters=[set_configurable_parameters(configurable_parameters)], - ), - ]) \ No newline at end of file diff --git a/docker/proximity_monitor/package.xml b/docker/proximity_monitor/package.xml deleted file mode 100644 index f1bbb88..0000000 --- a/docker/proximity_monitor/package.xml +++ /dev/null @@ -1,31 +0,0 @@ - - - - proximity_monitor - 0.0.0 - TODO: Package description - ros - TODO: License declaration - - rclpy - sensor_msgs - std_msgs - - ament_cmake - rosidl_default_generators - rosidl_default_runtime - - realsense2_camera - realsense2_description - - cv_bridge - - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest - - - ament_python - - diff --git a/docker/proximity_monitor/proximity_monitor/__init__.py b/docker/proximity_monitor/proximity_monitor/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/docker/proximity_monitor/proximity_monitor/proximity_monitor.py b/docker/proximity_monitor/proximity_monitor/proximity_monitor.py deleted file mode 100644 index 650f105..0000000 --- a/docker/proximity_monitor/proximity_monitor/proximity_monitor.py +++ /dev/null @@ -1,100 +0,0 @@ -import rclpy -from rclpy.node import Node -from rcl_interfaces.msg import SetParametersResult -from std_msgs.msg import Float32MultiArray -from sensor_msgs.msg import Image - -from rcl_interfaces.srv import SetParameters - -from cv_bridge import CvBridge, CvBridgeError -import numpy as np - -bridge = CvBridge() - -# ProximityWarning message -# Float32MultiArray: [warning, proximity, threshold] -# warning: 1 if the distance is less than the threshold -# proximity: distance to the nearest point (meters) -# threshold: distance threshold (meters) - -class ProximityMonitorNode(Node): - def __init__(self): - super().__init__('proximity_monitor') - - # Declare parameters and initialize them - self.declare_parameter('camera_name', 'camera') - self.declare_parameter('proximity_warning_threshold', 0.2) - - # Get parameters - self.camera_name = self.get_parameter('camera_name').get_parameter_value().string_value - self.threshold = self.get_parameter('proximity_warning_threshold').get_parameter_value().double_value - - # Publishers and Subscribers - self.warning_topic = f'/roboy/pinky/sensing/{self.camera_name}/proximity_warning' - self.warning_publisher = self.create_publisher(Float32MultiArray, self.warning_topic, 10) - - self.camera_subscriber = self.create_subscription(Image, f'/{self.camera_name}/depth/image_rect_raw', self.camera_callback, 10) - self.parameter_client = self.create_client(SetParameters, - f'/{self.camera_name}/set_parameters') - - self.get_logger().info(f'Listening for the camera on /{self.camera_name}/depth/image_rect_raw') - - self.last_warning = 0.0 - self.last_proximity = None - - self.timer = self.create_timer(0.025, self.publish_warning) # 40 Hz - - # Dynamic reconfiguration callback - self.add_on_set_parameters_callback(self.parameters_callback) - - def process_camera_data(self, data): - proximity = self.nearest_point_distance(data) - warning = 1.0 if proximity < self.threshold else 0.0 - return warning, proximity - - def camera_callback(self, data): - self.last_warning, self.last_proximity = self.process_camera_data(data) - - def publish_warning(self): - warning_msg = Float32MultiArray() - - if self.last_warning: - self.get_logger().info(f'Publishing warning: {self.last_proximity}', throttle_duration_sec=1.0) - else: - self.get_logger().debug(f'Publishing NO warning', throttle_duration_sec=1.0) - - warning_msg.data = [ - self.last_warning, - self.last_proximity or float('NaN'), - self.threshold - ] - self.warning_publisher.publish(warning_msg) - - def parameters_callback(self, params): - for param in params: - if param.name == 'proximity_warning_threshold' and param.type_ == param.PARAMETER_DOUBLE: - self.threshold = param.value - self.get_logger().info(f'New proximity_warning_threshold set: {self.threshold}') - return SetParametersResult(successful=True) - return SetParametersResult(successful=False) - - def nearest_point_distance(self, ros_image): - try: - depth_image = bridge.imgmsg_to_cv2(ros_image, desired_encoding='passthrough') - depth_array = np.array(depth_image, dtype=np.float64) - min_distance = float(np.min(depth_array[depth_array!=0])) - return min_distance * 0.001 # Convert to meters - except CvBridgeError as e: - self.get_logger().error(e) - - -def main(args=None): - rclpy.init(args=args) - proximity_monitor_node = ProximityMonitorNode() - proximity_monitor_node.get_logger().set_level(rclpy.logging.LoggingSeverity.DEBUG) - proximity_monitor_node.get_logger().info('Proximity Monitor Node started') - rclpy.spin(proximity_monitor_node) - rclpy.shutdown() - -if __name__ == '__main__': - main() diff --git a/docker/proximity_monitor/resource/proximity_monitor b/docker/proximity_monitor/resource/proximity_monitor deleted file mode 100644 index e69de29..0000000 diff --git a/docker/proximity_monitor/setup.cfg b/docker/proximity_monitor/setup.cfg deleted file mode 100644 index 48ed68f..0000000 --- a/docker/proximity_monitor/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/proximity_monitor -[install] -install_scripts=$base/lib/proximity_monitor diff --git a/docker/proximity_monitor/setup.py b/docker/proximity_monitor/setup.py deleted file mode 100644 index 7098b48..0000000 --- a/docker/proximity_monitor/setup.py +++ /dev/null @@ -1,26 +0,0 @@ -from setuptools import find_packages, setup - -package_name = 'proximity_monitor' - -setup( - name=package_name, - version='0.0.0', - packages=find_packages(exclude=['test']), - data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), - ], - install_requires=['setuptools'], - zip_safe=True, - maintainer='ros', - maintainer_email='ros@todo.todo', - description='TODO: Package description', - license='TODO: License declaration', - tests_require=['pytest'], - entry_points={ - 'console_scripts': [ - 'proximity_monitor = proximity_monitor.proximity_monitor:main' - ], - }, -) diff --git a/docker/proximity_monitor/test/test_copyright.py b/docker/proximity_monitor/test/test_copyright.py deleted file mode 100644 index 97a3919..0000000 --- a/docker/proximity_monitor/test/test_copyright.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_copyright.main import main -import pytest - - -# Remove the `skip` decorator once the source file(s) have a copyright header -@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') -@pytest.mark.copyright -@pytest.mark.linter -def test_copyright(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found errors' diff --git a/docker/proximity_monitor/test/test_flake8.py b/docker/proximity_monitor/test/test_flake8.py deleted file mode 100644 index 27ee107..0000000 --- a/docker/proximity_monitor/test/test_flake8.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_flake8.main import main_with_errors -import pytest - - -@pytest.mark.flake8 -@pytest.mark.linter -def test_flake8(): - rc, errors = main_with_errors(argv=[]) - assert rc == 0, \ - 'Found %d code style errors / warnings:\n' % len(errors) + \ - '\n'.join(errors) diff --git a/docker/proximity_monitor/test/test_pep257.py b/docker/proximity_monitor/test/test_pep257.py deleted file mode 100644 index b234a38..0000000 --- a/docker/proximity_monitor/test/test_pep257.py +++ /dev/null @@ -1,23 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_pep257.main import main -import pytest - - -@pytest.mark.linter -@pytest.mark.pep257 -def test_pep257(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found code style errors / warnings' diff --git a/docker/Dockerfile b/src/Dockerfile similarity index 93% rename from docker/Dockerfile rename to src/Dockerfile index 589f272..fadc33d 100644 --- a/docker/Dockerfile +++ b/src/Dockerfile @@ -86,10 +86,10 @@ RUN sudo apt-get install ros-humble-rmw-cyclonedds-cpp -y ENV RMW_IMPLEMENTATION=rmw_cyclonedds_cpp RUN sudo apt-get install ros-humble-image-transport-plugins -y -COPY docker/realsense-ros ./src -COPY docker/multi_camera_launch.py /ros2_ws/src/realsense2_camera/launch -COPY docker/depth_handler /ros2_ws/src/depth_handler -COPY docker/proximity_monitor /ros2_ws/src/proximity_monitor +COPY src/realsense-ros ./src +COPY src/multi_camera_launch.py /ros2_ws/src/realsense2_camera/launch +COPY src/depth_handler /ros2_ws/src/depth_handler +COPY src/proximity_monitor /ros2_ws/src/proximity_monitor RUN apt-get install -y python3-rosdep \ && apt-get update \ && source /opt/ros/${ROS_DISTRO}/setup.bash \ @@ -100,7 +100,7 @@ RUN apt-get install -y python3-rosdep \ && colcon build RUN sudo apt-get install python3-opencv RUN echo "source /ros2_ws/install/setup.bash" >> ~/.bashrc -COPY docker/launch.sh . +COPY src/launch.sh . RUN chmod +x /ros2_ws/launch.sh ENV DEBIAN_FRONTEND=dialog ENTRYPOINT ["/ros2_ws/launch.sh"] diff --git a/docker/depth_handler/depth_handler/__init__.py b/src/depth_handler/depth_handler/__init__.py similarity index 100% rename from docker/depth_handler/depth_handler/__init__.py rename to src/depth_handler/depth_handler/__init__.py diff --git a/docker/depth_handler/depth_handler/depth_subscriber.py b/src/depth_handler/depth_handler/depth_subscriber.py similarity index 100% rename from docker/depth_handler/depth_handler/depth_subscriber.py rename to src/depth_handler/depth_handler/depth_subscriber.py diff --git a/docker/depth_handler/depth_handler/joint_plotter.py b/src/depth_handler/depth_handler/joint_plotter.py similarity index 100% rename from docker/depth_handler/depth_handler/joint_plotter.py rename to src/depth_handler/depth_handler/joint_plotter.py diff --git a/docker/depth_handler/depth_handler/pcd_subscriber.py b/src/depth_handler/depth_handler/pcd_subscriber.py similarity index 100% rename from docker/depth_handler/depth_handler/pcd_subscriber.py rename to src/depth_handler/depth_handler/pcd_subscriber.py diff --git a/docker/depth_handler/package.xml b/src/depth_handler/package.xml similarity index 100% rename from docker/depth_handler/package.xml rename to src/depth_handler/package.xml diff --git a/docker/depth_handler/resource/depth_handler b/src/depth_handler/resource/depth_handler similarity index 100% rename from docker/depth_handler/resource/depth_handler rename to src/depth_handler/resource/depth_handler diff --git a/docker/depth_handler/setup.cfg b/src/depth_handler/setup.cfg similarity index 100% rename from docker/depth_handler/setup.cfg rename to src/depth_handler/setup.cfg diff --git a/docker/depth_handler/setup.py b/src/depth_handler/setup.py similarity index 100% rename from docker/depth_handler/setup.py rename to src/depth_handler/setup.py diff --git a/docker/depth_handler/test/test_copyright.py b/src/depth_handler/test/test_copyright.py similarity index 100% rename from docker/depth_handler/test/test_copyright.py rename to src/depth_handler/test/test_copyright.py diff --git a/docker/depth_handler/test/test_flake8.py b/src/depth_handler/test/test_flake8.py similarity index 100% rename from docker/depth_handler/test/test_flake8.py rename to src/depth_handler/test/test_flake8.py diff --git a/docker/depth_handler/test/test_pep257.py b/src/depth_handler/test/test_pep257.py similarity index 100% rename from docker/depth_handler/test/test_pep257.py rename to src/depth_handler/test/test_pep257.py diff --git a/docker/docker-compose-gui-nvidia.yml b/src/docker-compose-gui-nvidia.yml similarity index 100% rename from docker/docker-compose-gui-nvidia.yml rename to src/docker-compose-gui-nvidia.yml diff --git a/docker/docker-compose-gui.yml b/src/docker-compose-gui.yml similarity index 100% rename from docker/docker-compose-gui.yml rename to src/docker-compose-gui.yml diff --git a/docker/docker-compose-nvidia.yml b/src/docker-compose-nvidia.yml similarity index 100% rename from docker/docker-compose-nvidia.yml rename to src/docker-compose-nvidia.yml diff --git a/docker/docker-compose-unity.yml b/src/docker-compose-unity.yml similarity index 100% rename from docker/docker-compose-unity.yml rename to src/docker-compose-unity.yml diff --git a/docker/docker-compose.yml b/src/docker-compose.yml similarity index 86% rename from docker/docker-compose.yml rename to src/docker-compose.yml index b28db05..dd10db6 100644 --- a/docker/docker-compose.yml +++ b/src/docker-compose.yml @@ -3,7 +3,7 @@ services: realsense_ros2: build: context: .. - dockerfile: docker/Dockerfile + dockerfile: src/Dockerfile #target: librealsense tty: true volumes: diff --git a/src/launch.sh b/src/launch.sh new file mode 100644 index 0000000..1df99ff --- /dev/null +++ b/src/launch.sh @@ -0,0 +1,3 @@ +#!/bin/bash +source install/setup.bash +ros2 launch proximity_monitor multi_camera_launch.py diff --git a/src/proximity_monitor b/src/proximity_monitor new file mode 160000 index 0000000..19e2794 --- /dev/null +++ b/src/proximity_monitor @@ -0,0 +1 @@ +Subproject commit 19e27942d7d264d13f28874f66b9635ce8bf9fde diff --git a/src/realsense-ros b/src/realsense-ros new file mode 160000 index 0000000..de79dec --- /dev/null +++ b/src/realsense-ros @@ -0,0 +1 @@ +Subproject commit de79dec79fa239d3178145b4e150cdb86bf8bf9f diff --git a/test.sh b/test.sh deleted file mode 100755 index 793de56..0000000 --- a/test.sh +++ /dev/null @@ -1,6 +0,0 @@ -#!/bin/bash -set -e - -if [ -f install/setup.bash ]; then source install/setup.bash; fi -colcon test --merge-install -colcon test-result --verbose From 9abbdacdb3c4c58962eeb72a05b3ec1693e870ad Mon Sep 17 00:00:00 2001 From: Arkadiy Telegin Date: Thu, 28 Mar 2024 19:35:27 +0100 Subject: [PATCH 22/25] Fix submodule --- src/proximity_monitor | 1 - src/proximity_monitor/CMakeLists.txt | 20 ++++ .../launch/multi_camera_launch.py | 97 +++++++++++++++++ .../launch/proximity_monitor_launch.py | 25 +++++ src/proximity_monitor/package.xml | 31 ++++++ .../proximity_monitor/__init__.py | 0 .../proximity_monitor/proximity_monitor.py | 100 ++++++++++++++++++ .../resource/proximity_monitor | 0 src/proximity_monitor/setup.cfg | 4 + src/proximity_monitor/setup.py | 27 +++++ src/proximity_monitor/test/test_copyright.py | 25 +++++ src/proximity_monitor/test/test_flake8.py | 25 +++++ src/proximity_monitor/test/test_pep257.py | 23 ++++ 13 files changed, 377 insertions(+), 1 deletion(-) delete mode 160000 src/proximity_monitor create mode 100644 src/proximity_monitor/CMakeLists.txt create mode 100644 src/proximity_monitor/launch/multi_camera_launch.py create mode 100644 src/proximity_monitor/launch/proximity_monitor_launch.py create mode 100644 src/proximity_monitor/package.xml create mode 100644 src/proximity_monitor/proximity_monitor/__init__.py create mode 100644 src/proximity_monitor/proximity_monitor/proximity_monitor.py create mode 100644 src/proximity_monitor/resource/proximity_monitor create mode 100644 src/proximity_monitor/setup.cfg create mode 100644 src/proximity_monitor/setup.py create mode 100644 src/proximity_monitor/test/test_copyright.py create mode 100644 src/proximity_monitor/test/test_flake8.py create mode 100644 src/proximity_monitor/test/test_pep257.py diff --git a/src/proximity_monitor b/src/proximity_monitor deleted file mode 160000 index 19e2794..0000000 --- a/src/proximity_monitor +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 19e27942d7d264d13f28874f66b9635ce8bf9fde diff --git a/src/proximity_monitor/CMakeLists.txt b/src/proximity_monitor/CMakeLists.txt new file mode 100644 index 0000000..d75d9f6 --- /dev/null +++ b/src/proximity_monitor/CMakeLists.txt @@ -0,0 +1,20 @@ +cmake_minimum_required(VERSION 3.5) +project(your_package_name) + +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(rclpy REQUIRED) +find_package(std_msgs REQUIRED) + +# rosidl_generate_interfaces(${PROJECT_NAME} +# "msg/ProximityWarning.msg" +# ) +# We use built-in messages to avoid the need to compile for Unity + +# Install launch files +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME} + ) + +ament_package() diff --git a/src/proximity_monitor/launch/multi_camera_launch.py b/src/proximity_monitor/launch/multi_camera_launch.py new file mode 100644 index 0000000..5eda34b --- /dev/null +++ b/src/proximity_monitor/launch/multi_camera_launch.py @@ -0,0 +1,97 @@ +# Copyright 2023 Intel Corporation. All Rights Reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# DESCRIPTION # +# ----------- # +# Use this launch file to launch 2 devices. +# The Parameters available for definition in the command line for each camera are described in rs_launch.configurable_parameters +# For each device, the parameter name was changed to include an index. +# For example: to set camera_name for device1 set parameter camera_name1. +# command line example: +# ros2 launch realsense2_camera rs_multi_camera_launch.py camera_name1:=D400 device_type2:=l5. device_type1:=d4.. + +"""Launch realsense2_camera node.""" +import os +import copy +from launch import LaunchDescription +import launch_ros.actions +from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir +from launch.launch_description_sources import PythonLaunchDescriptionSource +from ament_index_python.packages import get_package_share_directory +import sys +import pathlib +sys.path.append(str(pathlib.Path(__file__).parent.absolute())) + + +def set_configurable_parameters(local_params): + return dict([(param['original_name'], LaunchConfiguration(param['name'])) for param in local_params]) + +def duplicate_params(general_params, posix): + local_params = copy.deepcopy(general_params) + for param in local_params: + param['original_name'] = param['name'] + param['name'] += posix + return local_params + +def generate_launch_description(): + realsense_launch_file = os.path.join( + get_package_share_directory('realsense2_camera'), + 'launch', + 'rs_launch.py' + ) + + return LaunchDescription([ + IncludeLaunchDescription( + PythonLaunchDescriptionSource(realsense_launch_file), + launch_arguments={ + 'camera_name': 'camera_back_top', + "serial_no":"_829212071824", + 'depth_module.profile': '424x240x6', + 'rgb_camera.profile': '320x240x6', + }.items(), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource(realsense_launch_file), + launch_arguments={ + 'camera_name': 'camera_back_bottom', + "serial_no":"_829212071844", + 'depth_module.profile': '424x240x6 ', + 'rgb_camera.profile': '320x240x6',}.items(), + ), + # launch_ros.actions.Node( + # package = "depth_handler", + # executable = "depth_subscriber" + # ), + # launch_ros.actions.Node( + # package = "depth_handler", + # executable = "pcd_subscriber", + # name = 'pointcloud_shoulder_left', + # parameters = [{'camera_side': 'left'}] + # ), + launch_ros.actions.Node( + package = "proximity_monitor", + executable = "proximity_monitor", + name = 'proximity_monitor_top', + parameters = [{'camera_name': 'camera_back_top'}, + {'proximity_warning_threshold': 0.2}] + ), + launch_ros.actions.Node ( + package = "proximity_monitor", + executable = "proximity_monitor", + name = 'proximity_monitor_bottom', + parameters = [{'camera_name': 'camera_back_bottom'}, + {'proximity_warning_threshold': 0.2}] + ), + ]) diff --git a/src/proximity_monitor/launch/proximity_monitor_launch.py b/src/proximity_monitor/launch/proximity_monitor_launch.py new file mode 100644 index 0000000..de6add5 --- /dev/null +++ b/src/proximity_monitor/launch/proximity_monitor_launch.py @@ -0,0 +1,25 @@ +from launch import LaunchDescription +from launch.actions import Node, DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration + +configurable_parameters = [ + {'name': 'first_camera_name', 'default': 'camera1', 'description': 'name of the first camera'}, + {'name': 'second_camera_name', 'default': 'camera2', 'description': 'name of the second camera'}, + {'name': 'threshold', 'default': 1.0, 'description': 'distance threshold for proximity warning in meters'}, +] + +def declare_configurable_parameters(parameters): + return [DeclareLaunchArgument(param['name'], default_value=param['default'], description=param['description']) for param in parameters] + +def set_configurable_parameters(parameters): + return dict([(param['name'], LaunchConfiguration(param['name'])) for param in parameters]) + +def generate_launch_description(): + return LaunchDescription( + declare_configurable_parameters(configurable_parameters) + [ + Node(package='proximity_monitor', + executable='proximity_monitor', + name='proximity_monitor', + parameters=[set_configurable_parameters(configurable_parameters)], + ), + ]) \ No newline at end of file diff --git a/src/proximity_monitor/package.xml b/src/proximity_monitor/package.xml new file mode 100644 index 0000000..f1bbb88 --- /dev/null +++ b/src/proximity_monitor/package.xml @@ -0,0 +1,31 @@ + + + + proximity_monitor + 0.0.0 + TODO: Package description + ros + TODO: License declaration + + rclpy + sensor_msgs + std_msgs + + ament_cmake + rosidl_default_generators + rosidl_default_runtime + + realsense2_camera + realsense2_description + + cv_bridge + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/proximity_monitor/proximity_monitor/__init__.py b/src/proximity_monitor/proximity_monitor/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/proximity_monitor/proximity_monitor/proximity_monitor.py b/src/proximity_monitor/proximity_monitor/proximity_monitor.py new file mode 100644 index 0000000..650f105 --- /dev/null +++ b/src/proximity_monitor/proximity_monitor/proximity_monitor.py @@ -0,0 +1,100 @@ +import rclpy +from rclpy.node import Node +from rcl_interfaces.msg import SetParametersResult +from std_msgs.msg import Float32MultiArray +from sensor_msgs.msg import Image + +from rcl_interfaces.srv import SetParameters + +from cv_bridge import CvBridge, CvBridgeError +import numpy as np + +bridge = CvBridge() + +# ProximityWarning message +# Float32MultiArray: [warning, proximity, threshold] +# warning: 1 if the distance is less than the threshold +# proximity: distance to the nearest point (meters) +# threshold: distance threshold (meters) + +class ProximityMonitorNode(Node): + def __init__(self): + super().__init__('proximity_monitor') + + # Declare parameters and initialize them + self.declare_parameter('camera_name', 'camera') + self.declare_parameter('proximity_warning_threshold', 0.2) + + # Get parameters + self.camera_name = self.get_parameter('camera_name').get_parameter_value().string_value + self.threshold = self.get_parameter('proximity_warning_threshold').get_parameter_value().double_value + + # Publishers and Subscribers + self.warning_topic = f'/roboy/pinky/sensing/{self.camera_name}/proximity_warning' + self.warning_publisher = self.create_publisher(Float32MultiArray, self.warning_topic, 10) + + self.camera_subscriber = self.create_subscription(Image, f'/{self.camera_name}/depth/image_rect_raw', self.camera_callback, 10) + self.parameter_client = self.create_client(SetParameters, + f'/{self.camera_name}/set_parameters') + + self.get_logger().info(f'Listening for the camera on /{self.camera_name}/depth/image_rect_raw') + + self.last_warning = 0.0 + self.last_proximity = None + + self.timer = self.create_timer(0.025, self.publish_warning) # 40 Hz + + # Dynamic reconfiguration callback + self.add_on_set_parameters_callback(self.parameters_callback) + + def process_camera_data(self, data): + proximity = self.nearest_point_distance(data) + warning = 1.0 if proximity < self.threshold else 0.0 + return warning, proximity + + def camera_callback(self, data): + self.last_warning, self.last_proximity = self.process_camera_data(data) + + def publish_warning(self): + warning_msg = Float32MultiArray() + + if self.last_warning: + self.get_logger().info(f'Publishing warning: {self.last_proximity}', throttle_duration_sec=1.0) + else: + self.get_logger().debug(f'Publishing NO warning', throttle_duration_sec=1.0) + + warning_msg.data = [ + self.last_warning, + self.last_proximity or float('NaN'), + self.threshold + ] + self.warning_publisher.publish(warning_msg) + + def parameters_callback(self, params): + for param in params: + if param.name == 'proximity_warning_threshold' and param.type_ == param.PARAMETER_DOUBLE: + self.threshold = param.value + self.get_logger().info(f'New proximity_warning_threshold set: {self.threshold}') + return SetParametersResult(successful=True) + return SetParametersResult(successful=False) + + def nearest_point_distance(self, ros_image): + try: + depth_image = bridge.imgmsg_to_cv2(ros_image, desired_encoding='passthrough') + depth_array = np.array(depth_image, dtype=np.float64) + min_distance = float(np.min(depth_array[depth_array!=0])) + return min_distance * 0.001 # Convert to meters + except CvBridgeError as e: + self.get_logger().error(e) + + +def main(args=None): + rclpy.init(args=args) + proximity_monitor_node = ProximityMonitorNode() + proximity_monitor_node.get_logger().set_level(rclpy.logging.LoggingSeverity.DEBUG) + proximity_monitor_node.get_logger().info('Proximity Monitor Node started') + rclpy.spin(proximity_monitor_node) + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/src/proximity_monitor/resource/proximity_monitor b/src/proximity_monitor/resource/proximity_monitor new file mode 100644 index 0000000..e69de29 diff --git a/src/proximity_monitor/setup.cfg b/src/proximity_monitor/setup.cfg new file mode 100644 index 0000000..48ed68f --- /dev/null +++ b/src/proximity_monitor/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/proximity_monitor +[install] +install_scripts=$base/lib/proximity_monitor diff --git a/src/proximity_monitor/setup.py b/src/proximity_monitor/setup.py new file mode 100644 index 0000000..d65d3d0 --- /dev/null +++ b/src/proximity_monitor/setup.py @@ -0,0 +1,27 @@ +from setuptools import find_packages, setup + +package_name = 'proximity_monitor' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ('share/' + package_name, ['launch/multi_camera_launch.py']) + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='ros', + maintainer_email='ros@todo.todo', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'proximity_monitor = proximity_monitor.proximity_monitor:main' + ], + }, +) diff --git a/src/proximity_monitor/test/test_copyright.py b/src/proximity_monitor/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/src/proximity_monitor/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/src/proximity_monitor/test/test_flake8.py b/src/proximity_monitor/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/src/proximity_monitor/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/src/proximity_monitor/test/test_pep257.py b/src/proximity_monitor/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/src/proximity_monitor/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' From 621a3a8cb48060100b122c0a8d071fa1306c0795 Mon Sep 17 00:00:00 2001 From: Arkadiy Telegin Date: Thu, 28 Mar 2024 19:37:10 +0100 Subject: [PATCH 23/25] Fix realsense submodule --- .gitmodules | 4 ++-- docker/realsense-ros | 1 - 2 files changed, 2 insertions(+), 3 deletions(-) delete mode 160000 docker/realsense-ros diff --git a/.gitmodules b/.gitmodules index bb4df47..afe87e9 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,3 +1,3 @@ -[submodule "docker/realsense-ros"] - path = docker/realsense-ros +[submodule "src/realsense-ros"] + path = src/realsense-ros url = https://github.com/juandelos/realsense-ros.git diff --git a/docker/realsense-ros b/docker/realsense-ros deleted file mode 160000 index de79dec..0000000 --- a/docker/realsense-ros +++ /dev/null @@ -1 +0,0 @@ -Subproject commit de79dec79fa239d3178145b4e150cdb86bf8bf9f From 6f695ed4b2e62f613e60e2625e9d5a543ff4d71e Mon Sep 17 00:00:00 2001 From: Arkadiy Telegin <58264717+arkadiy-telegin@users.noreply.github.com> Date: Mon, 8 Apr 2024 03:27:09 +0200 Subject: [PATCH 24/25] Update ReadMe.md to include devcontainers --- ReadMe.md | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/ReadMe.md b/ReadMe.md index b76be14..06b8807 100644 --- a/ReadMe.md +++ b/ReadMe.md @@ -1,7 +1,9 @@ # Docker for Intel Realsense cameras on ROS 2 -Author: Juan de los Rios(March 2023 - July 2023) +Author: Juan de los Rios (March 2023 - July 2023) + +Author \[proximity-monitor\]: Arkadiy Telegin (October 2023 - April 2024) Fork from [https://github.com/2b-t](https://github.com/2b-t) @@ -29,6 +31,13 @@ In the `docker-compose.yml` this is done with the options: - 'c 189:* rmw' ``` +#### 1.1 Development Docker container +For the ease of the development using VSCode, a similar Dockerfile is provided along `devcontainer.json` launching instruction. The difference with the main Dockerfile consists of mounting all the source files instead of copying them into the container image. Using devconainer allows code-completion within VSCode. Additionally all the changes reflected in the source code will be immediately available for building and running the code inside of the container without the need of rebuilding the docker image. This allows for much faster development and testing iterations. + +To build and launch the container development environment, please follow [these instructions (subsection "Open it in vscode")](https://github.com/athackst/vscode_ros2_workspace#open-it-in-vscode). After this, you'll have access to the development environment and terminal executing inside of the running container. + +To learn more about VSCode's devcontainers, please consult the [official documentation](https://code.visualstudio.com/docs/devcontainers/containers) + ## 2. Launching Allow the container to display contents on your host machine by typing From 4cff717df3f189525d1ba419b7d393a7c32d061c Mon Sep 17 00:00:00 2001 From: Arkadiy Telegin <58264717+arkadiy-telegin@users.noreply.github.com> Date: Mon, 8 Apr 2024 03:55:20 +0200 Subject: [PATCH 25/25] Update ReadMe.md - include proximity_monitor usage --- ReadMe.md | 41 ++++++++++++++++++++++++++++++++++++++++- 1 file changed, 40 insertions(+), 1 deletion(-) diff --git a/ReadMe.md b/ReadMe.md index 06b8807..7334379 100644 --- a/ReadMe.md +++ b/ReadMe.md @@ -18,6 +18,14 @@ In order to make it work with Nvidia Jetson Orin/Nano, librealsense has to be bu On the robot everything is installed and a server can be run to start on boot up under the name of peripheral.service +### 0.1 Proximity monitor +The latest addition to the repository is the `proximity_monitor` package. The ROS2 node defined in `proximity_monitor.py` reads the IntelRealsense camera feed, calculates the proximity of the closest object to the camera, and publishes a message to `/roboy/pinky/sensing//proximity_warning` topic at 40Hz. The published message is a [Float32MultiArray](https://github.com/ros2/common_interfaces/blob/rolling/std_msgs/msg/Float32MultiArray.msg) that contains: +- Warning: `1.0` if the proximity < `proximity_warning_threshold` and `0.0` otherwise +- Proximity: distance to the nearest point seen by the camera (meters) +- Current value of `proximity_warning_threshold` (meters) + +For configuration options of the proximity monitor please see section ["Proximity monitor usage"](#4-proximity-monitor-usage). + ## 1. Creating a Docker There are two different approaches for creating a Docker for a Realsense camera, one uses existing **Debian packages** while the other performs a full **compilation from source**. It is then important to mount `/dev` as a volume so that the Docker can access the hardware. @@ -157,7 +165,38 @@ def generate_launch_description(): ]) ``` -## 4. Debugging +## 4. Proximity monitor usage +The included `proximity-monitor` package declares the following configuration options: +- `camera_name` \[Default: `camera`\] +- `proximity_warning_threshold` \[Default: `0.2`\] + +Additionally, the `proximity_warning_threshold` parameter can be reconfigured dynamically during the runtime. For instructions how to dynamically reconfiure ROS2 parameters, please consult [the official documentation](https://docs.ros.org/en/humble/How-To-Guides/Using-ros2-param.html) + +The launch file `multi_camera_launch.py` provided by the package features a working launch configuration for two RealSense cameras. + +The following is a sample of ROS2 `LaunchDescription` using a camera named `camera1` with a warning threshold of 0.2 meters. You can adjust it according to your setup and complete it with the launch boilerplate as seen in `multi_camera_launch.py` to create your own ROS2 launch file that will start a RealSense ROS2 node along with the proximity monitor: +```python +LaunchDescription([ + IncludeLaunchDescription( + PythonLaunchDescriptionSource(realsense_launch_file), + launch_arguments={ + 'camera_name': 'camera1', + "serial_no":"_829212071824", # CHANGE THIS! + 'depth_module.profile': '424x240x6', + 'rgb_camera.profile': '320x240x6', + }.items(), + ), + launch_ros.actions.Node( + package = "proximity_monitor", + executable = "proximity_monitor", + name = 'proximity_monitor_1', + parameters = [{'camera_name': 'camera1'}, + {'proximity_warning_threshold': 0.2}] + ), +]) +``` + +## 5. Debugging The Intel Realsense driver has several serious flaws/bugs. Probably the main one is that it is **closely connected to the [kernel version of the Linux operating system](https://github.com/IntelRealSense/librealsense/issues/9360)**. If the Dockerfile above do not work then you are likely unlucky and it is an incompatible version of the kernel of your host system and you will either have to [downgrade your kernel](https://linuxhint.com/install-linux-kernel-ubuntu/) or switch to another Ubuntu version that is officially supported. Furthermore from time to time the software will give you cryptic error messages. For some restarting the corresponding software component might help, for others you will find a fix googling and with others you will have to learn to live. The Realsense is **pretty [picky about USB 3.x cables](https://github.com/IntelRealSense/librealsense/issues/2045)**. If your camera is detected via `rs-enumerate-devices`, you can see it `realsense-viewer` but can't output its video stream, then it might be that your cable lacks the bandwidth. Either you can try to turn down the resolution of the camera in the `realsense-viewer` or switch cable (preferably to one that is [already known to work](https://community.intel.com/t5/Items-with-no-label/long-USB-cable-for-realsense-D435i/m-p/694963)).