From 4915d277987e1d9b5bb94fa1f6fb540ee8b2557b Mon Sep 17 00:00:00 2001
From: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com>
Date: Mon, 13 Nov 2023 08:46:17 +0100
Subject: [PATCH] add ZED urdf + list of choices (#49)

* add ZED urdf + list of choices

* choices to root dir

* readme

* Update README.md

* Update README.md

* fix gz bridge

* Remapping

* Add word

* add suggestions

* add urdf tests

* Add None and velodyne to test

* Update pre-commit and fix tests
---
 .flake8                                       |   6 +
 .gitignore                                    |   1 -
 .pre-commit-config.yaml                       |   8 +-
 .wordlist.txt                                 |   2 +
 README.md                                     |  24 +-
 rosbot_xl_bringup/launch/bringup.launch.py    |  64 +++--
 .../test/test_diff_drive_ekf_and_scan.py      |  12 +-
 rosbot_xl_bringup/test/test_flake8.py         |   4 +-
 .../test/test_mecanum_ekf_and_scan.py         |  12 +-
 .../launch/controller.launch.py               | 120 ++++-----
 .../test/test_diff_drive_controllers.py       |  12 +-
 rosbot_xl_controller/test/test_flake8.py      |   4 +-
 .../test/test_mecanum_controllers.py          |  12 +-
 rosbot_xl_controller/test/test_xacro.py       |  21 +-
 .../urdf/rosbot_xl.urdf.xacro                 |  42 +++-
 rosbot_xl_gazebo/launch/simulation.launch.py  | 232 ++++++++++++------
 .../test/test_diff_drive_simulation.py        |  12 +-
 rosbot_xl_gazebo/test/test_flake8.py          |   4 +-
 .../test/test_mecanum_simulation.py           |  12 +-
 .../test/test_slamtec_rplidar_a2.py           |  12 +-
 .../test/test_slamtec_rplidar_a3.py           |  12 +-
 .../test/test_slamtec_rplidar_s1.py           |  12 +-
 22 files changed, 395 insertions(+), 245 deletions(-)
 create mode 100644 .flake8

diff --git a/.flake8 b/.flake8
new file mode 100644
index 00000000..1b7fbcb2
--- /dev/null
+++ b/.flake8
@@ -0,0 +1,6 @@
+[flake8]
+extend-ignore = E203, B902,C816,D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404,I202
+import-order-style = google
+max-line-length = 99
+show-source = true
+statistics = true
diff --git a/.gitignore b/.gitignore
index 8c2d2411..c57e18a4 100644
--- a/.gitignore
+++ b/.gitignore
@@ -11,7 +11,6 @@ install/
 log/
 __pycache__/
 
-
 # ROSbots submodules
 rosbot_hardware_interfaces/
 ros_components_description/
diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml
index a3c855e1..5743d209 100644
--- a/.pre-commit-config.yaml
+++ b/.pre-commit-config.yaml
@@ -16,7 +16,7 @@ repos:
             args: [--pytest-test-first]
 
     - repo: https://github.com/codespell-project/codespell
-      rev: v1.16.0
+      rev: v2.2.6
       hooks:
           - id: codespell
             name: codespell
@@ -26,13 +26,13 @@ repos:
             types: [text]
 
     - repo: https://github.com/jumanjihouse/pre-commit-hook-yamlfmt
-      rev: 0.2.1
+      rev: 0.2.3
       hooks:
           - id: yamlfmt
             files: ^.github|./\.yaml
 
     - repo: https://github.com/psf/black
-      rev: 23.10.1
+      rev: 23.11.0
       hooks:
         - id: black
           args: ["--line-length=99", "--preview"]
@@ -41,7 +41,7 @@ repos:
       rev: 6.1.0
       hooks:
         - id: flake8
-          args: ["--ignore=E501,W503"] # ignore too long line and line break before binary operator,
+          args: ["--ignore=E501,W503,E203"] # ignore too long line and line break before binary operator,
                                        # black checks it
 
     - repo: local
diff --git a/.wordlist.txt b/.wordlist.txt
index 4b632be8..6b7d0f8d 100644
--- a/.wordlist.txt
+++ b/.wordlist.txt
@@ -62,6 +62,8 @@ gazebosim
 msgs
 nav
 pytest
+FIXME
+TODO
 nan
 rclpy
 utils
diff --git a/README.md b/README.md
index 68e44a76..5e402d8b 100644
--- a/README.md
+++ b/README.md
@@ -42,17 +42,15 @@ To run the software on real ROSbot XL, also communication with Digital Board wil
 First update your firmware to make sure that you use the latest version, then run the `micro-ROS` agent.
 For detailed instructions refer to the [rosbot_xl_firmware repository](https://github.com/husarion/rosbot_xl_firmware).
 
-## Source build
+## Prepare environment
 
-### Prerequisites
-
-Install `colcon`, `vsc` and `rosdep`:
+1. **Install `colcon`, `vsc` and `rosdep`**
 ```
 sudo apt-get update
 sudo apt-get install -y python3-colcon-common-extensions python3-vcstool python3-rosdep
 ```
 
-Create workspace folder and clone `rosbot_xl_ros` repository:
+2. **Create workspace folder and clone `rosbot_xl_ros` repository:**
 ```
 mkdir -p ros2_ws/src
 cd ros2_ws
@@ -61,7 +59,7 @@ git clone https://github.com/husarion/rosbot_xl_ros src/
 
 ### Build and run on hardware
 
-Building:
+1. **Building**
 ```
 export HUSARION_ROS_BUILD=hardware
 
@@ -77,11 +75,10 @@ rosdep install -i --from-path src --rosdistro $ROS_DISTRO -y
 colcon build
 ```
 
-> **Prerequisites**
->
-> Before starting the software on the robot please make sure that you're using the latest firmware and run the `micro-ROS` agent (as described in the *Usage on hardware* step).
+> [!NOTE]
+> Before starting the software on the robot please make sure that you're using the latest firmware and run the `micro-ROS` agent as described in the [Usage on hardware](#usage-on-hardware) step.
 
-Running:
+2. **Running**
 ```
 source install/setup.bash
 ros2 launch rosbot_xl_bringup bringup.launch.py
@@ -89,7 +86,7 @@ ros2 launch rosbot_xl_bringup bringup.launch.py
 
 ### Build and run Gazebo simulation
 
-Building:
+1. **Building**
 ```
 export HUSARION_ROS_BUILD=simulation
 
@@ -104,7 +101,7 @@ rosdep install -i --from-path src --rosdistro $ROS_DISTRO -y
 colcon build
 ```
 
-Running:
+2. **Running**
 ```
 source install/setup.bash
 ros2 launch rosbot_xl_gazebo simulation.launch.py
@@ -123,6 +120,9 @@ pre-commit install
 
 # manually run tests
 pre-commit run -a
+
+# update revision
+pre-commit autoupdate
 ```
 
 After initialization [pre-commit configuration](.pre-commit-config.yaml) will applied on every commit.
diff --git a/rosbot_xl_bringup/launch/bringup.launch.py b/rosbot_xl_bringup/launch/bringup.launch.py
index 08b7d3a8..b3c4a006 100644
--- a/rosbot_xl_bringup/launch/bringup.launch.py
+++ b/rosbot_xl_bringup/launch/bringup.launch.py
@@ -35,18 +35,36 @@ def generate_launch_description():
         ),
     )
 
-    lidar_model = LaunchConfiguration("lidar_model")
-    declare_lidar_model_arg = DeclareLaunchArgument(
-        "lidar_model",
-        default_value="slamtec_rplidar_s1",
-        description="Lidar model added to the URDF",
-    )
-
     camera_model = LaunchConfiguration("camera_model")
     declare_camera_model_arg = DeclareLaunchArgument(
         "camera_model",
         default_value="None",
-        description="Camera model added to the URDF",
+        description="Add camera model to the robot URDF",
+        choices=[
+            "None",
+            "intel_realsense_d435",
+            "stereolabs_zed",
+            "stereolabs_zedm",
+            "stereolabs_zed2",
+            "stereolabs_zed2i",
+            "stereolabs_zedx",
+            "stereolabs_zedxm",
+        ],
+    )
+
+    lidar_model = LaunchConfiguration("lidar_model")
+    declare_lidar_model_arg = DeclareLaunchArgument(
+        "lidar_model",
+        default_value="slamtec_rplidar_s1",
+        description="Add LiDAR model to the robot URDF",
+        choices=[
+            "None",
+            "ouster_os1_32",
+            "slamtec_rplidar_a2",
+            "slamtec_rplidar_a3",
+            "slamtec_rplidar_s1",
+            "velodyne_puck",
+        ],
     )
 
     include_camera_mount = LaunchConfiguration("include_camera_mount")
@@ -72,13 +90,11 @@ def generate_launch_description():
 
     controller_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
-            PathJoinSubstitution(
-                [
-                    get_package_share_directory("rosbot_xl_controller"),
-                    "launch",
-                    "controller.launch.py",
-                ]
-            )
+            PathJoinSubstitution([
+                get_package_share_directory("rosbot_xl_controller"),
+                "launch",
+                "controller.launch.py",
+            ])
         ),
         launch_arguments={
             "mecanum": mecanum,
@@ -96,9 +112,9 @@ def generate_launch_description():
         name="ekf_filter_node",
         output="screen",
         parameters=[
-            PathJoinSubstitution(
-                [get_package_share_directory("rosbot_xl_bringup"), "config", "ekf.yaml"]
-            )
+            PathJoinSubstitution([
+                get_package_share_directory("rosbot_xl_bringup"), "config", "ekf.yaml"
+            ])
         ],
     )
 
@@ -106,13 +122,11 @@ def generate_launch_description():
         package="laser_filters",
         executable="scan_to_scan_filter_chain",
         parameters=[
-            PathJoinSubstitution(
-                [
-                    get_package_share_directory("rosbot_xl_bringup"),
-                    "config",
-                    "laser_filter.yaml",
-                ]
-            )
+            PathJoinSubstitution([
+                get_package_share_directory("rosbot_xl_bringup"),
+                "config",
+                "laser_filter.yaml",
+            ])
         ],
     )
 
diff --git a/rosbot_xl_bringup/test/test_diff_drive_ekf_and_scan.py b/rosbot_xl_bringup/test/test_diff_drive_ekf_and_scan.py
index 16c27a54..05ae8fb9 100644
--- a/rosbot_xl_bringup/test/test_diff_drive_ekf_and_scan.py
+++ b/rosbot_xl_bringup/test/test_diff_drive_ekf_and_scan.py
@@ -31,13 +31,11 @@ def generate_test_description():
     rosbot_xl_bringup = get_package_share_directory("rosbot_xl_bringup")
     bringup_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
-            PathJoinSubstitution(
-                [
-                    rosbot_xl_bringup,
-                    "launch",
-                    "bringup.launch.py",
-                ]
-            )
+            PathJoinSubstitution([
+                rosbot_xl_bringup,
+                "launch",
+                "bringup.launch.py",
+            ])
         ),
         launch_arguments={
             "use_sim": "False",
diff --git a/rosbot_xl_bringup/test/test_flake8.py b/rosbot_xl_bringup/test/test_flake8.py
index 49c1644f..40602d8a 100644
--- a/rosbot_xl_bringup/test/test_flake8.py
+++ b/rosbot_xl_bringup/test/test_flake8.py
@@ -13,11 +13,13 @@
 # limitations under the License.
 
 from ament_flake8.main import main_with_errors
+from os.path import join, dirname
 import pytest
 
 
 @pytest.mark.flake8
 @pytest.mark.linter
 def test_flake8():
-    rc, errors = main_with_errors(argv=[])
+    config_file = join(dirname(dirname(dirname(__file__))), ".flake8")
+    rc, errors = main_with_errors(argv=["--config", config_file])
     assert rc == 0, "Found %d code style errors / warnings:\n" % len(errors) + "\n".join(errors)
diff --git a/rosbot_xl_bringup/test/test_mecanum_ekf_and_scan.py b/rosbot_xl_bringup/test/test_mecanum_ekf_and_scan.py
index e6e60f12..3424785e 100644
--- a/rosbot_xl_bringup/test/test_mecanum_ekf_and_scan.py
+++ b/rosbot_xl_bringup/test/test_mecanum_ekf_and_scan.py
@@ -31,13 +31,11 @@ def generate_test_description():
     rosbot_xl_bringup = get_package_share_directory("rosbot_xl_bringup")
     bringup_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
-            PathJoinSubstitution(
-                [
-                    rosbot_xl_bringup,
-                    "launch",
-                    "bringup.launch.py",
-                ]
-            )
+            PathJoinSubstitution([
+                rosbot_xl_bringup,
+                "launch",
+                "bringup.launch.py",
+            ])
         ),
         launch_arguments={
             "use_sim": "False",
diff --git a/rosbot_xl_controller/launch/controller.launch.py b/rosbot_xl_controller/launch/controller.launch.py
index 1db83353..f06df1e5 100644
--- a/rosbot_xl_controller/launch/controller.launch.py
+++ b/rosbot_xl_controller/launch/controller.launch.py
@@ -41,18 +41,36 @@ def generate_launch_description():
         ),
     )
 
-    lidar_model = LaunchConfiguration("lidar_model")
-    declare_lidar_model_arg = DeclareLaunchArgument(
-        "lidar_model",
-        default_value="slamtec_rplidar_s1",
-        description="Lidar model added to the URDF",
-    )
-
     camera_model = LaunchConfiguration("camera_model")
     declare_camera_model_arg = DeclareLaunchArgument(
         "camera_model",
         default_value="None",
-        description="Camera model added to the URDF",
+        description="Add camera model to the robot URDF",
+        choices=[
+            "None",
+            "intel_realsense_d435",
+            "stereolabs_zed",
+            "stereolabs_zedm",
+            "stereolabs_zed2",
+            "stereolabs_zed2i",
+            "stereolabs_zedx",
+            "stereolabs_zedxm",
+        ],
+    )
+
+    lidar_model = LaunchConfiguration("lidar_model")
+    declare_lidar_model_arg = DeclareLaunchArgument(
+        "lidar_model",
+        default_value="None",
+        description="Add LiDAR model to the robot URDF",
+        choices=[
+            "None",
+            "ouster_os1_32",
+            "slamtec_rplidar_a2",
+            "slamtec_rplidar_a3",
+            "slamtec_rplidar_s1",
+            "velodyne_puck",
+        ],
     )
 
     include_camera_mount = LaunchConfiguration("include_camera_mount")
@@ -76,58 +94,48 @@ def generate_launch_description():
         description="Which simulation engine will be used",
     )
 
-    controller_config_name = PythonExpression(
-        [
-            "'mecanum_drive_controller.yaml' if ",
-            mecanum,
-            " else 'diff_drive_controller.yaml'",
-        ]
-    )
+    controller_config_name = PythonExpression([
+        "'mecanum_drive_controller.yaml' if ",
+        mecanum,
+        " else 'diff_drive_controller.yaml'",
+    ])
 
-    robot_controllers = PathJoinSubstitution(
-        [
-            FindPackageShare("rosbot_xl_controller"),
-            "config",
-            controller_config_name,
-        ]
-    )
+    robot_controllers = PathJoinSubstitution([
+        FindPackageShare("rosbot_xl_controller"),
+        "config",
+        controller_config_name,
+    ])
 
-    controller_manager_name = PythonExpression(
-        [
-            "'/simulation_controller_manager' if ",
-            use_sim,
-            " else '/controller_manager'",
-        ]
-    )
+    controller_manager_name = PythonExpression([
+        "'/simulation_controller_manager' if ",
+        use_sim,
+        " else '/controller_manager'",
+    ])
 
     # Get URDF via xacro
-    robot_description_content = Command(
-        [
-            PathJoinSubstitution([FindExecutable(name="xacro")]),
-            " ",
-            PathJoinSubstitution(
-                [
-                    FindPackageShare("rosbot_xl_description"),
-                    "urdf",
-                    "rosbot_xl.urdf.xacro",
-                ]
-            ),
-            " mecanum:=",
-            mecanum,
-            " lidar_model:=",
-            lidar_model,
-            " camera_model:=",
-            camera_model,
-            " include_camera_mount:=",
-            include_camera_mount,
-            " use_sim:=",
-            use_sim,
-            " simulation_engine:=",
-            simulation_engine,
-            " simulation_controllers_config_file:=",
-            robot_controllers,
-        ]
-    )
+    robot_description_content = Command([
+        PathJoinSubstitution([FindExecutable(name="xacro")]),
+        " ",
+        PathJoinSubstitution([
+            FindPackageShare("rosbot_xl_description"),
+            "urdf",
+            "rosbot_xl.urdf.xacro",
+        ]),
+        " mecanum:=",
+        mecanum,
+        " lidar_model:=",
+        lidar_model,
+        " camera_model:=",
+        camera_model,
+        " include_camera_mount:=",
+        include_camera_mount,
+        " use_sim:=",
+        use_sim,
+        " simulation_engine:=",
+        simulation_engine,
+        " simulation_controllers_config_file:=",
+        robot_controllers,
+    ])
     robot_description = {"robot_description": robot_description_content}
 
     control_node = Node(
diff --git a/rosbot_xl_controller/test/test_diff_drive_controllers.py b/rosbot_xl_controller/test/test_diff_drive_controllers.py
index 139dad8a..86c9b745 100644
--- a/rosbot_xl_controller/test/test_diff_drive_controllers.py
+++ b/rosbot_xl_controller/test/test_diff_drive_controllers.py
@@ -31,13 +31,11 @@ def generate_test_description():
     rosbot_xl_controller = get_package_share_directory("rosbot_xl_controller")
     bringup_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
-            PathJoinSubstitution(
-                [
-                    rosbot_xl_controller,
-                    "launch",
-                    "controller.launch.py",
-                ]
-            )
+            PathJoinSubstitution([
+                rosbot_xl_controller,
+                "launch",
+                "controller.launch.py",
+            ])
         ),
         launch_arguments={
             "use_sim": "False",
diff --git a/rosbot_xl_controller/test/test_flake8.py b/rosbot_xl_controller/test/test_flake8.py
index 49c1644f..40602d8a 100644
--- a/rosbot_xl_controller/test/test_flake8.py
+++ b/rosbot_xl_controller/test/test_flake8.py
@@ -13,11 +13,13 @@
 # limitations under the License.
 
 from ament_flake8.main import main_with_errors
+from os.path import join, dirname
 import pytest
 
 
 @pytest.mark.flake8
 @pytest.mark.linter
 def test_flake8():
-    rc, errors = main_with_errors(argv=[])
+    config_file = join(dirname(dirname(dirname(__file__))), ".flake8")
+    rc, errors = main_with_errors(argv=["--config", config_file])
     assert rc == 0, "Found %d code style errors / warnings:\n" % len(errors) + "\n".join(errors)
diff --git a/rosbot_xl_controller/test/test_mecanum_controllers.py b/rosbot_xl_controller/test/test_mecanum_controllers.py
index be687b3d..69df35d9 100644
--- a/rosbot_xl_controller/test/test_mecanum_controllers.py
+++ b/rosbot_xl_controller/test/test_mecanum_controllers.py
@@ -31,13 +31,11 @@ def generate_test_description():
     rosbot_xl_controller = get_package_share_directory("rosbot_xl_controller")
     bringup_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
-            PathJoinSubstitution(
-                [
-                    rosbot_xl_controller,
-                    "launch",
-                    "controller.launch.py",
-                ]
-            )
+            PathJoinSubstitution([
+                rosbot_xl_controller,
+                "launch",
+                "controller.launch.py",
+            ])
         ),
         launch_arguments={
             "use_sim": "False",
diff --git a/rosbot_xl_controller/test/test_xacro.py b/rosbot_xl_controller/test/test_xacro.py
index 84caf38a..a417a335 100644
--- a/rosbot_xl_controller/test/test_xacro.py
+++ b/rosbot_xl_controller/test/test_xacro.py
@@ -23,8 +23,23 @@ def test_rosbot_description_parsing():
     use_sim_values = ["true", "false"]
     use_gpu_values = ["true", "false"]
     simulation_engine_values = ["ignition-gazebo", "webots"]  # 'gazebo-classic'
-    lidar_model = ["slamtec_rplidar_s1", "slamtec_rplidar_a2", "slamtec_rplidar_a3"]
-    camera_model = ["intel_realsense_d435"]
+    lidar_model = [
+        "None",
+        "slamtec_rplidar_s1",
+        "slamtec_rplidar_a2",
+        "slamtec_rplidar_a3",
+        "velodyne_puck",
+    ]
+    camera_model = [
+        "None",
+        "intel_realsense_d435",
+        "stereolabs_zed",
+        "stereolabs_zedm",
+        "stereolabs_zed2",
+        "stereolabs_zed2i",
+        "stereolabs_zedx",
+        "stereolabs_zedxm",
+    ]
 
     all_combinations = list(
         itertools.product(
@@ -62,5 +77,5 @@ def test_rosbot_description_parsing():
             assert False, (
                 f"xacro parsing failed: {str(e)} for mecanum: {mecanum}, "
                 f"use_sim: {use_sim}, use_gpu: {use_gpu}, simulation_engine: {simulation_engine}, "
-                f"lidar_model: {lidar_model}, camera_model{camera_model}"
+                f"lidar_model: {lidar_model}, camera_model: {camera_model}"
             )
diff --git a/rosbot_xl_description/urdf/rosbot_xl.urdf.xacro b/rosbot_xl_description/urdf/rosbot_xl.urdf.xacro
index e510f571..44a0dc5e 100644
--- a/rosbot_xl_description/urdf/rosbot_xl.urdf.xacro
+++ b/rosbot_xl_description/urdf/rosbot_xl.urdf.xacro
@@ -3,7 +3,7 @@
 
   <xacro:arg name="mecanum" default="false" />
   <xacro:arg name="lidar_model" default="slamtec_rplidar_s1" />
-  <xacro:arg name="camera_model" default="None" />
+  <xacro:arg name="camera_model" default="intel_realsense_d435" />
   <xacro:arg name="include_camera_mount" default="false" />
   <xacro:arg name="use_sim" default="false" />
   <xacro:arg name="simulation_engine" default="ignition-gazebo" />
@@ -28,7 +28,7 @@
 
   <xacro:property name="lidar_model" value="$(arg lidar_model)" />
   <xacro:property name="lidar_parent_link" value="cover_link" />
-  <xacro:property name="lidar_xyz" value="0.0 0.0 0.0" />
+  <xacro:property name="lidar_xyz" value="0.02 0.0 0.0" />
   <xacro:property name="lidar_rpy" value="0.0 0.0 0.0" />
   <xacro:property name="lidar_use_gpu" value="true" />
   <!-- use_gpu has to be set to true, CPU lidar doesn't work in ignition -
@@ -102,10 +102,44 @@
       simulation_engine="$(arg simulation_engine)" />
   </xacro:if>
 
-  <!-- INCLUDE CAMERA MOUNT -->
+  <xacro:if value="${camera_model.startswith('stereolabs')}">
+    <xacro:if value="${camera_model == 'stereolabs_zed'}">
+      <xacro:property name="camera_model_type" value="zed" />
+    </xacro:if>
+    <xacro:if value="${camera_model == 'stereolabs_zedm'}">
+      <xacro:property name="camera_model_type" value="zedm" />
+    </xacro:if>
+    <xacro:if value="${camera_model == 'stereolabs_zed2'}">
+      <xacro:property name="camera_model_type" value="zed2" />
+    </xacro:if>
+    <xacro:if value="${camera_model == 'stereolabs_zed2i'}">
+      <xacro:property name="camera_model_type" value="zed2i" />
+    </xacro:if>
+    <xacro:if value="${camera_model == 'stereolabs_zedx'}">
+      <xacro:property name="camera_model_type" value="zedx" />
+    </xacro:if>
+    <xacro:if value="${camera_model == 'stereolabs_zedxm'}">
+      <xacro:property name="camera_model_type" value="zedxm" />
+    </xacro:if>
+
+    <xacro:include
+    filename="$(find ros_components_description)/urdf/stereolabs_zed.urdf.xacro"
+    ns="camera" />
+    <xacro:camera.zed_camera
+      parent_link="${camera_parent_link}"
+      xyz="${camera_xyz}"
+      rpy="${camera_rpy}"
+      model="${camera_model_type}"
+      name="camera"
+      topic="camera"
+      use_nominal_extrinsics="$(arg use_sim)"
+      simulation_engine="$(arg simulation_engine)" />
+  </xacro:if>
+
+  <!-- INCLUDE CAMERA MOUNT - add if camera_model is not empty -->
 
   <xacro:property name="include_camera_mount" value="$(arg include_camera_mount)" />
-  <xacro:if value="${include_camera_mount or camera_model == 'intel_realsense_d435'}">
+  <xacro:if value="${include_camera_mount or camera_model != 'None'}">
     <xacro:include filename="$(find rosbot_xl_description)/urdf/components/camera_mount.urdf.xacro"
       ns="camera_mount" />
     <xacro:camera_mount.camera_mount
diff --git a/rosbot_xl_gazebo/launch/simulation.launch.py b/rosbot_xl_gazebo/launch/simulation.launch.py
index 83e23d43..a84812f4 100644
--- a/rosbot_xl_gazebo/launch/simulation.launch.py
+++ b/rosbot_xl_gazebo/launch/simulation.launch.py
@@ -12,10 +12,11 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-from launch import LaunchDescription
+from launch import LaunchDescription, LaunchContext
 from launch.actions import (
     IncludeLaunchDescription,
     DeclareLaunchArgument,
+    OpaqueFunction,
 )
 from launch.substitutions import (
     PathJoinSubstitution,
@@ -29,6 +30,106 @@
 from ament_index_python.packages import get_package_share_directory
 
 
+def launch_gz_bridge(context: LaunchContext, *args, **kwargs):
+    camera_model = context.perform_substitution(LaunchConfiguration("camera_model"))
+    lidar_model = context.perform_substitution(LaunchConfiguration("lidar_model"))
+
+    gz_args = ["/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock"]
+    gz_remapping = []
+    depth_camera_parent_tf = None
+
+    # Add camera topic
+    if camera_model.startswith("intel_realsense"):
+        gz_args.append(
+            "/camera/color/camera_info@sensor_msgs/msg/CameraInfo[ignition.msgs.CameraInfo"
+        )
+        gz_args.append("/camera/color/image_raw@sensor_msgs/msg/Image[ignition.msgs.Image")
+        gz_args.append("/camera/camera_info@sensor_msgs/msg/CameraInfo[ignition.msgs.CameraInfo")
+        gz_args.append("/camera/depth@sensor_msgs/msg/Image[ignition.msgs.Image")
+        gz_args.append(
+            "/camera/depth/points@sensor_msgs/msg/PointCloud2[ignition.msgs.PointCloudPacked"
+        )
+
+        gz_remapping.append(("/camera/camera_info", "/camera/depth/camera_info"))
+        gz_remapping.append(("/camera/depth", "/camera/depth/image_raw"))
+
+        depth_camera_parent_tf = "camera_depth_frame"
+
+    elif camera_model.startswith("stereolabs_zed"):
+        zed = camera_model[len("stereolabs_") :]
+
+        gz_args.append(
+            f"/{zed}/zed_node/rgb/camera_info@sensor_msgs/msg/CameraInfo[ignition.msgs.CameraInfo"
+        )
+        gz_args.append(
+            f"/{zed}/zed_node/rgb/image_rect_color@sensor_msgs/msg/Image[ignition.msgs.Image"
+        )
+        gz_args.append(
+            f"/{zed}/zed_node/camera_info@sensor_msgs/msg/CameraInfo[ignition.msgs.CameraInfo"
+        )
+        gz_args.append(f"/{zed}/zed_node/depth@sensor_msgs/msg/Image[ignition.msgs.Image")
+        gz_args.append(
+            f"/{zed}/zed_node/depth/points@sensor_msgs/msg/PointCloud2"
+            "[ignition.msgs.PointCloudPacked"
+        )
+
+        gz_remapping.append((f"{zed}/zed_node/camera_info", f"/{zed}/zed_node/depth/camera_info"))
+        gz_remapping.append((f"{zed}/zed_node/depth", f"/{zed}/zed_node/depth/depth_registered"))
+        gz_remapping.append(
+            (f"{zed}/zed_node/depth/points", f"/{zed}/zed_node/point_cloud/cloud_registered")
+        )
+
+        depth_camera_parent_tf = "camera_center_optical_frame"
+    else:
+        pass
+
+    # Add lidar topic
+    if lidar_model.startswith("slamtec_rplidar"):
+        gz_args.append("/scan@sensor_msgs/msg/LaserScan[ignition.msgs.LaserScan")
+
+    elif lidar_model.startswith("velodyne"):
+        gz_args.append(
+            "/velodyne_points/points@sensor_msgs/msg/PointCloud2[ignition.msgs.PointCloudPacked"
+        )
+
+        gz_remapping.append(("/velodyne_points/points", "/velodyne_points"))
+    else:  # FIXME: Checkout ouster
+        pass
+
+    gz_bridge_node = Node(
+        package="ros_gz_bridge",
+        executable="parameter_bridge",
+        name="ign_bridge",
+        arguments=gz_args,
+        remappings=gz_remapping,
+        output="screen",
+    )
+
+    # The frame of the point cloud from ignition gazebo 6 isn't provided by <frame_id>.
+    # See https://github.com/gazebosim/gz-sensors/issues/239
+    point_cloud_tf = Node(
+        package="tf2_ros",
+        executable="static_transform_publisher",
+        name="point_cloud_tf",
+        output="log",
+        arguments=[
+            "0.0",
+            "0.0",
+            "0.0",
+            "0.0",
+            "0.0",
+            "0.0",
+            depth_camera_parent_tf,
+            "rosbot_xl/base_link/camera_depth",
+        ],
+    )
+
+    if depth_camera_parent_tf:
+        return [gz_bridge_node, point_cloud_tf]
+    else:
+        return [gz_bridge_node]
+
+
 def generate_launch_description():
     mecanum = LaunchConfiguration("mecanum")
     declare_mecanum_arg = DeclareLaunchArgument(
@@ -39,18 +140,36 @@ def generate_launch_description():
         ),
     )
 
+    camera_model = LaunchConfiguration("camera_model")
+    declare_camera_model_arg = DeclareLaunchArgument(
+        "camera_model",
+        default_value="intel_realsense_d435",
+        description="Add camera model to the robot URDF",
+        choices=[
+            "None",
+            "intel_realsense_d435",
+            "stereolabs_zed",
+            "stereolabs_zedm",
+            "stereolabs_zed2",
+            "stereolabs_zed2i",
+            "stereolabs_zedx",
+            "stereolabs_zedxm",
+        ],
+    )
+
     lidar_model = LaunchConfiguration("lidar_model")
     declare_lidar_model_arg = DeclareLaunchArgument(
         "lidar_model",
         default_value="slamtec_rplidar_s1",
-        description="Lidar model added to the URDF",
-    )
-
-    camera_model = LaunchConfiguration("camera_model")
-    declare_camera_model_arg = DeclareLaunchArgument(
-        "camera_model",
-        default_value="None",
-        description="Camera model added to the URDF",
+        description="Add LiDAR model to the robot URDF",
+        choices=[
+            "None",
+            "ouster_os1_32",
+            "slamtec_rplidar_a2",
+            "slamtec_rplidar_a3",
+            "slamtec_rplidar_s1",
+            "velodyne_puck",
+        ],
     )
 
     include_camera_mount = LaunchConfiguration("include_camera_mount")
@@ -74,24 +193,20 @@ def generate_launch_description():
         description="Run Gazebo Ignition in the headless mode",
     )
 
-    headless_cfg = PythonExpression(
-        [
-            "'--headless-rendering -s -r' if ",
-            headless,
-            " else '-r'",
-        ]
-    )
+    headless_cfg = PythonExpression([
+        "'--headless-rendering -s -r' if ",
+        headless,
+        " else '-r'",
+    ])
     gz_args = [headless_cfg, " ", world_cfg]
 
     gz_sim = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
-            PathJoinSubstitution(
-                [
-                    get_package_share_directory("ros_gz_sim"),
-                    "launch",
-                    "gz_sim.launch.py",
-                ]
-            )
+            PathJoinSubstitution([
+                get_package_share_directory("ros_gz_sim"),
+                "launch",
+                "gz_sim.launch.py",
+            ])
         ),
         launch_arguments={
             "gz_args": gz_args,
@@ -118,43 +233,14 @@ def generate_launch_description():
         ],
         output="screen",
     )
-    ign_bridge = Node(
-        package="ros_gz_bridge",
-        executable="parameter_bridge",
-        name="ign_bridge",
-        arguments=[
-            "/clock" + "@rosgraph_msgs/msg/Clock" + "[ignition.msgs.Clock",
-            "/scan" + "@sensor_msgs/msg/LaserScan" + "[ignition.msgs.LaserScan",
-            "/velodyne_points/points"
-            + "@sensor_msgs/msg/PointCloud2"
-            + "[ignition.msgs.PointCloudPacked",
-            "/camera/color/camera_info"
-            + "@sensor_msgs/msg/CameraInfo"
-            + "[ignition.msgs.CameraInfo",
-            "/camera/color/image_raw" + "@sensor_msgs/msg/Image" + "[ignition.msgs.Image",
-            "/camera/camera_info" + "@sensor_msgs/msg/CameraInfo" + "[ignition.msgs.CameraInfo",
-            "/camera/depth" + "@sensor_msgs/msg/Image" + "[ignition.msgs.Image",
-            "/camera/depth/points"
-            + "@sensor_msgs/msg/PointCloud2"
-            + "[ignition.msgs.PointCloudPacked",
-        ],
-        remappings=[
-            ("/velodyne_points/points", "/velodyne_points"),
-            ("/camera/camera_info", "/camera/depth/camera_info"),
-            ("/camera/depth", "/camera/depth/image_raw"),
-        ],
-        output="screen",
-    )
 
     bringup_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
-            PathJoinSubstitution(
-                [
-                    get_package_share_directory("rosbot_xl_bringup"),
-                    "launch",
-                    "bringup.launch.py",
-                ]
-            )
+            PathJoinSubstitution([
+                get_package_share_directory("rosbot_xl_bringup"),
+                "launch",
+                "bringup.launch.py",
+            ])
         ),
         launch_arguments={
             "mecanum": mecanum,
@@ -166,20 +252,18 @@ def generate_launch_description():
         }.items(),
     )
 
-    return LaunchDescription(
-        [
-            declare_mecanum_arg,
-            declare_lidar_model_arg,
-            declare_camera_model_arg,
-            declare_include_camera_mount_arg,
-            declare_world_arg,
-            declare_headless_arg,
-            # Sets use_sim_time for all nodes started below
-            # (doesn't work for nodes started from ignition gazebo)
-            SetParameter(name="use_sim_time", value=True),
-            gz_sim,
-            ign_bridge,
-            gz_spawn_entity,
-            bringup_launch,
-        ]
-    )
+    return LaunchDescription([
+        declare_mecanum_arg,
+        declare_lidar_model_arg,
+        declare_camera_model_arg,
+        declare_include_camera_mount_arg,
+        declare_world_arg,
+        declare_headless_arg,
+        # Sets use_sim_time for all nodes started below
+        # (doesn't work for nodes started from ignition gazebo)
+        SetParameter(name="use_sim_time", value=True),
+        gz_sim,
+        OpaqueFunction(function=launch_gz_bridge),
+        gz_spawn_entity,
+        bringup_launch,
+    ])
diff --git a/rosbot_xl_gazebo/test/test_diff_drive_simulation.py b/rosbot_xl_gazebo/test/test_diff_drive_simulation.py
index b9cd2b79..4124afee 100644
--- a/rosbot_xl_gazebo/test/test_diff_drive_simulation.py
+++ b/rosbot_xl_gazebo/test/test_diff_drive_simulation.py
@@ -33,13 +33,11 @@ def generate_test_description():
     rosbot_xl_gazebo = get_package_share_directory("rosbot_xl_gazebo")
     simulation_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
-            PathJoinSubstitution(
-                [
-                    rosbot_xl_gazebo,
-                    "launch",
-                    "simulation.launch.py",
-                ]
-            )
+            PathJoinSubstitution([
+                rosbot_xl_gazebo,
+                "launch",
+                "simulation.launch.py",
+            ])
         ),
         launch_arguments={
             "headless": "True",
diff --git a/rosbot_xl_gazebo/test/test_flake8.py b/rosbot_xl_gazebo/test/test_flake8.py
index 49c1644f..40602d8a 100644
--- a/rosbot_xl_gazebo/test/test_flake8.py
+++ b/rosbot_xl_gazebo/test/test_flake8.py
@@ -13,11 +13,13 @@
 # limitations under the License.
 
 from ament_flake8.main import main_with_errors
+from os.path import join, dirname
 import pytest
 
 
 @pytest.mark.flake8
 @pytest.mark.linter
 def test_flake8():
-    rc, errors = main_with_errors(argv=[])
+    config_file = join(dirname(dirname(dirname(__file__))), ".flake8")
+    rc, errors = main_with_errors(argv=["--config", config_file])
     assert rc == 0, "Found %d code style errors / warnings:\n" % len(errors) + "\n".join(errors)
diff --git a/rosbot_xl_gazebo/test/test_mecanum_simulation.py b/rosbot_xl_gazebo/test/test_mecanum_simulation.py
index 00e24473..6113eea7 100644
--- a/rosbot_xl_gazebo/test/test_mecanum_simulation.py
+++ b/rosbot_xl_gazebo/test/test_mecanum_simulation.py
@@ -33,13 +33,11 @@ def generate_test_description():
     rosbot_xl_gazebo = get_package_share_directory("rosbot_xl_gazebo")
     simulation_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
-            PathJoinSubstitution(
-                [
-                    rosbot_xl_gazebo,
-                    "launch",
-                    "simulation.launch.py",
-                ]
-            )
+            PathJoinSubstitution([
+                rosbot_xl_gazebo,
+                "launch",
+                "simulation.launch.py",
+            ])
         ),
         launch_arguments={
             "mecanum": "True",
diff --git a/rosbot_xl_gazebo/test/test_slamtec_rplidar_a2.py b/rosbot_xl_gazebo/test/test_slamtec_rplidar_a2.py
index eb7b99cf..f1607a85 100644
--- a/rosbot_xl_gazebo/test/test_slamtec_rplidar_a2.py
+++ b/rosbot_xl_gazebo/test/test_slamtec_rplidar_a2.py
@@ -33,13 +33,11 @@ def generate_test_description():
     rosbot_xl_gazebo = get_package_share_directory("rosbot_xl_gazebo")
     simulation_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
-            PathJoinSubstitution(
-                [
-                    rosbot_xl_gazebo,
-                    "launch",
-                    "simulation.launch.py",
-                ]
-            )
+            PathJoinSubstitution([
+                rosbot_xl_gazebo,
+                "launch",
+                "simulation.launch.py",
+            ])
         ),
         launch_arguments={
             "headless": "True",
diff --git a/rosbot_xl_gazebo/test/test_slamtec_rplidar_a3.py b/rosbot_xl_gazebo/test/test_slamtec_rplidar_a3.py
index 83c9d728..ce68f668 100644
--- a/rosbot_xl_gazebo/test/test_slamtec_rplidar_a3.py
+++ b/rosbot_xl_gazebo/test/test_slamtec_rplidar_a3.py
@@ -33,13 +33,11 @@ def generate_test_description():
     rosbot_xl_gazebo = get_package_share_directory("rosbot_xl_gazebo")
     simulation_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
-            PathJoinSubstitution(
-                [
-                    rosbot_xl_gazebo,
-                    "launch",
-                    "simulation.launch.py",
-                ]
-            )
+            PathJoinSubstitution([
+                rosbot_xl_gazebo,
+                "launch",
+                "simulation.launch.py",
+            ])
         ),
         launch_arguments={
             "headless": "True",
diff --git a/rosbot_xl_gazebo/test/test_slamtec_rplidar_s1.py b/rosbot_xl_gazebo/test/test_slamtec_rplidar_s1.py
index 2640f868..a209cc27 100644
--- a/rosbot_xl_gazebo/test/test_slamtec_rplidar_s1.py
+++ b/rosbot_xl_gazebo/test/test_slamtec_rplidar_s1.py
@@ -33,13 +33,11 @@ def generate_test_description():
     rosbot_xl_gazebo = get_package_share_directory("rosbot_xl_gazebo")
     simulation_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
-            PathJoinSubstitution(
-                [
-                    rosbot_xl_gazebo,
-                    "launch",
-                    "simulation.launch.py",
-                ]
-            )
+            PathJoinSubstitution([
+                rosbot_xl_gazebo,
+                "launch",
+                "simulation.launch.py",
+            ])
         ),
         launch_arguments={
             "headless": "True",