Ros2 add launch and remappings

.gitignore

@@ -1 +1,2 @@
 .vscode
+__pycache__

CMakeLists.txt

@@ -4,10 +4,25 @@ project(ros_components_description)
 find_package(ament_cmake REQUIRED)
 
 install(
-  DIRECTORY meshes urdf
+  DIRECTORY meshes urdf launch config test
   DESTINATION share/${PROJECT_NAME}
 )
 
+if(BUILD_TESTING)
+  find_package(ament_cmake_pytest REQUIRED)
+  set(_pytest_tests
+    test/test_components_yaml_parse.py
+  )
+  foreach(_test_path ${_pytest_tests})
+    get_filename_component(_test_name ${_test_path} NAME_WE)
+    ament_add_pytest_test(${_test_name} ${_test_path}
+      APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR}
+      TIMEOUT 60
+      WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
+    )
+  endforeach()
+endif()
+
 ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/env-hooks/${PROJECT_NAME}.sh.in")
 ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
 ament_package()

config/gz_orbbec_astra_remappings.yaml

@@ -0,0 +1,27 @@
+---
+# https://github.com/orbbec/OrbbecSDK_ROS2#all-available-topics
+  - topic_name: <robot_namespace><device_namespace>/<camera_name>/color/camera_info
+    ros_type_name: sensor_msgs/msg/CameraInfo
+    gz_type_name: ignition.msgs.CameraInfo
+    lazy: true
+
+  - topic_name: <robot_namespace><device_namespace>/<camera_name>/color/image_raw
+    ros_type_name: sensor_msgs/msg/Image
+    gz_type_name: ignition.msgs.Image
+    lazy: true
+
+  - topic_name: <robot_namespace><device_namespace>/<camera_name>/depth/camera_info
+    ros_type_name: sensor_msgs/msg/CameraInfo
+    gz_type_name: ignition.msgs.CameraInfo
+    lazy: true
+
+  - topic_name: <robot_namespace><device_namespace>/<camera_name>/depth/image_raw
+    ros_type_name: sensor_msgs/msg/Image
+    gz_type_name: ignition.msgs.Image
+    lazy: true
+
+  - ros_topic_name: <robot_namespace><device_namespace>/<camera_name>/depth/points
+    gz_topic_name: <robot_namespace><device_namespace>/<camera_name>/depth/image_raw/points
+    ros_type_name: sensor_msgs/msg/PointCloud2
+    gz_type_name: ignition.msgs.PointCloudPacked
+    lazy: true

config/gz_ouster_os_remappings.yaml

@@ -0,0 +1,6 @@
+---
+# https://github.com/ouster-lidar/ouster-ros/blob/ros2/README.md#overview
+  - ros_topic_name: <robot_namespace><device_namespace>/ouster/points
+    gz_topic_name: <robot_namespace><device_namespace>/ouster/points/points
+    ros_type_name: sensor_msgs/msg/PointCloud2
+    gz_type_name: ignition.msgs.PointCloudPacked

config/gz_slamtec_rplidar_remappings.yaml

@@ -0,0 +1,4 @@
+---
+  - topic_name: <robot_namespace><device_namespace>/scan
+    ros_type_name: sensor_msgs/msg/LaserScan
+    gz_type_name: ignition.msgs.LaserScan

config/gz_velodyne_remappings.yaml

@@ -0,0 +1,6 @@
+---
+# https://github.com/ros-drivers/velodyne/tree/humble-devel/velodyne_pointcloud#published-topics
+  - ros_topic_name: <robot_namespace><device_namespace>/velodyne_points
+    gz_topic_name: <robot_namespace><device_namespace>/velodyne_points/points
+    ros_type_name: sensor_msgs/msg/PointCloud2
+    gz_type_name: ignition.msgs.PointCloudPacked

launch/gz_components.launch.py

@@ -0,0 +1,120 @@
+# Copyright 2024 Husarion sp. z o.o.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import os
+import yaml
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import (
+    DeclareLaunchArgument,
+    IncludeLaunchDescription,
+    OpaqueFunction,
+)
+
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, EnvironmentVariable
+
+
+def get_value(node: yaml.Node, key: str):
+    try:
+        value = node[key]
+        if value == "None":
+            value = ""
+        return value
+
+    except KeyError:
+        return ""
+
+
+def get_launch_description(name: str, package: str, namespace: str, component: yaml.Node):
+    return IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([package, "/launch/gz_", name, ".launch.py"]),
+        launch_arguments={
+            "robot_namespace": namespace,
+            "device_namespace": get_value(component, "namespace"),
+            "tf_prefix": get_value(component, "tf_prefix"),
+            "gz_bridge_name": component["namespace"][1:] + "_gz_bridge",
+            "camera_name": get_value(component, "name"),
+        }.items(),
+    )
+
+
+def get_launch_descriptions_from_yaml_node(
+    node: yaml.Node, package: os.PathLike, namespace: str
+) -> IncludeLaunchDescription:
+    actions = []
+    for component in node["components"]:
+        if component["type"] == "LDR01" or component["type"] == "LDR06":
+            actions.append(get_launch_description("slamtec_rplidar", package, namespace, component))
+
+        if component["type"] == "LDR13":
+            actions.append(get_launch_description("ouster_os", package, namespace, component))
+
+        if component["type"] == "LDR20":
+            actions.append(get_launch_description("velodyne", package, namespace, component))
+
+        if component["type"] == "CAM01":
+            actions.append(get_launch_description("orbbec_astra", package, namespace, component))
+
+
+    return actions
+
+
+def launch_setup(context, *args, **kwargs):
+    ros_components_description = get_package_share_directory("ros_components_description")
+
+    components_config_path = LaunchConfiguration("components_config_path").perform(context)
+    namespace = LaunchConfiguration("namespace").perform(context)
+
+    components_config = None
+    if components_config_path == "None":
+        return []
+
+    with open(os.path.join(components_config_path), 'r') as file:
+        components_config = yaml.safe_load(file)
+
+    actions = []
+    if components_config != None:
+        actions += get_launch_descriptions_from_yaml_node(
+            components_config, ros_components_description, namespace
+        )
+
+    return actions
+
+
+def generate_launch_description():
+    declare_components_config_path_arg = DeclareLaunchArgument(
+        "components_config_path",
+        default_value="None",
+        description=(
+            "Additional components configuration file. Components described in this file "
+            "are dynamically included in Panther's urdf."
+            "Panther options are described here "
+            "https://husarion.com/manuals/panther/panther-options/"
+        ),
+    )
+
+    declare_namespace_arg = DeclareLaunchArgument(
+        "namespace",
+        default_value=EnvironmentVariable("ROBOT_NAMESPACE", default_value=""),
+        description="Add namespace to all launched nodes",
+    )
+
+    actions = [
+        declare_components_config_path_arg,
+        declare_namespace_arg,
+        OpaqueFunction(function=launch_setup),
+    ]
+
+    return LaunchDescription(actions)

launch/gz_orbbec_astra.launch.py

@@ -0,0 +1,130 @@
+# Copyright 2024 Husarion sp. z o.o.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import os
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, OpaqueFunction
+from launch_ros.actions import Node
+from launch.substitutions import EnvironmentVariable, LaunchConfiguration
+from nav2_common.launch import ReplaceString
+from ament_index_python import get_package_share_directory
+
+
+# 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
+def fix_depth_image_tf(context, *args, **kwargs):
+    robot_namespace = LaunchConfiguration("robot_namespace").perform(context)
+    device_namespace = LaunchConfiguration("device_namespace").perform(context)
+    tf_prefix = LaunchConfiguration("tf_prefix").perform(context)
+    camera_name = LaunchConfiguration("camera_name").perform(context)
+
+    device_namespace_ext = device_namespace + "/"
+    if device_namespace == "":
+        device_namespace_ext = ""
+
+    tf_prefix_ext = tf_prefix + "_"
+    if tf_prefix == "":
+        tf_prefix_ext = ""
+
+    parent_frame = tf_prefix_ext + camera_name + "_depth_optical_frame"
+    child_frame = (
+        "panther/base_link//"
+        + device_namespace_ext
+        + tf_prefix_ext
+        + camera_name
+        + "_orbbec_astra_depth"
+    )
+
+    static_transform_publisher = Node(
+        package="tf2_ros",
+        executable="static_transform_publisher",
+        name="point_cloud_tf",
+        output="log",
+        arguments=["0", "0", "0", "1.57", "-1.57", "0", parent_frame, child_frame],
+        parameters=[{"use_sim_time": True}],
+        namespace=robot_namespace,
+    )
+    return [static_transform_publisher]
+
+
+def generate_launch_description():
+    ros_components_description = get_package_share_directory("ros_components_description")
+    gz_bridge_config_path = os.path.join(
+        ros_components_description, "config", "gz_orbbec_astra_remappings.yaml"
+    )
+
+    robot_namespace = LaunchConfiguration("robot_namespace")
+    device_namespace = LaunchConfiguration("device_namespace")
+    camera_name = LaunchConfiguration("camera_name")
+    gz_bridge_name = LaunchConfiguration("gz_bridge_name")
+
+    namespaced_gz_bridge_config_path = ReplaceString(
+        source_file=gz_bridge_config_path,
+        replacements={
+            "<robot_namespace>": robot_namespace,
+            "<device_namespace>": device_namespace,
+            "<camera_name>": camera_name,
+        },
+    )
+
+    declare_device_namespace = DeclareLaunchArgument(
+        "device_namespace",
+        default_value="",
+        description="Sensor namespace that will appear before all non absolute topics and TF frames, used for distinguishing multiple cameras on the same robot.",
+    )
+
+    declare_tf_prefix = DeclareLaunchArgument(
+        "tf_prefix",
+        default_value="",
+        description="Prefix added for all links of device. Here used as fix for static transform publisher.",
+    )
+
+    declare_robot_namespace = DeclareLaunchArgument(
+        "robot_namespace",
+        default_value=EnvironmentVariable("ROBOT_NAMESPACE", default_value=""),
+        description="Namespace which will appear in front of all topics (including /tf and /tf_static).",
+    )
+
+    declare_camera_name = DeclareLaunchArgument(
+        "camera_name",
+        default_value="camera",
+        description="Name of the camera. It will appear before all tfs and topics.",
+    )
+
+    declare_gz_bridge_name = DeclareLaunchArgument(
+        "gz_bridge_name",
+        default_value="gz_bridge",
+        description="Name of gz bridge node.",
+    )
+
+    gz_bridge = Node(
+        package="ros_gz_bridge",
+        executable="parameter_bridge",
+        name=gz_bridge_name,
+        parameters=[{"config_file": namespaced_gz_bridge_config_path}],
+        namespace=robot_namespace,
+        output="screen",
+    )
+
+    return LaunchDescription(
+        [
+            declare_device_namespace,
+            declare_robot_namespace,
+            declare_tf_prefix,
+            declare_camera_name,
+            declare_gz_bridge_name,
+            gz_bridge,
+            OpaqueFunction(function=fix_depth_image_tf),
+        ]
+    )