feat(#157): Started lidar mapping

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
 }

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
 

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

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

code/perception/launch/perception.launch

@@ -26,4 +26,13 @@
     <param name="debug" value="true"/>
     <param name="self_diagnose" value="true"/>
   </node>
+
+  <node pkg="lidar_camera" name="lidar_camera" type="lidar_camera_node" output="screen" required="true">
+    <remap from="cloud_in" to="/carla/hero/LIDAR"/>
+    <remap from="image_in" to="/carla/hero/Center/image"/>
+    <remap from="camera_info" to="/carla/hero/Center/camera_info"/>
+  </node>
+
+  <node pkg="perception" type="lidar_depth_buffer.py" name="lidar_depth_buffer" output="screen">
+  </node>
 </launch>

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()

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