373-integrate-beamagine

.devcontainer/dev/devcontainer.json

@@ -46,7 +46,8 @@
         "redhat.vscode-yaml",
         "redhat.vscode-xml",
         "github.vscode-pull-request-github",
-        "davidanson.vscode-markdownlint"
+        "davidanson.vscode-markdownlint",
+        "redhat.vscode-xml"
       ]
     }
   }

alliander_robotics/alliander_beamagine/alliander_beamagine.Dockerfile

@@ -0,0 +1,51 @@
+# SPDX-FileCopyrightText: Alliander N. V.
+#
+# SPDX-License-Identifier: Apache-2.0
+ARG BASE_IMAGE=ubuntu:latest
+FROM $BASE_IMAGE 
+
+ARG COLCON_BUILD_SEQUENTIAL
+ENV ROS_DISTRO=jazzy
+
+# Install L3Cam SDK:
+ARG TEMP_DIR="/tmp/beamagine_install"
+ARG RUN_FILE="$TEMP_DIR/l3cam_sdk.run"
+RUN mkdir -p "$TEMP_DIR"
+RUN if [ $(dpkg --print-architecture) = "amd64" ]; \
+  then wget -O "$RUN_FILE" "https://github.com/beamaginelidar/libl3cam/releases/download/0.2.1R/libl3cam_0.2.1-1_amd64.deb"; \
+  elif [ $(dpkg --print-architecture) = "arm64" ]; \ 
+  then wget -O "$RUN_FILE" "https://github.com/beamaginelidar/libl3cam/releases/download/0.2.1R/libl3cam_0.2.1-1_arm64.deb"; \
+  else echo "Unsupported architecture: $(dpkg --print-architecture)"; exit 1; fi
+RUN dpkg -i "${RUN_FILE}"
+
+# Set buffer size:
+RUN echo 'net.core.rmem_default=268435456' >> /etc/sysctl.conf && \
+  echo 'net.core.rmem_max=268435456' >> /etc/sysctl.conf && \
+  echo 'net.core.netdev_max_backlog=5000' >> /etc/sysctl.conf
+
+# Install ROS driver:
+WORKDIR /$WORKDIR/external
+RUN apt update \
+  && git clone -b 1.0.3 https://github.com/beamaginelidar/l3cam_ros2.git src/l3cam_ros2 \
+  && cd /$WORKDIR/external \
+  && rosdep update --rosdistro $ROS_DISTRO \
+  && rosdep install --from-paths src -y -i
+RUN /$WORKDIR/colcon_build.sh
+
+# Install repo package:
+WORKDIR /$WORKDIR/ros
+COPY alliander_robotics/alliander_core/src/ /$WORKDIR/ros/src
+COPY alliander_robotics/alliander_beamagine/src/ /$WORKDIR/ros/src
+RUN /$WORKDIR/colcon_build.sh
+
+# Install python dependencies:
+WORKDIR $WORKDIR
+COPY pyproject.toml /$WORKDIR/pyproject.toml
+RUN uv sync \
+  && echo "export PYTHONPATH=\"$(dirname $(dirname $(uv python find)))/lib/python3.12/site-packages:\$PYTHONPATH\"" >> /root/.bashrc \
+  && echo "export PATH=\"$(dirname $(dirname $(uv python find)))/bin:\$PATH\"" >> /root/.bashrc
+
+# Finalize
+WORKDIR /$WORKDIR
+ENTRYPOINT ["/entrypoint.sh"]
+CMD ["sleep", "infinity"]

alliander_robotics/alliander_beamagine/docker-compose.yml

@@ -0,0 +1,19 @@
+# SPDX-FileCopyrightText: Alliander N. V.
+#
+# SPDX-License-Identifier: Apache-2.0
+services:
+  alliander_beamagine:
+    image: allianderrobotics/beamagine
+    container_name: alliander_beamagine
+    runtime: nvidia
+    network_mode: host
+    privileged: true
+    mem_limit: 6gb
+    tty: true
+    env_file:
+      - .env
+    volumes:
+      - "/tmp/.X11-unix:/tmp/.X11-unix"
+      - "/dev:/dev"
+    command:
+      ["/bin/bash", "-c", "ros2 launch alliander_beamagine beamagine.launch.py"]

alliander_robotics/alliander_beamagine/src/alliander_beamagine/CMakeLists.txt

@@ -0,0 +1,24 @@
+# SPDX-FileCopyrightText: Alliander N. V.
+#
+# SPDX-License-Identifier: Apache-2.0
+
+cmake_minimum_required(VERSION 3.5)
+project(alliander_beamagine)
+
+# CMake dependencies:
+find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_python REQUIRED)
+
+# Shared folders:
+install(
+  DIRECTORY launch
+  DESTINATION share/${PROJECT_NAME}
+)
+
+# Default test:
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

alliander_robotics/alliander_beamagine/src/alliander_beamagine/launch/beamagine.launch.py

@@ -0,0 +1,79 @@
+# SPDX-FileCopyrightText: Alliander N. V.
+#
+# SPDX-License-Identifier: Apache-2.0
+
+from alliander_utilities.config_objects import Beamagine
+from alliander_utilities.launch_argument import LaunchArgument
+from alliander_utilities.launch_utils import SKIP, state_publisher_node, static_tf_node
+from alliander_utilities.register import Register, RegisteredLaunchDescription
+from alliander_utilities.ros_utils import get_file_path
+from launch import LaunchContext, LaunchDescription
+from launch.actions import OpaqueFunction
+from launch_ros.actions import Node
+
+platform_arg = LaunchArgument("platform_config", "")
+
+
+def launch_setup(context: LaunchContext) -> list:
+    """The launch setup.
+
+    Args:
+        context (LaunchContext): The launch context.
+
+    Returns:
+        list: The actions to start.
+    """
+    beamagine_config = Beamagine.from_str(platform_arg.string_value(context))
+
+    state_publisher = state_publisher_node(
+        namespace=beamagine_config.namespace,
+        platform="beamagine",
+        xacro="l3cam.urdf.xacro",
+        xacro_arguments={
+            "namespace": beamagine_config.namespace,
+            "parent": "" if beamagine_config.parent.link else "world",
+            "use_sim": str(beamagine_config.simulation),
+        },
+    )
+
+    parent = beamagine_config.parent
+    static_tf = static_tf_node(
+        parent_frame=f"{parent.namespace}/{parent.link}" if parent.link else "map",
+        child_frame=f"{beamagine_config.namespace}/{parent.connects_to}",
+        position=beamagine_config.position,
+        orientation=beamagine_config.orientation,
+    )
+
+    hardware = RegisteredLaunchDescription(
+        get_file_path("alliander_beamagine", ["launch"], "hardware.launch.py"),
+        {"platform_config": beamagine_config.to_str()},
+    )
+
+    convert_32FC1_to_16UC1 = Node(  # noqa: N806
+        package="alliander_utilities",
+        executable="convert_32FC1_to_16UC1",
+        namespace=beamagine_config.namespace,
+    )
+
+    return [
+        Register.on_start(state_publisher, context),
+        Register.on_start(static_tf, context),
+        Register.on_start(convert_32FC1_to_16UC1, context)
+        if beamagine_config.simulation
+        else SKIP,
+        Register.group(hardware, context) if not beamagine_config.simulation else SKIP,
+    ]
+
+
+def generate_launch_description() -> LaunchDescription:
+    """Generate the launch description.
+
+    Returns:
+        LaunchDescription: The launch description.
+    """
+    return LaunchDescription(
+        [
+            platform_arg.declaration,
+            OpaqueFunction(function=launch_setup),
+        ]
+    )

alliander_robotics/alliander_beamagine/src/alliander_beamagine/launch/hardware.launch.py

@@ -0,0 +1,63 @@
+# SPDX-FileCopyrightText: Alliander N. V.
+#
+# SPDX-License-Identifier: Apache-2.0
+from alliander_utilities.config_objects import Beamagine
+from alliander_utilities.launch_argument import LaunchArgument
+from alliander_utilities.launch_utils import static_tf_node
+from alliander_utilities.register import Register, RegisteredLaunchDescription
+from alliander_utilities.ros_utils import get_file_path
+from launch import LaunchContext, LaunchDescription
+from launch.actions import OpaqueFunction
+
+platform_arg = LaunchArgument("platform_config", "")
+
+
+def launch_setup(context: LaunchContext) -> list:
+    """The launch setup.
+
+    Args:
+        context (LaunchContext): The launch context.
+
+    Returns:
+        list: The actions to start.
+    """
+    beamagine_config = Beamagine.from_str(platform_arg.string_value(context))
+
+    l3cam_launch = RegisteredLaunchDescription(
+        launch_description_source=get_file_path(
+            "l3cam_ros2", ["launch"], "l3cam_launch.xml"
+        ),
+        launch_arguments={
+            "stream": "true",
+            "configure": "true",
+            "rviz2": "false",
+            "rqt_reconfigure": "false",
+        },
+    )
+
+    parent = beamagine_config.parent
+    static_tf = static_tf_node(
+        parent_frame=f"{parent.namespace}/{parent.link}" if parent.link else "map",
+        child_frame="lidar",
+        position=beamagine_config.position,
+        orientation=beamagine_config.orientation,
+    )
+
+    return [
+        Register.on_start(static_tf, context),
+        Register.group(l3cam_launch, context),
+    ]
+
+
+def generate_launch_description() -> LaunchDescription:
+    """Generate the launch description.
+
+    Returns:
+        LaunchDescription: The launch description.
+    """
+    return LaunchDescription(
+        [
+            platform_arg.declaration,
+            OpaqueFunction(function=launch_setup),
+        ]
+    )

alliander_robotics/alliander_beamagine/src/alliander_beamagine/package.xml

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+
+<!--
+SPDX-FileCopyrightText: Alliander N. V.
+
+SPDX-License-Identifier: Apache-2.0
+-->
+
+<package format="3">
+  <name>alliander_beamagine</name>
+  <version>0.1.0</version>
+  <description>Contains the integration for Beamagine L3Cam.</description>
+  <maintainer email="robots@alliander.com">Alliander Robotics</maintainer>
+  <license>Apache 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>ament_cmake_python</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

alliander_robotics/alliander_core/src/alliander_description/CMakeLists.txt

@@ -11,7 +11,7 @@ find_package(ament_cmake_python REQUIRED)
 
 # Shared folders:
 install(
-  DIRECTORY ewellix franka husarion nmea_gps ouster realsense seekthermal velodyne ur xsens zed
+  DIRECTORY ewellix beamagine franka husarion nmea_gps ouster realsense seekthermal velodyne ur xsens zed
   DESTINATION share/${PROJECT_NAME}
 )