Merge branch 'ros2' into rplidar-merge

config/gz_orbbec_astra_remappings.yaml

@@ -1,31 +1,27 @@
 ---
 # https://github.com/orbbec/OrbbecSDK_ROS2#all-available-topics
-- ros_topic_name: <robot_namespace><device_namespace>/<camera_name>/color/camera_info
-  gz_topic_name: <robot_namespace><device_namespace>/color/camera_info
+- topic_name: <robot_namespace><device_namespace>/color/camera_info
   ros_type_name: sensor_msgs/msg/CameraInfo
   gz_type_name: ignition.msgs.CameraInfo
   lazy: true
 
-- ros_topic_name: <robot_namespace><device_namespace>/<camera_name>/color/image_raw
-  gz_topic_name: <robot_namespace><device_namespace>/color/image_raw
+- topic_name: <robot_namespace><device_namespace>/color/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/camera_info
-  gz_topic_name: <robot_namespace><device_namespace>/depth/camera_info
+- topic_name: <robot_namespace><device_namespace>/depth/camera_info
   ros_type_name: sensor_msgs/msg/CameraInfo
   gz_type_name: ignition.msgs.CameraInfo
   lazy: true
 
-- ros_topic_name: <robot_namespace><device_namespace>/<camera_name>/depth/image_raw
-  gz_topic_name: <robot_namespace><device_namespace>/depth/image_raw
+- topic_name: <robot_namespace><device_namespace>/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_ros_topic_name: <robot_namespace><device_namespace>/depth/image_raw/points
+- ros_topic_name: <robot_namespace><device_namespace>/depth/points
+  gz_topic_name: <robot_namespace><device_namespace>/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

@@ -4,3 +4,4 @@
     gz_topic_name: <robot_namespace><device_namespace>/ouster/points/points
     ros_type_name: sensor_msgs/msg/PointCloud2
     gz_type_name: ignition.msgs.PointCloudPacked
+    lazy: true

config/gz_slamtec_rplidar_remappings.yaml

@@ -2,3 +2,4 @@
   - topic_name: <robot_namespace><device_namespace>/scan
     ros_type_name: sensor_msgs/msg/LaserScan
     gz_type_name: ignition.msgs.LaserScan
+    lazy: true

config/gz_stereolabs_zed_remappings.yaml

@@ -0,0 +1,31 @@
+---
+# https://github.com/stereolabs/zed-ros2-wrapper
+- ros_topic_name: <robot_namespace><device_namespace>/zed_node/rgb/camera_info
+  gz_topic_name: <robot_namespace><device_namespace>/zed_node/rgb/camera_info
+  ros_type_name: sensor_msgs/msg/CameraInfo
+  gz_type_name: ignition.msgs.CameraInfo
+  lazy: true
+
+- ros_topic_name: <robot_namespace><device_namespace>/zed_node/rgb/image_rect_color
+  gz_topic_name: <robot_namespace><device_namespace>/zed_node/rgb/image_rect_color
+  ros_type_name: sensor_msgs/msg/Image
+  gz_type_name: ignition.msgs.Image
+  lazy: true
+
+- ros_topic_name: <robot_namespace><device_namespace>/zed_node/camera_info
+  gz_topic_name: <robot_namespace><device_namespace>/zed_node/camera_info
+  ros_type_name: sensor_msgs/msg/CameraInfo
+  gz_type_name: ignition.msgs.CameraInfo
+  lazy: true
+
+- ros_topic_name: <robot_namespace><device_namespace>/zed_node/depth
+  gz_topic_name: <robot_namespace><device_namespace>/zed_node/depth
+  ros_type_name: sensor_msgs/msg/Image
+  gz_type_name: ignition.msgs.Image
+  lazy: true
+
+- ros_topic_name: <robot_namespace><device_namespace>/zed_node/depth/points
+  gz_ros_topic_name: <robot_namespace><device_namespace>/zed_node/depth/points
+  ros_type_name: sensor_msgs/msg/PointCloud2
+  gz_type_name: ignition.msgs.PointCloudPacked
+  lazy: true

config/gz_velodyne_remappings.yaml

@@ -4,3 +4,4 @@
     gz_topic_name: <robot_namespace><device_namespace>/velodyne_points/points
     ros_type_name: sensor_msgs/msg/PointCloud2
     gz_type_name: ignition.msgs.PointCloudPacked
+    lazy: true

launch/gz_components.launch.py

@@ -73,6 +73,9 @@ def get_launch_descriptions_from_yaml_node(
         "LDR15": "ouster_os",
         "LDR20": "velodyne",
         "CAM01": "orbbec_astra",
+        "CAM03": "stereolabs_zed",
+        "CAM04": "stereolabs_zed",
+        "CAM06": "stereolabs_zed",
         "MAN01": "ur",
         "MAN02": "ur",
         # "MAN03": "kinova_lite"  sim_isaac error

launch/gz_orbbec_astra.launch.py

@@ -26,25 +26,15 @@
 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"
-    )
+
+    if robot_namespace.startswith('/'):
+        robot_namespace = robot_namespace[1:] + "/"
+
+    if device_namespace.startswith('/'):
+        device_namespace = device_namespace[1:]
+
+    parent_frame = robot_namespace + device_namespace + "_depth_optical_frame"
+    child_frame = "panther/base_link/" + robot_namespace + device_namespace + "_orbbec_astra_depth"
 
     static_transform_publisher = Node(
         package="tf2_ros",
@@ -66,15 +56,13 @@ def generate_launch_description():
 
     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,
         },
     )
 
@@ -84,24 +72,12 @@ def generate_launch_description():
         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",
@@ -121,8 +97,6 @@ def generate_launch_description():
         [
             declare_device_namespace,
             declare_robot_namespace,
-            declare_tf_prefix,
-            declare_camera_name,
             declare_gz_bridge_name,
             gz_bridge,
             OpaqueFunction(function=fix_depth_image_tf),

launch/gz_stereolabs_zed.launch.py

@@ -0,0 +1,104 @@
+# 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)
+
+    if robot_namespace.startswith('/'):
+        robot_namespace = robot_namespace[1:] + "/"
+
+    if device_namespace.startswith('/'):
+        device_namespace = device_namespace[1:]
+
+    parent_frame = robot_namespace + device_namespace + "_center_optical_frame"
+    child_frame = "panther/base_link/" + robot_namespace + device_namespace + "_stereolabs_zed_depth"
+
+    static_transform_publisher = Node(
+        package="tf2_ros",
+        executable="static_transform_publisher",
+        name="point_cloud_tf",
+        output="log",
+        arguments=["0", "0", "0", "0", "0", "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_stereolabs_zed_remappings.yaml"
+    )
+
+    robot_namespace = LaunchConfiguration("robot_namespace")
+    device_namespace = LaunchConfiguration("device_namespace")
+    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,
+        },
+    )
+
+    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_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_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_gz_bridge_name,
+            gz_bridge,
+            OpaqueFunction(function=fix_depth_image_tf),
+        ]
+    )

test/test_components_xacro.py

@@ -36,6 +36,9 @@
     "LDR15": ["ouster_os1_128", "os_lidar", "os_lidar", "ouster_os1_128_sensor"],
     "LDR20": ["velodyne_puck", "velodyne", "velodyne", "velodyne_puck_sensor"],
     "CAM01": ["orbbec_astra", "link", "link", "orbbec_astra_color"],
+    "CAM03": ["zed2", "center", "center_optical_frame", "stereolabs_zed_depth"],
+    "CAM04": ["zed2i", "center", "center_optical_frame", "stereolabs_zed_depth"],
+    "CAM06": ["zedi", "center", "center_optical_frame", "stereolabs_zed_depth"],
     "MAN01": ["ur3e", "base_link", "", ""],
     "MAN02": ["ur5e", "base_link", "", ""],
     # "MAN03": ["kinova_lite",               "base_link",    "",         ""], use_isaac error