Ros2 add launch and remappings (#43)

* Fixed prefix Signed-off-by: Jakub Delicat <[email protected]> * Added launches and gz mappings Signed-off-by: Jakub Delicat <[email protected]> * update launches Signed-off-by: Jakub Delicat <[email protected]> * fixed astra Signed-off-by: Jakub Delicat <[email protected]> * removed print Signed-off-by: Jakub Delicat <[email protected]> * Fixed unused file Signed-off-by: Jakub Delicat <[email protected]> * renamed files Signed-off-by: Jakub Delicat <[email protected]> * Added suggestion Signed-off-by: Jakub Delicat <[email protected]> * simplfied urfd | removed use_gpu | removed simulation-engine Signed-off-by: Jakub Delicat <[email protected]> * simplfied urfd | removed use_gpu | removed simulation-engine Signed-off-by: Jakub Delicat <[email protected]> * removed simulation-engine from s3 | fix with no namespace Signed-off-by: Jakub Delicat <[email protected]> * Suggestions * add * workflow simplify * Removed unnecessary topic form realsense Signed-off-by: Jakub Delicat <[email protected]> * Made new tests Signed-off-by: Jakub Delicat <[email protected]> * fixed astra Signed-off-by: Jakub Delicat <[email protected]> --------- Signed-off-by: Jakub Delicat <[email protected]> Co-authored-by: rafal-gorecki <[email protected]> Co-authored-by: rafal-gorecki <[email protected]>
husarion · May 13, 2024 · cdd86f0 · cdd86f0
1 parent 1cb2560
commit cdd86f0
Show file tree

Hide file tree

Showing 27 changed files with 1,375 additions and 933 deletions.
diff --git a/.github/workflows/colcon_test_build.yaml b/.github/workflows/colcon_test_build.yaml
@@ -7,13 +7,10 @@ jobs:
   colcon-test-build-ubuntu-22-04:
     strategy:
       matrix:
-        build-type: [ignition-gazebo, gazebo-classic]
         ros-distro: [humble]
-    env:
-      GAZEBO_VERSION: ${{ matrix.build-type }}
     runs-on: ubuntu-22.04
 
-    name: ${{ matrix.ros-distro }}
+    name: Build ${{ matrix.ros-distro }}
     steps:
       - uses: ros-tooling/[email protected]
         with:

diff --git a/.gitignore b/.gitignore
@@ -1 +1,2 @@
 .vscode
+__pycache__
diff --git a/CMakeLists.txt b/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_xacro.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()
diff --git a/README.md b/README.md
@@ -15,12 +15,6 @@ git clone https://github.com/husarion/ros_components_description.git src/ros_com
 # in case the package will be used within simulation
 export HUSARION_ROS_BUILD_TYPE=simulation
 
-# to specify which simulation engine will be used
-# for gazebo classic
-export SIMULATION_ENGINE=gazebo-classic
-# for ignition gazebo
-export SIMULATION_ENGINE=ignition-gazebo
-
 rosdep update --rosdistro $ROS_DISTRO
 rosdep install -i --from-path src --rosdistro $ROS_DISTRO -y
 colcon build
@@ -36,22 +30,17 @@ To include the sensor, use the following code:
 <xacro:lidar.slamtec_rplidar_s1
   parent_link="cover_link"
   xyz="0.0 0.0 0.0"
-  rpy="0.0 0.0 0.0"
-  use_gpu="true"
-  simulation_engine="gazebo-classic" />
+  rpy="0.0 0.0 0.0" />
 ```
 
 A list of parameters can be found here:
 
 - `parent_link` [*string*, default: **None**] parent link to which sensor should be attached.
 - `xyz` [*float list*, default: **None**] 3 float values defining translation between base of a sensor and parent link. Values in **m**.
 - `rpy` [*float list*, default: **None**] 3 float values define rotation between parent link and base of a sensor. Values in **rad**.
-- `tf_prefix` [*string*, optional] tf prefix applied before all links created by sensor. If defined, applies `<tf_prefix>_<sensor_name>`. If not defined, leaves `<sensor_name>` intact. Applies also to `frame_id` parameter.
-- `topic` [*string*, default: **same as manufacturer's default**] name of topic at which simulated sensor will publish data.
-- `frame_id` [*string*, default: **same as manufacturer's default**] name of final tf to which sensor will be attached. Should match one from message published by sensor.
-- `use_gpu` [*bool*, default: **false**] enable GPU acceleration for sensor. Available only if sensor can be accelerated.
-- `simulation_engine` [*string*, default: **gazebo-classic**] selected for which simulation engine plugins should be loaded. Currently the only supported:
-  - **gazebo-classic** used to select [Gazebo Classic](https://classic.gazebosim.org/).
-  - **ignition-gazebo** used to select [Ignition Gazebo](https://gazebosim.org/home).
+- `namespace` [*string*, default: **None**] global namespace common to the entire robot.
+- `device_namespace` [*string*, default: **None**] local namespace allowing to distinguish two identical devices from each other.
+
+- `model` [*string*, default: **None**] model argument that appears when you want to load the appropriate model from a given manufacturer.
 
 Some sensors can define their specific parameters. Refer to their definition for more info.
diff --git a/config/gz_orbbec_astra_remappings.yaml b/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
diff --git a/config/gz_ouster_os_remappings.yaml b/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
diff --git a/config/gz_slamtec_rplidar_remappings.yaml b/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
diff --git a/config/gz_velodyne_remappings.yaml b/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
diff --git a/launch/gz_components.launch.py b/launch/gz_components.launch.py
@@ -0,0 +1,129 @@
+# 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):
+    device_namespace = get_value(component, "device_namespace")
+    robot_namespace = namespace
+
+    if len(robot_namespace) and robot_namespace[0] != "/":
+        robot_namespace = "/" + robot_namespace
+    if len(device_namespace) and device_namespace[0] != "/":
+        device_namespace = "/" + device_namespace
+
+    return IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([package, "/launch/gz_", name, ".launch.py"]),
+        launch_arguments={
+            "robot_namespace": robot_namespace,
+            "device_namespace": device_namespace,
+            "gz_bridge_name": component["device_namespace"] + "_gz_bridge",
+        }.items(),
+    )
+
+
+def get_launch_descriptions_from_yaml_node(
+    node: yaml.Node, package: os.PathLike, namespace: str
+) -> IncludeLaunchDescription:
+    actions = []
+
+    components_types_with_names = {
+        "LDR01": "slamtec_rplidar",
+        "LDR06": "slamtec_rplidar",
+        "LDR13": "ouster_os",
+        "LDR20": "velodyne",
+        "CAM01": "orbbec_astra",
+    }
+
+    for component in node["components"]:
+        component_type = component["type"]
+        if component_type in components_types_with_names:
+            launch_description = get_launch_description(
+                components_types_with_names[component_type], package, namespace, component
+            )
+            actions.append(launch_description)
+
+    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)
diff --git a/launch/gz_orbbec_astra.launch.py b/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),
+        ]
+    )