Ouster sensor integration (#314)

* Add required ros installs for ouster integration

* Change tactic, install ouster ros package instead

* Create Ouster OS1 URDF XACRO, model works, confirmed when replacing path at the spot of the velodyne urdf load in robot.py

* Start adding the Ouster lidar as option to the files, but a launch file is not created yet

* Ouster Lidar works in simulation, nr of channels lowered since the simulation could not handle 128 channels ('Extrapolation Error looking up target frame')

* Update documentation, hardware information still needs to be filled in

* Add ouster and velodyne tests

* Add missing license lines

* Fix license for OS1 DAE file

* Add nav2 test for panther+ouster lidar

* Move Ouster section above Velodyne section

Signed-off-by: Rosalie <[email protected]>

* Change the general 'lidar' name to 'velodyne' specifically since we now have multiple lidar options

Signed-off-by: Rosalie <[email protected]>

* Rename test_panther_nav2 file to contain velodyne name

Signed-off-by: Rosalie <[email protected]>

* Change naming.

Signed-off-by: Jelmer de Wolde <[email protected]>

---------

Signed-off-by: Rosalie <[email protected]>
Signed-off-by: Jelmer de Wolde <[email protected]>
Co-authored-by: Jelmer de Wolde <[email protected]>

Author: rosalievanark

dockerfiles/install_scripts/core_packages.sh

@@ -24,6 +24,7 @@ apt install -y \
     ros-$ROS_DISTRO-moveit-visual-tools \
     ros-$ROS_DISTRO-navigation2 \
     ros-$ROS_DISTRO-nav2-bringup \
+    ros-$ROS_DISTRO-ouster-ros \
     ros-$ROS_DISTRO-plotjuggler-ros \
     ros-$ROS_DISTRO-realsense2-camera \
     ros-$ROS_DISTRO-realsense2-description \

docs/content/platforms.md

@@ -116,21 +116,32 @@ A ZED camera can be launched in simulation by creating a configuration with an *
 
 A ZED camera can be used by connecting it to the host device via USB. To allow non-root users to access the camera, UDEV rules must be installed on the host machine. The required script can be found [here](https://gist.github.com/adujardin/2d5ce8f000fc6a7bd40bee2709749ff8).
 
+## Ouster
+
+![Ouster](../img/ouster/ouster_os1.png)
+
+### Simulation Ouster
+
+An Ouster lidar can be launched in simulation by creating a configuration with a *Lidar* of type *Ouster*.
+
+### Hardware Ouster
+
+*TODO: update when sensor has arrived.*
+
 ## Velodyne
 
 ![Velodyne](../img/velodyne/velodyne.png)
 
 ### Simulation Velodyne
 
-A Velodyne lidar can be launched in simulation by creating a configuration with an *Lidar* of type *velodyne*.
+A Velodyne lidar can be launched in simulation by creating a configuration with a *Lidar* of type *velodyne*.
 
 ### Hardware Velodyne
 
 When using the Velodyne lidar, make sure that the IP-address of the host device (where the velodyne node is running) is set correctly in the settings. One can edit the settings of the velodyne using a web interface on it's IP-adress.
 
 | ![Velodyne settings](../img/velodyne/velodyne_settings.png) | ![Teltonika settings](../img/teltonika/teltonika_settings.png) |
 |-------------------------------------------------------------|----------------------------------------------------------------|
-| velodyne settings                                           | teltonika settings                                             |
 
 ## Teltonika GPS
 

docs/img/ouster/ouster_os1.png

@@ -0,0 +1,3 @@
+SPDX-FileCopyrightText: Alliander N. V.
+
+SPDX-License-Identifier: Apache-2.0

docs/img/ouster/ouster_os1.png.license

@@ -22,7 +22,8 @@
         "",
         "axis",
         "gps",
-        "lidar",
+        "ouster",
+        "velodyne",
         "realsense",
         "zed",
         "franka",
@@ -34,9 +35,11 @@
         "panther_gps",
         "panther_realsense",
         "panther_zed",
-        "panther_lidar",
+        "panther_velodyne",
+        "panther_ouster",
         "mm",
-        "mm_lidar",
+        "mm_velodyne",
+        "mm_ouster",
         "panther_and_franka",
     ],
 )
@@ -71,7 +74,9 @@ def launch_setup(context: LaunchContext) -> list:  # noqa: PLR0912, PLR0915
             Platform("axis", [0, 0, 0])
         case "gps":
             GPS("nmea", [0, 0, 0.5], ip_address="10.15.20.202")
-        case "lidar":
+        case "ouster":
+            Lidar("ouster", [0, 0, 0.5])
+        case "velodyne":
             Lidar("velodyne", [0, 0, 0.5])
         case "realsense":
             Camera("realsense", [0, 0, 0.5])
@@ -106,16 +111,23 @@ def launch_setup(context: LaunchContext) -> list:  # noqa: PLR0912, PLR0915
         case "panther_zed":
             panther = Vehicle("panther", [0, 0, 0.2])
             Camera("zed", [0, 0, 0.5], parent=panther)
-        case "panther_lidar":
+        case "panther_velodyne":
             panther = Vehicle("panther", [0, 0, 0.2], navigation=True)
             Lidar("velodyne", [0.13, -0.13, 0.35], parent=panther)
+        case "panther_ouster":
+            panther = Vehicle("panther", [0, 0, 0.2], navigation=True)
+            Lidar("ouster", [0.13, -0.13, 0.35], parent=panther)
         case "mm":
             panther = Vehicle("panther", [0, 0, 0.2])
             Arm("franka", [0, 0, 0.14], gripper=True, parent=panther, moveit=True)
-        case "mm_lidar":
+        case "mm_velodyne":
             panther = Vehicle("panther", [0, 0, 0.2], navigation=True)
             Arm("franka", [0, 0, 0.14], gripper=True, parent=panther, moveit=True)
             Lidar("velodyne", [0.13, -0.13, 0.35], parent=panther)
+        case "mm_ouster":
+            panther = Vehicle("panther", [0, 0, 0.2], navigation=True)
+            Arm("franka", [0, 0, 0.14], gripper=True, parent=panther, moveit=True)
+            Lidar("ouster", [0.13, -0.13, 0.35], parent=panther)
         case "panther_and_franka":
             Vehicle("panther", [0, -0.5, 0.2])
             Arm("franka", [0, 0.5, 0])

ros2_ws/src/rcdt_launch/launch/robots.launch.py

@@ -90,7 +90,16 @@ def order_platforms() -> None:
         Raises:
             ValueError: If an unknown platform is encountered.
         """
-        order = ["panther", "franka", "velodyne", "realsense", "zed", "nmea", "axis"]
+        order = [
+            "panther",
+            "franka",
+            "velodyne",
+            "ouster",
+            "realsense",
+            "zed",
+            "nmea",
+            "axis",
+        ]
 
         for platform in Platform.platforms:
             if platform.platform not in order:
@@ -296,7 +305,14 @@ def create_map_links() -> list[Node]:
     def __init__(  # noqa: PLR0913
         self,
         platform: Literal[
-            "panther", "franka", "velodyne", "realsense", "zed", "nmea", "axis"
+            "panther",
+            "franka",
+            "velodyne",
+            "ouster",
+            "realsense",
+            "zed",
+            "nmea",
+            "axis",
         ],
         position: list,
         orientation: list | None = None,
@@ -307,7 +323,7 @@ def __init__(  # noqa: PLR0913
         """Initialize a robot instance.
 
         Args:
-            platform (Literal["panther", "franka", "velodyne", "realsense", "zed", "nmea", "axis"]): The platform type of the robot.
+            platform (Literal["panther", "franka", "velodyne", "ouster", "realsense", "zed", "nmea", "axis"]): The platform type of the robot.
             position (list): The initial position of the robot.
             orientation (list | None): The initial orientation of the robot.
             namespace (str | None): The namespace of the robot. If None, a unique namespace will be generated.
@@ -400,6 +416,10 @@ def xacro_path(self) -> str:  # noqa: PLR0911
                 return get_file_path(
                     "rcdt_sensors", ["urdf"], "rcdt_velodyne.urdf.xacro"
                 )
+            case "ouster":
+                return get_file_path(
+                    "rcdt_sensors", ["urdf"], "rcdt_os1_128.urdf.xacro"
+                )
             case "realsense":
                 return get_file_path(
                     "rcdt_sensors", ["urdf"], "rcdt_realsense_d435.urdf.xacro"
@@ -432,6 +452,8 @@ def base_link(self) -> str:  # noqa: PLR0911
                 return "fr3_link0"
             case "velodyne":
                 return "base_link"
+            case "ouster":
+                return "base_link"
             case "realsense":
                 return "base_link"
             case "zed":
@@ -685,7 +707,7 @@ class Lidar(Platform):
 
     def __init__(  # noqa: PLR0913
         self,
-        platform: Literal["velodyne"],
+        platform: Literal["velodyne", "ouster"],
         position: list,
         orientation: list | None = None,
         namespace: str | None = None,
@@ -695,7 +717,7 @@ def __init__(  # noqa: PLR0913
         """Initialize the Lidar platform.
 
         Args:
-            platform (Literal["velodyne"]): The platform type.
+            platform (Literal["velodyne", "ouster"]): The platform type.
             position (list): The position of the lidar.
             orientation (list | None): The initial orientation of the lidar.
             namespace (str | None): The namespace of the lidar.
@@ -723,7 +745,10 @@ def create_launch_description(self) -> list[RegisteredLaunchDescription]:
             list[RegisteredLaunchDescription]: The launch description for the platform.
         """
         launch_descriptions = []
-        launch_descriptions.append(self.create_velodyne_launch())
+        if self.platform == "velodyne":
+            launch_descriptions.append(self.create_velodyne_launch())
+        if self.platform == "ouster":
+            launch_descriptions.append(self.create_ouster_launch())
         return launch_descriptions
 
     def create_velodyne_launch(self) -> RegisteredLaunchDescription:
@@ -746,6 +771,26 @@ def create_velodyne_launch(self) -> RegisteredLaunchDescription:
             },
         )
 
+    def create_ouster_launch(self) -> RegisteredLaunchDescription:
+        """Create the Ouster launch description.
+
+        Returns:
+            RegisteredLaunchDescription: The launch description for Ouster.
+        """
+        if self.parent:
+            target_frame = f"{self.parent.namespace}/{self.parent.base_link}"
+        else:
+            target_frame = f"{self.namespace}/base_link"
+
+        return RegisteredLaunchDescription(
+            get_file_path("rcdt_sensors", ["launch"], "ouster.launch.py"),
+            launch_arguments={
+                "simulation": str(Platform.simulation),
+                "namespace": self.namespace,
+                "target_frame": target_frame,
+            },
+        )
+
 
 class Arm(Platform):
     """Extension on Platform with arm specific functionalities."""

ros2_ws/src/rcdt_launch/rcdt_launch/robot.py

@@ -23,7 +23,7 @@ install(
 
 # Shared folders:
 install(
-  DIRECTORY launch urdf config
+  DIRECTORY launch urdf config meshes
   DESTINATION share/${PROJECT_NAME}
 )
 
@@ -32,4 +32,5 @@ if(BUILD_TESTING)
   ament_lint_auto_find_test_dependencies()
 endif()
 
+ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/env-hooks/${PROJECT_NAME}.sh.in")
 ament_package()

ros2_ws/src/rcdt_sensors/CMakeLists.txt

@@ -0,0 +1,5 @@
+# SPDX-FileCopyrightText: Alliander N. V.
+#
+# SPDX-License-Identifier: Apache-2.0
+
+ament_prepend_unique_value GZ_SIM_RESOURCE_PATH "@CMAKE_INSTALL_PREFIX@/share"

ros2_ws/src/rcdt_sensors/env-hooks/rcdt_sensors.sh.in

@@ -0,0 +1,66 @@
+# SPDX-FileCopyrightText: Alliander N. V.
+#
+# SPDX-License-Identifier: Apache-2.0
+
+
+from launch import LaunchContext, LaunchDescription
+from launch.actions import OpaqueFunction
+from launch_ros.actions import Node
+from rcdt_utilities.launch_utils import LaunchArgument
+from rcdt_utilities.register import Register
+
+use_sim_arg = LaunchArgument("simulation", True, [True, False])
+namespace_arg = LaunchArgument("namespace", "ouster")
+target_frame_arg = LaunchArgument("target_frame", "")
+
+
+def launch_setup(context: LaunchContext) -> list:
+    """Setup the launch description for the Ouster lidar.
+
+    Args:
+        context (LaunchContext): The launch context.
+
+    Returns:
+        list: A list of actions to be executed.
+    """
+    # Simulation-only setup
+    namespace = namespace_arg.string_value(context)
+    target_frame = target_frame_arg.string_value(context)
+
+    pointcloud_to_laserscan_node = Node(
+        package="pointcloud_to_laserscan",
+        executable="pointcloud_to_laserscan_node",
+        remappings=[
+            ("cloud_in", f"/{namespace}/scan/points"),
+            ("scan", f"/{namespace}/scan"),
+        ],
+        parameters=[
+            {
+                "target_frame": target_frame,
+                "min_height": 0.1,
+                "max_height": 2.0,
+                "range_min": 0.05,
+                "range_max": 90.0,
+            }
+        ],
+        namespace=namespace,
+    )
+
+    return [
+        Register.on_start(pointcloud_to_laserscan_node, context),
+    ]
+
+
+def generate_launch_description() -> LaunchDescription:
+    """Generate the launch description for the realsense camera.
+
+    Returns:
+        LaunchDescription: The launch description for the realsense camera.
+    """
+    return LaunchDescription(
+        [
+            use_sim_arg.declaration,
+            namespace_arg.declaration,
+            OpaqueFunction(function=launch_setup),
+        ]
+    )

ros2_ws/src/rcdt_sensors/launch/ouster.launch.py

@@ -15,7 +15,7 @@
 
 
 def launch_setup(context: LaunchContext) -> list:
-    """Setup the launch description for the realsense camera.
+    """Setup the launch description for the Velodyne lidar.
 
     Args:
         context (LaunchContext): The launch context.