From 14e7da756762ace2f3a27afea501ddeade6e571e Mon Sep 17 00:00:00 2001 From: Korbinian Stein Date: Mon, 23 Jan 2023 09:53:53 +0100 Subject: [PATCH 1/8] feat(#111): Add adjusted camera and lidar placements --- code/agent/config/dev_objects.json | 91 ++++++++++++++++++++++++------ 1 file changed, 73 insertions(+), 18 deletions(-) diff --git a/code/agent/config/dev_objects.json b/code/agent/config/dev_objects.json index 31cc7038..bd071920 100644 --- a/code/agent/config/dev_objects.json +++ b/code/agent/config/dev_objects.json @@ -5,6 +5,10 @@ "id": "OpenDRIVE", "reading_frequency": 1 }, + { + "type": "sensor.pseudo.actor_list", + "id": "actor_list" + }, { "type": "vehicle.lincoln.mkz_2020", "id": "hero", @@ -22,9 +26,9 @@ "type": "sensor.camera.rgb", "id": "Center", "spawn_point": { - "x": 0.7, - "y": 0.4, - "z": 1.60, + "x": 0.0, + "y": 0.0, + "z": 1.70, "roll": 0.0, "pitch": 0.0, "yaw": 0.0, @@ -33,23 +37,74 @@ "chromatic_aberration_intensity": 0.5, "chromatic_aberration_offset": 0.0 }, - "image_size_x": 300, - "image_size_y": 200, - "fov": 100, - "attached_objects": [ - { - "type": "actor.pseudo.control", - "id": "control" - } - ] + "image_size_x": 1920, + "image_size_y": 1080, + "fov": 120 + }, + { + "type": "sensor.camera.rgb", + "id": "Left", + "spawn_point": { + "x": 0.0, + "y": 0.0, + "z": 1.70, + "roll": 0.0, + "pitch": 0.0, + "yaw": 90.0, + "lens_circle_multiplier": 3.0, + "lens_circle_falloff": 3.0, + "chromatic_aberration_intensity": 0.5, + "chromatic_aberration_offset": 0.0 + }, + "image_size_x": 1920, + "image_size_y": 1080, + "fov": 120 + }, + { + "type": "sensor.camera.rgb", + "id": "Right", + "spawn_point": { + "x": 0.0, + "y": 0.0, + "z": 1.70, + "roll": 0.0, + "pitch": 0.0, + "yaw": -90.0, + "lens_circle_multiplier": 3.0, + "lens_circle_falloff": 3.0, + "chromatic_aberration_intensity": 0.5, + "chromatic_aberration_offset": 0.0 + }, + "image_size_x": 1920, + "image_size_y": 1080, + "fov": 120 + }, + { + "type": "sensor.camera.rgb", + "id": "Back", + "spawn_point": { + "x": 0.0, + "y": 0.0, + "z": 1.70, + "roll": 0.0, + "pitch": 0.0, + "yaw": 180.0, + "lens_circle_multiplier": 3.0, + "lens_circle_falloff": 3.0, + "chromatic_aberration_intensity": 0.5, + "chromatic_aberration_offset": 0.0 + }, + "image_size_x": 1920, + "image_size_y": 1080, + "fov": 120 }, { "type": "sensor.lidar.ray_cast", "id": "LIDAR", "spawn_point": { - "x": 0.7, - "y": 0.4, - "z": 1.60, + "x": 0.0, + "y": 0.0, + "z": 1.70, "roll": 0.0, "pitch": 0.0, "yaw": -45.0 @@ -69,9 +124,9 @@ "type": "sensor.other.radar", "id": "RADAR", "spawn_point": { - "x": 0.7, - "y": 0.4, - "z": 1.60, + "x": 0.0, + "y": 0.0, + "z": 1.70, "roll": 0.0, "pitch": 0.0, "yaw": -45.0 From 9fcb0b8bff00ac8c8a8bb1da3f43ebda1e4c21ba Mon Sep 17 00:00:00 2001 From: Tim Dreier Date: Tue, 24 Jan 2023 21:48:06 +0100 Subject: [PATCH 2/8] feat(#157): Started lidar mapping --- code/agent/config/dev_objects.json | 8 +++--- code/perception/launch/perception.launch | 5 ++-- .../src/perception/lidar_debugger.py | 27 +++++++++++++++++++ .../src/perception/lidar_utility.py | 25 +++++++++++++++++ code/requirements.txt | 1 + 5 files changed, 60 insertions(+), 6 deletions(-) create mode 100644 code/perception/src/perception/lidar_debugger.py create mode 100644 code/perception/src/perception/lidar_utility.py diff --git a/code/agent/config/dev_objects.json b/code/agent/config/dev_objects.json index 31cc7038..9a55bb82 100644 --- a/code/agent/config/dev_objects.json +++ b/code/agent/config/dev_objects.json @@ -47,12 +47,12 @@ "type": "sensor.lidar.ray_cast", "id": "LIDAR", "spawn_point": { - "x": 0.7, - "y": 0.4, - "z": 1.60, + "x": 0.0, + "y": 0.0, + "z": 1.70, "roll": 0.0, "pitch": 0.0, - "yaw": -45.0 + "yaw": 0.0 }, "range": 85, "rotation_frequency": 10, diff --git a/code/perception/launch/perception.launch b/code/perception/launch/perception.launch index 3c51173a..7ba764ef 100644 --- a/code/perception/launch/perception.launch +++ b/code/perception/launch/perception.launch @@ -1,4 +1,5 @@ - - \ No newline at end of file + + + diff --git a/code/perception/src/perception/lidar_debugger.py b/code/perception/src/perception/lidar_debugger.py new file mode 100644 index 00000000..093e78c7 --- /dev/null +++ b/code/perception/src/perception/lidar_debugger.py @@ -0,0 +1,27 @@ +#!/usr/bin/env python +import rospy +from std_msgs.msg import String +from lidar_utility import LidarUtility + + +def callback(data): + lidar_utility = LidarUtility() + lidar_utility.getDepthAtPoint(0, 0) + + +def listener(): + # In ROS, nodes are uniquely named. If two nodes with the same + # name are launched, the previous one is kicked off. The + # anonymous=True flag means that rospy will choose a unique + # name for our 'listener' node so that multiple listeners can + # run simultaneously. + rospy.init_node('lidar_debugger', anonymous=True) + + rospy.Subscriber("/carla/hero/LIDAR", String, callback) + + # spin() simply keeps python from exiting until this node is stopped + rospy.spin() + + +if __name__ == '__main__': + listener() diff --git a/code/perception/src/perception/lidar_utility.py b/code/perception/src/perception/lidar_utility.py new file mode 100644 index 00000000..940fe0ec --- /dev/null +++ b/code/perception/src/perception/lidar_utility.py @@ -0,0 +1,25 @@ +import rospy +from sensor_msgs.point_cloud2 import PointCloud2 +import cv2 as cv + + +class LidarUtility: + + def getDepthAtPoint(self, x: int, y: int): + print('') + + def processLidarImage(self, lidar_image): + # https://docs.opencv.org/3.4/d9/d0c/group__calib3d.html#ga1019495a2c8d1743ed5cc23fa0daff8c + + # http://wiki.ros.org/image_geometry (maybe use this?) + cv.projectPoints( + lidar_image.data, + lidar_image.R, + None, + lidar_image.K, + lidar_image.D + ) + + def getLidarImage(self) -> PointCloud2: + lidar_data = rospy.wait_for_message("/carla/hero/LIDAR", PointCloud2) + return lidar_data diff --git a/code/requirements.txt b/code/requirements.txt index 0ab5f86b..8229134d 100644 --- a/code/requirements.txt +++ b/code/requirements.txt @@ -1,2 +1,3 @@ dvc==2.36.0 dvc-gdrive==2.19.1 +opencv-python==4.7.0.68 From 7a12e983ea612ff0ed945cc0fd8008b0cad261ff Mon Sep 17 00:00:00 2001 From: Tim Dreier Date: Mon, 6 Feb 2023 19:32:42 +0100 Subject: [PATCH 3/8] feat(#157): Add ROS package --- build/docker/agent/Dockerfile | 5 +- code/perception/launch/perception.launch | 6 +- .../src/perception/lidar_debugger.py | 32 ++++---- .../src/perception/lidar_utility.py | 81 +++++++++++++++---- code/requirements.txt | 1 + 5 files changed, 93 insertions(+), 32 deletions(-) mode change 100644 => 100755 code/perception/src/perception/lidar_debugger.py mode change 100644 => 100755 code/perception/src/perception/lidar_utility.py diff --git a/build/docker/agent/Dockerfile b/build/docker/agent/Dockerfile index 520b76a8..8902e818 100644 --- a/build/docker/agent/Dockerfile +++ b/build/docker/agent/Dockerfile @@ -50,7 +50,7 @@ RUN apt-get update && apt-get install -y \ ros-noetic-ackermann-msgs ros-noetic-derived-object-msgs \ ros-noetic-carla-msgs ros-noetic-pcl-conversions \ ros-noetic-rviz ros-noetic-rqt ros-noetic-pcl-ros ros-noetic-rosbridge-suite ros-noetic-rosbridge-server \ - ros-noetic-smach-ros + ros-noetic-smach-ros ros-noetic-cv-bridge ros-noetic-image-geometry ros-noetic-ros-numpy SHELL ["/bin/bash", "-c"] @@ -138,6 +138,9 @@ COPY --chown=$USERNAME:$USERNAME ./code /workspace/code/ # Link code into catkin workspace RUN ln -s /workspace/code /catkin_ws/src +# Download ROS packages from GitHub +RUN cd src && git clone https://github.com/timdreier/lidar-camera.git + # re-make the catkin workspace RUN source /opt/ros/noetic/setup.bash && catkin_make diff --git a/code/perception/launch/perception.launch b/code/perception/launch/perception.launch index 7ba764ef..ee59d517 100644 --- a/code/perception/launch/perception.launch +++ b/code/perception/launch/perception.launch @@ -1,5 +1,7 @@ - - + + + + diff --git a/code/perception/src/perception/lidar_debugger.py b/code/perception/src/perception/lidar_debugger.py old mode 100644 new mode 100755 index 093e78c7..a5578636 --- a/code/perception/src/perception/lidar_debugger.py +++ b/code/perception/src/perception/lidar_debugger.py @@ -1,27 +1,29 @@ #!/usr/bin/env python import rospy -from std_msgs.msg import String +from sensor_msgs.msg import Image from lidar_utility import LidarUtility - -def callback(data): +class LidarDebugger(): lidar_utility = LidarUtility() - lidar_utility.getDepthAtPoint(0, 0) + def callback(self, data): + #rospy.loginfo(data) + self.lidar_utility.processLidarImage(data) -def listener(): - # In ROS, nodes are uniquely named. If two nodes with the same - # name are launched, the previous one is kicked off. The - # anonymous=True flag means that rospy will choose a unique - # name for our 'listener' node so that multiple listeners can - # run simultaneously. - rospy.init_node('lidar_debugger', anonymous=True) + def listener(self): + # In ROS, nodes are uniquely named. If two nodes with the same + # name are launched, the previous one is kicked off. The + # anonymous=True flag means that rospy will choose a unique + # name for our 'listener' node so that multiple listeners can + # run simultaneously. + rospy.init_node('lidar_debugger', anonymous=True) - rospy.Subscriber("/carla/hero/LIDAR", String, callback) + rospy.Subscriber("/carla/hero/Center/image", Image, self.callback) - # spin() simply keeps python from exiting until this node is stopped - rospy.spin() + # spin() simply keeps python from exiting until this node is stopped + rospy.spin() if __name__ == '__main__': - listener() + lidar_debugger = LidarDebugger() + lidar_debugger.listener() diff --git a/code/perception/src/perception/lidar_utility.py b/code/perception/src/perception/lidar_utility.py old mode 100644 new mode 100755 index 940fe0ec..d97dec71 --- a/code/perception/src/perception/lidar_utility.py +++ b/code/perception/src/perception/lidar_utility.py @@ -1,25 +1,78 @@ +import copy + import rospy -from sensor_msgs.point_cloud2 import PointCloud2 +from sensor_msgs.msg import PointCloud2, CameraInfo, Image import cv2 as cv - +from cv_bridge import CvBridge +import image_geometry +import numpy as np +import sensor_msgs.point_cloud2 as pc2 +import ros_numpy class LidarUtility: + latest_lidar_data = None + latest_camera_info = None + latest_camera_image = None + one_shown = False + + def __init__(self): + rospy.Subscriber("/carla/hero/LIDAR", PointCloud2, self.process_lidar_image) + rospy.Subscriber("/carla/hero/Center/camera_info", CameraInfo, self.process_camera_info) + rospy.Subscriber("/carla/hero/Center/image", Image, self.process_image) + + + def process_lidar_image(self, data): + rospy.loginfo('new lidar data received') + self.latest_lidar_data = ros_numpy.numpify(data) + + def process_camera_info(self, data): + rospy.loginfo('new camera info received') + self.latest_camera_info = data + + def process_image(self, data): + rospy.loginfo('new image received') + self.latest_camera_image = data + def getDepthAtPoint(self, x: int, y: int): print('') def processLidarImage(self, lidar_image): - # https://docs.opencv.org/3.4/d9/d0c/group__calib3d.html#ga1019495a2c8d1743ed5cc23fa0daff8c - - # http://wiki.ros.org/image_geometry (maybe use this?) - cv.projectPoints( - lidar_image.data, - lidar_image.R, - None, - lidar_image.K, - lidar_image.D + if self.one_shown is True: + return + + if type(None) not in (type(self.latest_lidar_data), type(self.process_camera_info), type(self.latest_camera_image)): + rospy.loginfo(self.extract_rgb(self.latest_lidar_data, self.latest_camera_image, self.latest_camera_info)) + + # def getLidarImage(self) -> PointCloud2: + # lidar_data = rospy.wait_for_message("/carla/hero/LIDAR", PointCloud2) + # return lidar_data + + def extract_rgb(self, lidar_data, image, camera_info): + bridge = CvBridge() + camera_object = image_geometry.PinholeCameraModel() + + camera_object.fromCameraInfo(msg=camera_info) + + rvec = np.array([0., 0., 0.]) + tvec = np.array([0., 0., 0.]) + + points = np.array(self.latest_lidar_data.view(np.float32).reshape(self.latest_lidar_data.shape + (-1,)))[:, :3] + points = points.astype('float64') + + img_points, _ = cv.projectPoints( + objectPoints=points, + rvec=rvec, + tvec=tvec, + cameraMatrix=camera_object.intrinsicMatrix(), + distCoeffs=camera_object.distortionCoeffs(), ) - def getLidarImage(self) -> PointCloud2: - lidar_data = rospy.wait_for_message("/carla/hero/LIDAR", PointCloud2) - return lidar_data + print('-----------') + print(img_points) + img = np.zeros((camera_info.height, camera_info.width, 3), np.uint8) + img[img_points[:, 1], img_points[:, 0]] = (0, 0, 255) + self.one_shown = True + + cv.imshow("projected points", img) + cv.waitKey(0) diff --git a/code/requirements.txt b/code/requirements.txt index 8229134d..b49920f1 100644 --- a/code/requirements.txt +++ b/code/requirements.txt @@ -1,3 +1,4 @@ dvc==2.36.0 dvc-gdrive==2.19.1 opencv-python==4.7.0.68 +numpy==1.23 From 477a11ff68a3bfd2dd76883ce7d4f6999864ff20 Mon Sep 17 00:00:00 2001 From: Tim Dreier Date: Mon, 6 Feb 2023 19:37:00 +0100 Subject: [PATCH 4/8] feat(#157): Remove old files --- code/perception/launch/perception.launch | 9 ++- .../src/perception/lidar_debugger.py | 29 ------- .../src/perception/lidar_utility.py | 78 ------------------- code/requirements.txt | 2 - 4 files changed, 5 insertions(+), 113 deletions(-) delete mode 100755 code/perception/src/perception/lidar_debugger.py delete mode 100755 code/perception/src/perception/lidar_utility.py diff --git a/code/perception/launch/perception.launch b/code/perception/launch/perception.launch index ee59d517..03346340 100644 --- a/code/perception/launch/perception.launch +++ b/code/perception/launch/perception.launch @@ -1,7 +1,8 @@ - - - - + + + + + diff --git a/code/perception/src/perception/lidar_debugger.py b/code/perception/src/perception/lidar_debugger.py deleted file mode 100755 index a5578636..00000000 --- a/code/perception/src/perception/lidar_debugger.py +++ /dev/null @@ -1,29 +0,0 @@ -#!/usr/bin/env python -import rospy -from sensor_msgs.msg import Image -from lidar_utility import LidarUtility - -class LidarDebugger(): - lidar_utility = LidarUtility() - - def callback(self, data): - #rospy.loginfo(data) - self.lidar_utility.processLidarImage(data) - - def listener(self): - # In ROS, nodes are uniquely named. If two nodes with the same - # name are launched, the previous one is kicked off. The - # anonymous=True flag means that rospy will choose a unique - # name for our 'listener' node so that multiple listeners can - # run simultaneously. - rospy.init_node('lidar_debugger', anonymous=True) - - rospy.Subscriber("/carla/hero/Center/image", Image, self.callback) - - # spin() simply keeps python from exiting until this node is stopped - rospy.spin() - - -if __name__ == '__main__': - lidar_debugger = LidarDebugger() - lidar_debugger.listener() diff --git a/code/perception/src/perception/lidar_utility.py b/code/perception/src/perception/lidar_utility.py deleted file mode 100755 index d97dec71..00000000 --- a/code/perception/src/perception/lidar_utility.py +++ /dev/null @@ -1,78 +0,0 @@ -import copy - -import rospy -from sensor_msgs.msg import PointCloud2, CameraInfo, Image -import cv2 as cv -from cv_bridge import CvBridge -import image_geometry -import numpy as np -import sensor_msgs.point_cloud2 as pc2 -import ros_numpy - -class LidarUtility: - - latest_lidar_data = None - latest_camera_info = None - latest_camera_image = None - one_shown = False - - def __init__(self): - rospy.Subscriber("/carla/hero/LIDAR", PointCloud2, self.process_lidar_image) - rospy.Subscriber("/carla/hero/Center/camera_info", CameraInfo, self.process_camera_info) - rospy.Subscriber("/carla/hero/Center/image", Image, self.process_image) - - - def process_lidar_image(self, data): - rospy.loginfo('new lidar data received') - self.latest_lidar_data = ros_numpy.numpify(data) - - def process_camera_info(self, data): - rospy.loginfo('new camera info received') - self.latest_camera_info = data - - def process_image(self, data): - rospy.loginfo('new image received') - self.latest_camera_image = data - - def getDepthAtPoint(self, x: int, y: int): - print('') - - def processLidarImage(self, lidar_image): - if self.one_shown is True: - return - - if type(None) not in (type(self.latest_lidar_data), type(self.process_camera_info), type(self.latest_camera_image)): - rospy.loginfo(self.extract_rgb(self.latest_lidar_data, self.latest_camera_image, self.latest_camera_info)) - - # def getLidarImage(self) -> PointCloud2: - # lidar_data = rospy.wait_for_message("/carla/hero/LIDAR", PointCloud2) - # return lidar_data - - def extract_rgb(self, lidar_data, image, camera_info): - bridge = CvBridge() - camera_object = image_geometry.PinholeCameraModel() - - camera_object.fromCameraInfo(msg=camera_info) - - rvec = np.array([0., 0., 0.]) - tvec = np.array([0., 0., 0.]) - - points = np.array(self.latest_lidar_data.view(np.float32).reshape(self.latest_lidar_data.shape + (-1,)))[:, :3] - points = points.astype('float64') - - img_points, _ = cv.projectPoints( - objectPoints=points, - rvec=rvec, - tvec=tvec, - cameraMatrix=camera_object.intrinsicMatrix(), - distCoeffs=camera_object.distortionCoeffs(), - ) - - print('-----------') - print(img_points) - img = np.zeros((camera_info.height, camera_info.width, 3), np.uint8) - img[img_points[:, 1], img_points[:, 0]] = (0, 0, 255) - self.one_shown = True - - cv.imshow("projected points", img) - cv.waitKey(0) diff --git a/code/requirements.txt b/code/requirements.txt index b49920f1..0ab5f86b 100644 --- a/code/requirements.txt +++ b/code/requirements.txt @@ -1,4 +1,2 @@ dvc==2.36.0 dvc-gdrive==2.19.1 -opencv-python==4.7.0.68 -numpy==1.23 From 5a78cd6f6a57b153d1aab51fd165aeecae559f2c Mon Sep 17 00:00:00 2001 From: Tim Dreier Date: Wed, 8 Feb 2023 14:08:43 +0100 Subject: [PATCH 5/8] feat(#157): adapted rviz --- build/docker/agent/Dockerfile | 2 +- code/agent/config/rviz_config.rviz | 41 ++++++++++++++++++++++++++++++ 2 files changed, 42 insertions(+), 1 deletion(-) diff --git a/build/docker/agent/Dockerfile b/build/docker/agent/Dockerfile index 8902e818..e1235e81 100644 --- a/build/docker/agent/Dockerfile +++ b/build/docker/agent/Dockerfile @@ -50,7 +50,7 @@ RUN apt-get update && apt-get install -y \ ros-noetic-ackermann-msgs ros-noetic-derived-object-msgs \ ros-noetic-carla-msgs ros-noetic-pcl-conversions \ ros-noetic-rviz ros-noetic-rqt ros-noetic-pcl-ros ros-noetic-rosbridge-suite ros-noetic-rosbridge-server \ - ros-noetic-smach-ros ros-noetic-cv-bridge ros-noetic-image-geometry ros-noetic-ros-numpy + ros-noetic-smach-ros SHELL ["/bin/bash", "-c"] diff --git a/code/agent/config/rviz_config.rviz b/code/agent/config/rviz_config.rviz index acbbcad5..756590c9 100644 --- a/code/agent/config/rviz_config.rviz +++ b/code/agent/config/rviz_config.rviz @@ -70,6 +70,47 @@ Visualization Manager: PointCloud2: true Value: true Zoom Factor: 1 + - Class: rviz/Image + Enabled: true + Image Topic: /depth_out + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Depth Image + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: true + - Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 1 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Processed Pointcloud + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /processed_points + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true - Alpha: 1 Class: rviz_plugin_tutorials/Imu Color: 204; 51; 204 From 3184daa10426f47871c031aa13ec2da4623ccf61 Mon Sep 17 00:00:00 2001 From: Tim Dreier Date: Wed, 8 Feb 2023 15:15:57 +0100 Subject: [PATCH 6/8] feat(#157): adapted rviz --- code/agent/config/rviz_config.rviz | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/code/agent/config/rviz_config.rviz b/code/agent/config/rviz_config.rviz index 756590c9..cf689093 100644 --- a/code/agent/config/rviz_config.rviz +++ b/code/agent/config/rviz_config.rviz @@ -91,7 +91,7 @@ Visualization Manager: Channel Name: intensity Class: rviz/PointCloud2 Color: 255; 255; 255 - Color Transformer: RGB8 + Color Transformer: BGR Decay Time: 1 Enabled: true Invert Rainbow: false From ff4b1377952df645f4be7923295cbe8fad796aa1 Mon Sep 17 00:00:00 2001 From: Tim Dreier Date: Tue, 21 Mar 2023 19:38:42 +0100 Subject: [PATCH 7/8] feat(#157): Implemented Buffer node --- build/Taskfile | 2 +- build/docker/agent/Dockerfile | 2 +- code/agent/config/dev_objects.json | 2 +- code/agent/config/rviz_config.rviz | 24 ++++++++------ code/perception/launch/perception.launch | 3 ++ code/perception/src/lidar_depth_buffer.py | 38 +++++++++++++++++++++++ code/requirements.txt | 1 + 7 files changed, 59 insertions(+), 13 deletions(-) create mode 100755 code/perception/src/lidar_depth_buffer.py diff --git a/build/Taskfile b/build/Taskfile index ef40a638..070ee45a 100644 --- a/build/Taskfile +++ b/build/Taskfile @@ -32,7 +32,7 @@ task:shell() { ########################################## task:install() { task:install:git_hooks - task:gitconfig:copy + # task:gitconfig:copy install:gpu-support docker:install } diff --git a/build/docker/agent/Dockerfile b/build/docker/agent/Dockerfile index 05b71762..c289efee 100644 --- a/build/docker/agent/Dockerfile +++ b/build/docker/agent/Dockerfile @@ -52,7 +52,7 @@ RUN apt-get update && apt-get install -y \ ros-noetic-ackermann-msgs ros-noetic-derived-object-msgs \ ros-noetic-carla-msgs ros-noetic-pcl-conversions \ ros-noetic-rviz ros-noetic-rqt ros-noetic-pcl-ros ros-noetic-rosbridge-suite ros-noetic-rosbridge-server \ - ros-noetic-robot-pose-ekf \ + ros-noetic-robot-pose-ekf ros-noetic-ros-numpy\ ros-noetic-py-trees-ros ros-noetic-rqt-py-trees ros-noetic-rqt-reconfigure SHELL ["/bin/bash", "-c"] diff --git a/code/agent/config/dev_objects.json b/code/agent/config/dev_objects.json index b7ac1c97..9e904e50 100644 --- a/code/agent/config/dev_objects.json +++ b/code/agent/config/dev_objects.json @@ -107,7 +107,7 @@ "z": 1.70, "roll": 0.0, "pitch": 0.0, - "yaw": 0.0 + "yaw": -90.0 }, "range": 85, "rotation_frequency": 10, diff --git a/code/agent/config/rviz_config.rviz b/code/agent/config/rviz_config.rviz index f2e71a85..6a48c09f 100644 --- a/code/agent/config/rviz_config.rviz +++ b/code/agent/config/rviz_config.rviz @@ -3,9 +3,10 @@ Panels: Help Height: 78 Name: Displays Property Tree Widget: - Expanded: ~ + Expanded: + - /Depth Image1 Splitter Ratio: 0.5 - Tree Height: 417 + Tree Height: 227 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -60,15 +61,17 @@ Visualization Manager: Unreliable: false Value: true Visibility: + Depth Image: true Grid: true Imu: true Path: true PointCloud2: true + Processed Pointcloud: true Value: true Zoom Factor: 1 - Class: rviz/Image Enabled: true - Image Topic: /depth_out + Image Topic: /depth_buffered_out Max Value: 1 Median window: 5 Min Value: 0 @@ -78,7 +81,8 @@ Visualization Manager: Transport Hint: raw Unreliable: false Value: true - - Autocompute Intensity Bounds: true + - Alpha: 1 + Autocompute Intensity Bounds: true Autocompute Value Bounds: Max Value: 10 Min Value: -10 @@ -87,14 +91,12 @@ Visualization Manager: Channel Name: intensity Class: rviz/PointCloud2 Color: 255; 255; 255 - Color Transformer: BGR + Color Transformer: RGB8 Decay Time: 1 Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 4096 Min Color: 0; 0; 0 - Min Intensity: 0 Name: Processed Pointcloud Position Transformer: XYZ Queue Size: 10 @@ -248,12 +250,14 @@ Visualization Manager: Window Geometry: Camera: collapsed: false + Depth Image: + collapsed: false Displays: collapsed: false - Height: 1376 + Height: 1136 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd000000040000000000000304000004c6fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000022a000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d006500720061010000026b000002960000001600ffffff000000010000010f000004c6fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000004c6000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009b80000003efc0100000002fb0000000800540069006d00650100000000000009b80000030700fffffffb0000000800540069006d0065010000000000000450000000000000000000000599000004c600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd000000040000000000000304000003d6fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000016c000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006101000001ad000001b20000001600fffffffb000000160044006500700074006800200049006d0061006700650100000365000000ac0000001600ffffff000000010000010f000003d6fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000003d6000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d00650100000000000007380000030700fffffffb0000000800540069006d0065010000000000000450000000000000000000000319000003d600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -262,6 +266,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 2488 + Width: 1848 X: 1992 Y: 27 diff --git a/code/perception/launch/perception.launch b/code/perception/launch/perception.launch index 59f18031..cb13331b 100644 --- a/code/perception/launch/perception.launch +++ b/code/perception/launch/perception.launch @@ -32,4 +32,7 @@ + + + diff --git a/code/perception/src/lidar_depth_buffer.py b/code/perception/src/lidar_depth_buffer.py new file mode 100755 index 00000000..f953b6c7 --- /dev/null +++ b/code/perception/src/lidar_depth_buffer.py @@ -0,0 +1,38 @@ +#!/usr/bin/env python +import rospy +import ros_numpy +from sensor_msgs.msg import Image + + +class LidarDepthBuffer(): + + last_img = None + + def callback(self, data): + img = ros_numpy.numpify(data) + + rospy.loginfo(self.last_img) + + if self.last_img is not None: + mask = (img > 5000) + self.last_img[mask] = img[mask] + + else: + self.last_img = img + + self.pub_depth.publish( + ros_numpy.msgify(Image, self.last_img, encoding='16UC1') + ) + + def listener(self): + rospy.init_node('lidar_depth_buffer') + + self.pub_depth = rospy.Publisher('/depth_buffered_out', Image) + + rospy.Subscriber("/depth_out", Image, self.callback) + rospy.spin() + + +if __name__ == '__main__': + lidar_depth_buffer = LidarDepthBuffer() + lidar_depth_buffer.listener() diff --git a/code/requirements.txt b/code/requirements.txt index 02c3ece8..30766a4c 100644 --- a/code/requirements.txt +++ b/code/requirements.txt @@ -7,3 +7,4 @@ ruamel.yaml==0.17.21 scipy==1.10.0 xmltodict==0.13.0 py-trees==2.1.6 +numpy==1.23.5 From 78157641f42be671a476f69673a54c916ca177fa Mon Sep 17 00:00:00 2001 From: Tim Dreier Date: Mon, 27 Mar 2023 23:14:43 +0200 Subject: [PATCH 8/8] feat(#157): Documented code --- code/perception/src/lidar_depth_buffer.py | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/code/perception/src/lidar_depth_buffer.py b/code/perception/src/lidar_depth_buffer.py index f953b6c7..8bf3b6b0 100755 --- a/code/perception/src/lidar_depth_buffer.py +++ b/code/perception/src/lidar_depth_buffer.py @@ -5,18 +5,26 @@ class LidarDepthBuffer(): + """ Node to override all Pixels which are greater than a lower limit. + It takes a PointCloud2 message as input and outputs a PointCloud2 + message on which the described method was applied + """ + # Store combined depth map last_img = None def callback(self, data): + # make numpy array from depthmap img = ros_numpy.numpify(data) - rospy.loginfo(self.last_img) - + # create bitmask (True, if distance greater than 5000) + # and override all pixels where the bitmask is True if self.last_img is not None: mask = (img > 5000) self.last_img[mask] = img[mask] + # if we have no hostorical data yet just use the + # values we got else: self.last_img = img