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 efdcc336..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"] @@ -142,6 +142,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/agent/config/dev_objects.json b/code/agent/config/dev_objects.json index c0817bd2..9e904e50 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,26 +37,77 @@ "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 + "yaw": -90.0 }, "range": 85, "rotation_frequency": 10, @@ -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 diff --git a/code/agent/config/rviz_config.rviz b/code/agent/config/rviz_config.rviz index 1385e6dc..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,12 +61,54 @@ 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_buffered_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 + - Alpha: 1 + 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 + Min Color: 0; 0; 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 @@ -207,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: @@ -221,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 55b89747..cb13331b 100644 --- a/code/perception/launch/perception.launch +++ b/code/perception/launch/perception.launch @@ -26,4 +26,13 @@ + + + + + + + + + diff --git a/code/perception/src/lidar_depth_buffer.py b/code/perception/src/lidar_depth_buffer.py new file mode 100755 index 00000000..8bf3b6b0 --- /dev/null +++ b/code/perception/src/lidar_depth_buffer.py @@ -0,0 +1,46 @@ +#!/usr/bin/env python +import rospy +import ros_numpy +from sensor_msgs.msg import Image + + +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) + + # 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 + + 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