315 sensor integrate ouster hardware (#346)

* Update launch file with lifecycle node for the ouster driver

Signed-off-by: Rosalie <rosalie.van.ark@alliander.com>

* Add temporary placeholder for topic remappings for the ouster driver

Signed-off-by: Rosalie <rosalie.van.ark@alliander.com>

* Update ouster launch file to match with ouster hardware, and update Lidar class to also bridge /scan topic for the ouster since the hardware itself does that as well

Signed-off-by: Rosalie <rosalie.van.ark@alliander.com>

* Update ouster documentation

Signed-off-by: Rosalie <rosalie.van.ark@alliander.com>

* Remove unused import

Signed-off-by: Rosalie <rosalie.van.ark@alliander.com>

* Turn off scan method of the Ouster since it only takes a single row of the entire scan instead of merging the rows, and add the pointcloud_to_laserscan node

Signed-off-by: Rosalie <rosalie.van.ark@alliander.com>

* Correctly update the documentation and make small adjustment to launch file to make the ip addresses slightly more dynamically adjustable when desired

Signed-off-by: Rosalie <rosalie.van.ark@alliander.com>

* Generalize setting the ip address of the device a bit more

Signed-off-by: Rosalie <rosalie.van.ark@alliander.com>

* Add missing line for linting

Signed-off-by: Rosalie <rosalie.van.ark@alliander.com>

---------

Signed-off-by: Rosalie <rosalie.van.ark@alliander.com>

Author: rosalievanark

docs/content/platforms.md

@@ -39,6 +39,14 @@ We created a fork with robot-specific enhancements: [https://github.com/alliande
 
 ![Panther](../img/panther/panther.png)
 
+Regarding all vehicles, the general rule we apply for assigning IP addresses follows the following numbering order:
+
+> 1 - Router \
+2 - Low-level computer (Raspberry Pi) \
+3 - High-level computer \
+4 - Additional component (e.g. Franka Arm) \
+5 - LiDAR
+
 ### Simulation Panther
 
 A Panther vehicle can be launched in simulation by creating a configuration with an *Vehicle* of type *Panther*. Note that the E-Stop is triggered by default and needs to be released before driving is possible. This can be done by a service call:
@@ -126,7 +134,27 @@ An Ouster lidar can be launched in simulation by creating a configuration with a
 
 ### Hardware Ouster
 
-*TODO: update when sensor has arrived.*
+**Network settings:**
+\
+When using the Ouster lidar, make sure that the IP-address of the host device (where the Ouster node is running) is set correctly in the settings of the Teltonika router. One can assign a static IP address to the Ouster via the Teltonika interface. In case of the Husarion vehicles, this interface is reachable via `http://10.15.20.1/`, and the static IP address should be set to `10.15.20.5` as number 5 is reserved for LiDARs.
+
+Additionally, it is important to assign the correct UDP destination IP address for the Ouster LiDAR. This can both be done via the Ouster's configuration interface at `http://os-{serial_number}.local/`, or it can be done via the launch file of the Ouster (`ouster.launch.py` in the `rcdt_sensors` package).
+
+_Note:_ If the firewall is enabled in Ubuntu, communication with the LiDAR is most likely blocked. Unblock it by allowing the IP-address of the LiDAR:
+
+```bash
+sudo ufw allow to {IPv4_address}
+sudo ufw allow from {IPv4_address}
+```
+
+**ROS2 setup:**
+\
+The [Ouster driver](https://github.com/ouster-lidar/ouster-ros/tree/ros2) runs as a [LifeCycle node](https://design.ros2.org/articles/node_lifecycle.html), meaning that once created, the node starts in an `Unconfigured` state. It needs to be `configured` and `activated` to start the driver.
+
+Find all of the connected Ouster's information at `http://os-{serial_number}.local/`, where the following parameters for the driver node can be found:
+- `sensor_hostname`: Dashboard > System Information > IPv4 _(Remove the prefix length)_
+- `udp_dest`: Dashboard > System Status > Web Client Address
+
 
 ## Velodyne
 

ros2_ws/src/rcdt_launch/rcdt_launch/platforms/lidar.py

@@ -34,6 +34,7 @@ def __init__(  # noqa: PLR0913
         namespace: str | None = None,
         parent: Vehicle | None = None,
         parent_link: str = "",
+        ip_address: str = "",
     ):
         """Initialize the Lidar platform.
 
@@ -44,10 +45,12 @@ def __init__(  # noqa: PLR0913
             namespace (str | None): The namespace of the lidar.
             parent (Vehicle | None): The parent platform.
             parent_link (str): The link of the parent to which the platform is attached. If empty, the base_link of the parent is used.
+            ip_address (str): The IP address of the platform.
         """
         super().__init__(
             platform, position, orientation, namespace, parent, parent_link
         )
+        self.ip_address = ip_address
 
         if parent:
             parent.lidar = self
@@ -120,6 +123,7 @@ def create_velodyne_launch(self) -> RegisteredLaunchDescription:
                 "simulation": str(EnvironmentConfiguration.simulation),
                 "namespace": self.namespace,
                 "target_frame": target_frame,
+                "ip_address": self.ip_address,
             },
         )
 
@@ -140,5 +144,7 @@ def create_ouster_launch(self) -> RegisteredLaunchDescription:
                 "simulation": str(EnvironmentConfiguration.simulation),
                 "namespace": self.namespace,
                 "target_frame": target_frame,
+                "ip_device": self.ip_address,
+                "ip_udp_destination": "10.15.20.3",
             },
         )

ros2_ws/src/rcdt_launch/rcdt_launch/predefined_configurations.py

@@ -84,12 +84,12 @@ def config_gps() -> None:  # noqa: D103
 
 @register_configuration("ouster")
 def config_ouster() -> None:  # noqa: D103
-    Lidar("ouster", [0, 0, 0.5])
+    Lidar("ouster", [0, 0, 0.5], ip_address="10.15.20.5")
 
 
 @register_configuration("velodyne")
 def config_velodyne() -> None:  # noqa: D103
-    Lidar("velodyne", [0, 0, 0.5])
+    Lidar("velodyne", [0, 0, 0.5], ip_address="10.15.20.5")
 
 
 @register_configuration("realsense")
@@ -156,31 +156,31 @@ def config_panther_zed() -> None:  # noqa: D103
 @register_configuration("panther_velodyne")
 def config_panther_velodyne() -> None:  # noqa: D103
     panther = Vehicle("panther", [0, 0, 0.2])
-    Lidar("velodyne", [0.13, -0.13, 0.35], parent=panther)
+    Lidar("velodyne", [0.13, -0.13, 0.35], parent=panther, ip_address="10.15.20.5")
 
 
 @register_configuration("panther_ouster")
 def config_panther_ouster() -> None:  # noqa: D103
     panther = Vehicle("panther", [0, 0, 0.2])
-    Lidar("ouster", [0.13, -0.13, 0.35], parent=panther)
+    Lidar("ouster", [0.13, -0.13, 0.35], parent=panther, ip_address="10.15.20.5")
 
 
 @register_configuration("panther_collision_monitor")
 def config_panther_collision_monitor() -> None:  # noqa: D103
     panther = Vehicle("panther", [0, 0, 0.2], collision_monitor=True)
-    Lidar("velodyne", [0.13, -0.13, 0.35], parent=panther)
+    Lidar("velodyne", [0.13, -0.13, 0.35], parent=panther, ip_address="10.15.20.5")
 
 
 @register_configuration("panther_slam")
 def config_panther_slam() -> None:  # noqa: D103
     panther = Vehicle("panther", [0, 0, 0.2], slam=True)
-    Lidar("velodyne", [0.13, -0.13, 0.35], parent=panther)
+    Lidar("velodyne", [0.13, -0.13, 0.35], parent=panther, ip_address="10.15.20.5")
 
 
 @register_configuration("panther_lidar_navigation")
 def config_panther_lidar_navigation() -> None:  # noqa: D103
     panther = Vehicle("panther", [0, 0, 0.2], navigation=True)
-    Lidar("velodyne", [0.13, -0.13, 0.35], parent=panther)
+    Lidar("velodyne", [0.13, -0.13, 0.35], parent=panther, ip_address="10.15.20.5")
 
 
 @register_configuration("panther_gps_navigation")
@@ -190,7 +190,7 @@ def config_panther_gps() -> None:  # noqa: D103
     panther = Vehicle(
         "panther", [0, 0, 0.2], navigation=True, use_gps=True, window_size=50
     )
-    Lidar("velodyne", [0.13, -0.13, 0.35], parent=panther)
+    Lidar("velodyne", [0.13, -0.13, 0.35], parent=panther, ip_address="10.15.20.5")
     GPS("nmea", [0, 0, 0.2], parent=panther)
 
 
@@ -205,14 +205,14 @@ def config_mm() -> None:  # noqa: D103
 def config_mm_velodyne() -> None:  # noqa: D103
     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)
+    Lidar("velodyne", [0.13, -0.13, 0.35], parent=panther, ip_address="10.15.20.5")
 
 
 @register_configuration("mm_ouster")
 def config_mm_ouster() -> None:  # noqa: D103
     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)
+    Lidar("ouster", [0.13, -0.13, 0.35], parent=panther, ip_address="10.15.20.5")
 
 
 @register_configuration("mm_gps")
@@ -222,7 +222,7 @@ def config_mm_gps() -> None:  # noqa: D103
         "panther", [0, 0, 0.2], navigation=True, use_gps=True, window_size=50
     )
     Arm("franka", [0, 0, 0.14], gripper=True, parent=panther, moveit=True)
-    Lidar("velodyne", [0.13, -0.13, 0.35], parent=panther)
+    Lidar("velodyne", [0.13, -0.13, 0.35], parent=panther, ip_address="10.15.20.5")
     GPS("nmea", [0, 0, 0.2], parent=panther)
 
 

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

@@ -2,16 +2,20 @@
 #
 # SPDX-License-Identifier: Apache-2.0
 
-
 from launch import LaunchContext, LaunchDescription
-from launch.actions import OpaqueFunction
-from launch_ros.actions import Node
+from launch.actions import ExecuteProcess, OpaqueFunction
+from launch_ros.actions import LifecycleNode, Node
 from rcdt_utilities.launch_argument import LaunchArgument
+from rcdt_utilities.launch_utils import SKIP
 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", "")
+ip_device_arg = LaunchArgument("ip_device", "")  # IP address of the Ouster LiDAR
+ip_udp_destination_arg = LaunchArgument(
+    "ip_udp_destination", ""
+)  # Data destination address (host computer)
 
 
 def launch_setup(context: LaunchContext) -> list:
@@ -23,9 +27,64 @@ def launch_setup(context: LaunchContext) -> list:
     Returns:
         list: A list of actions to be executed.
     """
-    # Simulation-only setup
+    use_sim = use_sim_arg.bool_value(context)
     namespace = namespace_arg.string_value(context)
     target_frame = target_frame_arg.string_value(context)
+    ip_device = ip_device_arg.string_value(context)
+    ip_udp_destination = ip_udp_destination_arg.string_value(context)
+    driver_node_name = "ouster_driver"
+
+    sensor_frame = f"{namespace}/ouster"
+    lidar_frame = f"{namespace}/os_lidar"
+    imu_frame = f"{namespace}/os_imu"
+
+    ouster_driver_node = LifecycleNode(
+        package="ouster_ros",
+        executable="os_driver",
+        namespace=namespace,
+        name=driver_node_name,
+        parameters=[
+            {
+                "sensor_hostname": ip_device,
+                "udp_dest": ip_udp_destination,
+                "lidar_port": 7502,
+                "imu_port": 7503,
+                "lidar_mode": "1024x10",  # options: { 512x10, 512x20, 1024x10, 1024x20, 2048x10, 4096x5 }
+                "sensor_frame": sensor_frame,
+                "lidar_frame": lidar_frame,
+                "imu_frame": imu_frame,
+                "point_cloud_frame": lidar_frame,
+                "proc_mask": "PCL",  # options: IMU|PCL|SCAN|IMG|RAW|TLM
+                "metadata": "/tmp/ouster_metadata.json",  # Place the metadata in a temporary folder since we do not need it.
+            }
+        ],
+        remappings=[
+            ("points", "scan/points")
+        ],  # Remap for the pointcloud_to_laserscan Node
+        output="both",
+    )
+
+    configure_ouster_driver = ExecuteProcess(
+        cmd=[
+            "ros2",
+            "lifecycle",
+            "set",
+            f"/{namespace}/{driver_node_name}",
+            "configure",
+        ],
+        shell=False,
+    )
+
+    activate_ouster_driver = ExecuteProcess(
+        cmd=[
+            "ros2",
+            "lifecycle",
+            "set",
+            f"/{namespace}/{driver_node_name}",
+            "activate",
+        ],
+        shell=False,
+    )
 
     pointcloud_to_laserscan_node = Node(
         package="pointcloud_to_laserscan",
@@ -47,6 +106,9 @@ def launch_setup(context: LaunchContext) -> list:
     )
 
     return [
+        Register.on_start(ouster_driver_node, context) if not use_sim else SKIP,
+        Register.on_exit(configure_ouster_driver, context) if not use_sim else SKIP,
+        Register.on_exit(activate_ouster_driver, context) if not use_sim else SKIP,
         Register.on_start(pointcloud_to_laserscan_node, context),
     ]
 
@@ -61,6 +123,9 @@ def generate_launch_description() -> LaunchDescription:
         [
             use_sim_arg.declaration,
             namespace_arg.declaration,
+            target_frame_arg.declaration,
+            ip_device_arg.declaration,
+            ip_udp_destination_arg.declaration,
             OpaqueFunction(function=launch_setup),
         ]
     )

ros2_ws/src/rcdt_sensors/launch/velodyne.launch.py

@@ -14,6 +14,7 @@
 use_sim_arg = LaunchArgument("simulation", True, [True, False])
 namespace_arg = LaunchArgument("namespace", "velodyne")
 target_frame_arg = LaunchArgument("target_frame", "")
+ip_address_arg = LaunchArgument("ip_address", "")
 
 
 def launch_setup(context: LaunchContext) -> list:
@@ -28,6 +29,7 @@ def launch_setup(context: LaunchContext) -> list:
     use_sim = use_sim_arg.bool_value(context)
     namespace = namespace_arg.string_value(context)
     target_frame = target_frame_arg.string_value(context)
+    ip_address = ip_address_arg.string_value(context)
 
     frame_prefix = namespace + "/" if namespace else ""
 
@@ -38,7 +40,7 @@ def launch_setup(context: LaunchContext) -> list:
         parameters=[
             {
                 "model": "VLP16",
-                "device_ip": "10.15.20.5",
+                "device_ip": ip_address,
                 "frame_id": frame_prefix + "velodyne",
             }
         ],
@@ -99,6 +101,8 @@ def generate_launch_description() -> LaunchDescription:
         [
             use_sim_arg.declaration,
             namespace_arg.declaration,
+            target_frame_arg.declaration,
+            ip_address_arg.declaration,
             OpaqueFunction(function=launch_setup),
         ]
     )