Merge remote-tracking branch 'origin/486-intermediatelayer-main' into…

… 572-lidar-clusters-to-intermediate
una-auxme · Dec 18, 2024 · 7c8d9e2 · 7c8d9e2
2 parents e73d743 + 0889061
commit 7c8d9e2
Show file tree

Hide file tree

Showing 55 changed files with 2,557 additions and 104 deletions.
diff --git a/.vscode/extensions.json b/.vscode/extensions.json
@@ -13,6 +13,7 @@
         "ms-vscode-remote.remote-containers",
         "valentjn.vscode-ltex",
         "ms-python.black-formatter",
-        "augustocdias.tasks-shell-input"
+        "augustocdias.tasks-shell-input",
+        "ktnrg45.vscode-cython"
     ]
 }
diff --git a/.vscode/tasks.json b/.vscode/tasks.json
@@ -4,7 +4,7 @@
         {
             "label": "Run 'catkin_make' in docker container. Choose container from list of running containers.",
             "type": "shell",
-            "command": "docker exec -it ${input:container_name} bash -c 'cd ../../catkin_ws && source /opt/ros/noetic/setup.bash && bash devel/setup.bash && catkin_make'",
+            "command": "docker exec -it ${input:container_name} bash -c 'cd /catkin_ws && source /opt/ros/noetic/setup.bash && source devel/setup.bash && catkin_make'",
             "problemMatcher": [],
             "detail": "Executes 'catkin_make' selected container. Requires Tasks Shell Input Extension, in order to generate the list of containers.",
         }

diff --git a/build/docker-compose.leaderboard.yaml b/build/docker-compose.leaderboard.yaml
@@ -12,7 +12,9 @@ services:
       # Reserved ports for the debugger
       - "53000-53100:53000-53100"
     command: |-
-      bash -c "sleep 10 && sudo chown -R ${USER_UID}:${USER_GID} ../ && \
+      bash -c "\
+      bash -c \"set -e && cd /catkin_ws && catkin_make\" && \
+      sleep 10 && sudo chown -R ${USER_UID}:${USER_GID} ../ && \
       sudo chmod -R a+w ../ && sudo mkdir -p $${XDG_RUNTIME_DIR} && sudo chmod 0700 $${XDG_RUNTIME_DIR} && sudo chown -R ${USER_UID}:${USER_GID} $${XDG_RUNTIME_DIR} && \
       (rqt_console &) && disown -a && \
       python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=0 --routes=$${ROUTE} --agent=/workspace/code/agent/src/agent/agent.py --host=$${CARLA_SIM_HOST} --track=MAP"
diff --git a/build/docker/agent/Dockerfile b/build/docker/agent/Dockerfile
@@ -70,7 +70,8 @@ RUN apt-get update && apt-get install -y \
     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-ros-numpy \
-    ros-noetic-py-trees-ros ros-noetic-rqt-py-trees ros-noetic-rqt-reconfigure
+    ros-noetic-py-trees-ros ros-noetic-rqt-py-trees ros-noetic-rqt-reconfigure \
+    ros-noetic-ros-pytest
 
 SHELL ["/bin/bash", "-c"]
 

diff --git a/code/agent/config/rviz_config.rviz b/code/agent/config/rviz_config.rviz
@@ -3,12 +3,9 @@ Panels:
     Help Height: 78
     Name: Displays
     Property Tree Widget:
-      Expanded:
-        - /PointCloud23
-        - /PointCloud24
-        - /PointCloud25
+      Expanded: ~
       Splitter Ratio: 0.5
-    Tree Height: 308
+    Tree Height: 313
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -26,7 +23,7 @@ Panels:
   - Class: rviz/Time
     Name: Time
     SyncMode: 0
-    SyncSource: PointCloud2
+    SyncSource: Lidar
 Preferences:
   PromptSaveOnExit: true
 Toolbars:
@@ -65,27 +62,26 @@ Visualization Manager:
       Visibility:
         Grid: false
         Imu: false
+        Intermediate layer: true
+        Lidar: false
+        Lidar_filtered: false
         Path: false
-        PointCloud2: false
+        Radar: false
         Value: true
+        VisonNode Output: true
       Zoom Factor: 1
     - Class: rviz/Image
       Enabled: true
-      Image Rendering: background and overlay
       Image Topic: /paf/hero/Center/segmented_image
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
       Name: VisonNode Output
-      Overlay Alpha: 0.5
+      Normalize Range: true
       Queue Size: 2
       Transport Hint: raw
       Unreliable: false
       Value: true
-      Visibility:
-        Grid: true
-        Imu: true
-        Path: true
-        PointCloud2: true
-        Value: true
-      Zoom Factor: 1
     - Alpha: 1
       Class: rviz_plugin_tutorials/Imu
       Color: 204; 51; 204
@@ -99,26 +95,26 @@ Visualization Manager:
     - Alpha: 1
       Autocompute Intensity Bounds: true
       Autocompute Value Bounds:
-        Max Value: 10
-        Min Value: -10
+        Max Value: 83.1390609741211
+        Min Value: -66.25593566894531
         Value: true
-      Axis: Z
+      Axis: X
       Channel Name: intensity
       Class: rviz/PointCloud2
-      Color: 255; 255; 255
-      Color Transformer: Intensity
+      Color: 150; 150; 150
+      Color Transformer: FlatColor
       Decay Time: 0
       Enabled: true
       Invert Rainbow: false
       Max Color: 255; 255; 255
       Min Color: 0; 0; 0
-      Name: PointCloud2
+      Name: Lidar
       Position Transformer: XYZ
-      Queue Size: 10
+      Queue Size: 2
       Selectable: true
-      Size (Pixels): 3
+      Size (Pixels): 2
       Size (m): 0.009999999776482582
-      Style: Flat Squares
+      Style: Points
       Topic: /carla/hero/LIDAR
       Unreliable: false
       Use Fixed Frame: true
@@ -133,25 +129,25 @@ Visualization Manager:
       Axis: Z
       Channel Name: intensity
       Class: rviz/PointCloud2
-      Color: 255; 255; 255
-      Color Transformer: Intensity
+      Color: 255; 100; 100
+      Color Transformer: FlatColor
       Decay Time: 0
-      Enabled: false
+      Enabled: true
       Invert Rainbow: false
       Max Color: 255; 255; 255
       Min Color: 0; 0; 0
-      Name: PointCloud2
+      Name: Radar
       Position Transformer: XYZ
-      Queue Size: 10
+      Queue Size: 2
       Selectable: true
-      Size (Pixels): 3
+      Size (Pixels): 5
       Size (m): 0.009999999776482582
-      Style: Flat Squares
+      Style: Points
       Topic: /carla/hero/RADAR
       Unreliable: false
       Use Fixed Frame: true
       Use rainbow: true
-      Value: false
+      Value: true
     - Alpha: 1
       Buffer Length: 1
       Class: rviz/Path
@@ -186,13 +182,13 @@ Visualization Manager:
       Channel Name: intensity
       Class: rviz/PointCloud2
       Color: 255; 255; 255
-      Color Transformer: Intensity
+      Color Transformer: ""
       Decay Time: 0
-      Enabled: true
+      Enabled: false
       Invert Rainbow: false
       Max Color: 255; 255; 255
       Min Color: 0; 0; 0
-      Name: PointCloud2
+      Name: Lidar_filtered
       Position Transformer: XYZ
       Queue Size: 10
       Selectable: true
@@ -203,62 +199,14 @@ Visualization Manager:
       Unreliable: false
       Use Fixed Frame: true
       Use rainbow: true
-      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: Intensity
-      Decay Time: 0
-      Enabled: true
-      Invert Rainbow: false
-      Max Color: 255; 255; 255
-      Min Color: 0; 0; 0
-      Name: PointCloud2
-      Position Transformer: XYZ
-      Queue Size: 10
-      Selectable: true
-      Size (Pixels): 3
-      Size (m): 0.20000000298023224
-      Style: Flat Squares
-      Topic: /carla/hero/LIDAR_filtered_rear_left
-      Unreliable: false
-      Use Fixed Frame: true
-      Use rainbow: true
-      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: Intensity
-      Decay Time: 0
+      Value: false
+    - Class: rviz/MarkerArray
       Enabled: true
-      Invert Rainbow: false
-      Max Color: 255; 255; 255
-      Min Color: 0; 0; 0
-      Name: PointCloud2
-      Position Transformer: XYZ
-      Queue Size: 10
-      Selectable: true
-      Size (Pixels): 3
-      Size (m): 0.20000000298023224
-      Style: Flat Squares
-      Topic: /carla/hero/LIDAR_filtered_rear_right
-      Unreliable: false
-      Use Fixed Frame: true
-      Use rainbow: true
+      Marker Topic: /paf/hero/mapping/marker_array
+      Name: Intermediate layer
+      Namespaces:
+        m: true
+      Queue Size: 2
       Value: true
   Enabled: true
   Global Options:
@@ -288,7 +236,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 34.785499572753906
+      Distance: 22.39063835144043
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -304,19 +252,19 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.19039836525917053
+      Pitch: 0.8247969746589661
       Target Frame: <Fixed Frame>
-      Yaw: 4.520427227020264
+      Yaw: 2.7423195838928223
     Saved: ~
 Window Geometry:
   Camera:
     collapsed: false
   Displays:
     collapsed: false
-  Height: 1376
+  Height: 1371
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd000000040000000000000304000004c6fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000022a000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d006500720061010000026b000002960000001600ffffff000000010000010f000004c6fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000004c6000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009b80000003efc0100000002fb0000000800540069006d00650100000000000009b80000030700fffffffb0000000800540069006d0065010000000000000450000000000000000000000599000004c600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd000000040000000000000304000004c1fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730200000690000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000001c2000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006101000002030000021e0000001600fffffffb00000020005600690073006f006e004e006f006400650020004f007500740070007500740100000427000000d50000001600ffffff000000010000010f000004c1fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000004c1000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009be0000003efc0100000002fb0000000800540069006d00650100000000000009be0000030700fffffffb0000000800540069006d006501000000000000045000000000000000000000059f000004c100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -325,6 +273,8 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: false
-  Width: 2488
-  X: 1992
-  Y: 27
+  VisonNode Output:
+    collapsed: false
+  Width: 2494
+  X: 2626
+  Y: 32
diff --git a/code/agent/launch/agent.launch b/code/agent/launch/agent.launch
@@ -25,5 +25,13 @@
     <include file="$(find debugging)/launch/debugging.launch">
     </include>
 
+    <!-- mapping -->
+    <include file="$(find mapping)/launch/mapping.launch">
+    </include>
+
+    <!-- mapping visualization -->
+    <include file="$(find mapping_visualization)/launch/visualization.launch">
+    </include>
+
     <node type="rviz" name="rviz" pkg="rviz" args="-d $(find agent)/config/rviz_config.rviz" />
 </launch>