Merge pull request #607 from Amronos/ros2-jazzy-backport

Backport Some Changes from ros2 to jazzy

ros_gz_bridge/CMakeLists.txt

@@ -22,6 +22,9 @@ find_package(gz-transport REQUIRED)
 find_package(gz_msgs_vendor REQUIRED)
 find_package(gz-msgs REQUIRED)
 
+# Install the python module for this package
+ament_python_install_package(${PROJECT_NAME})
+
 set(GZ_MSGS_VERSION_FULL ${gz-msgs_VERSION})
 
 set(BRIDGE_MESSAGE_TYPES
@@ -133,6 +136,11 @@ install(
   DESTINATION include/${PROJECT_NAME}
 )
 
+install(
+  DIRECTORY launch/
+  DESTINATION share/${PROJECT_NAME}/launch
+)
+
 set(bridge_executables
   parameter_bridge
   static_bridge

ros_gz_bridge/launch/ros_gz_bridge.launch

@@ -0,0 +1,18 @@
+<launch>
+  <arg name="bridge_name" />
+  <arg name="config_file" />
+  <arg name="container_name" default="ros_gz_container" />
+  <arg name="namespace" default="" />
+  <arg name="use_composition" default="True" />
+  <arg name="use_respawn" default="False" />
+  <arg name="log_level" default="info" />
+  <ros_gz_bridge
+    bridge_name="$(var bridge_name)"
+    config_file="$(var config_file)"
+    container_name="$(var container_name)"
+    namespace="$(var namespace)"
+    use_composition="$(var use_composition)"
+    use_respawn="$(var use_respawn)"
+    log_level="$(var log_level)">
+  </ros_gz_bridge>
+</launch>

ros_gz_bridge/launch/ros_gz_bridge.launch.py

@@ -0,0 +1,114 @@
+# Copyright 2024 Open Source Robotics Foundation, Inc.
+#
+# 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.
+
+"""Launch ros_gz bridge in a component container."""
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, GroupAction
+from launch.conditions import IfCondition
+from launch.substitutions import LaunchConfiguration, PythonExpression
+from launch_ros.actions import LoadComposableNodes, Node
+from launch_ros.descriptions import ComposableNode
+
+
+def generate_launch_description():
+
+    bridge_name = LaunchConfiguration('bridge_name')
+    config_file = LaunchConfiguration('config_file')
+    container_name = LaunchConfiguration('container_name')
+    namespace = LaunchConfiguration('namespace')
+    use_composition = LaunchConfiguration('use_composition')
+    use_respawn = LaunchConfiguration('use_respawn')
+    log_level = LaunchConfiguration('log_level')
+
+    declare_bridge_name_cmd = DeclareLaunchArgument(
+        'bridge_name', description='Name of ros_gz_bridge node'
+    )
+
+    declare_config_file_cmd = DeclareLaunchArgument(
+        'config_file', description='YAML config file'
+    )
+
+    declare_container_name_cmd = DeclareLaunchArgument(
+        'container_name',
+        default_value='ros_gz_container',
+        description='Name of container that nodes will load in if use composition',
+    )
+
+    declare_namespace_cmd = DeclareLaunchArgument(
+        'namespace', default_value='', description='Top-level namespace'
+    )
+
+    declare_use_composition_cmd = DeclareLaunchArgument(
+        'use_composition', default_value='True', description='Use composed bringup if True'
+    )
+
+    declare_use_respawn_cmd = DeclareLaunchArgument(
+        'use_respawn',
+        default_value='False',
+        description='Whether to respawn if a node crashes. Applied when composition is disabled.',
+    )
+
+    declare_log_level_cmd = DeclareLaunchArgument(
+        'log_level', default_value='info', description='log level'
+    )
+
+    load_nodes = GroupAction(
+        condition=IfCondition(PythonExpression(['not ', use_composition])),
+        actions=[
+            Node(
+                package='ros_gz_bridge',
+                executable='bridge_node',
+                name=bridge_name,
+                namespace=namespace,
+                output='screen',
+                respawn=use_respawn,
+                respawn_delay=2.0,
+                parameters=[{'config_file': config_file}],
+                arguments=['--ros-args', '--log-level', log_level],
+            ),
+        ],
+    )
+
+    load_composable_nodes = LoadComposableNodes(
+        condition=IfCondition(use_composition),
+        target_container=container_name,
+        composable_node_descriptions=[
+            ComposableNode(
+                package='ros_gz_bridge',
+                plugin='ros_gz_bridge::RosGzBridge',
+                name=bridge_name,
+                namespace=namespace,
+                parameters=[{'config_file': config_file}],
+                extra_arguments=[{'use_intra_process_comms': True}],
+            ),
+        ],
+    )
+
+    # Create the launch description and populate
+    ld = LaunchDescription()
+
+    # Declare the launch options
+    ld.add_action(declare_bridge_name_cmd)
+    ld.add_action(declare_config_file_cmd)
+    ld.add_action(declare_container_name_cmd)
+    ld.add_action(declare_namespace_cmd)
+    ld.add_action(declare_use_composition_cmd)
+    ld.add_action(declare_use_respawn_cmd)
+    ld.add_action(declare_log_level_cmd)
+    # Add the actions to launch all of the bridge nodes
+    ld.add_action(load_nodes)
+    ld.add_action(load_composable_nodes)
+
+    return ld

ros_gz_bridge/package.xml

@@ -11,14 +11,18 @@
 
   <author>Shivesh Khaitan</author>
   <author>Louise Poubel</author>
+  <author email="[email protected]">Carlos Agüero</author>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>ament_cmake_python</buildtool_depend>
   <buildtool_depend>pkg-config</buildtool_depend>
   <buildtool_depend>rosidl_pycommon</buildtool_depend>
 
   <depend>actuator_msgs</depend>
   <depend>geometry_msgs</depend>
   <depend>gps_msgs</depend>
+  <depend>launch</depend>
+  <depend>launch_ros</depend>
   <depend>nav_msgs</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>

ros_gz_bridge/ros_gz_bridge/__init__.py

@@ -20,6 +20,12 @@
 
 from rosidl_pycommon import expand_template
 
+from . import actions
+
+__all__ = [
+    'actions',
+]
+
 
 @dataclass
 class MessageMapping:

ros_gz_bridge/ros_gz_bridge/actions/__init__.py

@@ -0,0 +1,22 @@
+# Copyright 2024 Open Source Robotics Foundation, Inc.
+#
+# 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.
+
+"""Actions module."""
+
+from .ros_gz_bridge import RosGzBridge
+
+
+__all__ = [
+    'RosGzBridge',
+]

ros_gz_bridge/ros_gz_bridge/actions/ros_gz_bridge.py

@@ -0,0 +1,147 @@
+# Copyright 2024 Open Source Robotics Foundation, Inc.
+#
+# 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.
+
+"""Module for the ros_gz bridge action."""
+
+from typing import List
+from typing import Optional
+
+from launch.action import Action
+from launch.actions import IncludeLaunchDescription
+from launch.frontend import Entity, expose_action, Parser
+from launch.launch_context import LaunchContext
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.some_substitutions_type import SomeSubstitutionsType
+from launch.substitutions import PathJoinSubstitution
+from launch_ros.substitutions import FindPackageShare
+
+
+@expose_action('ros_gz_bridge')
+class RosGzBridge(Action):
+    """Action that executes a ros_gz bridge ROS [composable] node."""
+
+    def __init__(
+        self,
+        *,
+        bridge_name: SomeSubstitutionsType,
+        config_file: SomeSubstitutionsType,
+        container_name: Optional[SomeSubstitutionsType] = 'ros_gz_container',
+        namespace: Optional[SomeSubstitutionsType] = '',
+        use_composition: Optional[SomeSubstitutionsType] = 'True',
+        use_respawn: Optional[SomeSubstitutionsType] = 'False',
+        log_level: Optional[SomeSubstitutionsType] = 'info',
+        **kwargs
+    ) -> None:
+        """
+        Construct a ros_gz bridge action.
+
+        All arguments are forwarded to `ros_gz_bridge.launch.ros_gz_bridge.launch.py`,
+        so see the documentation of that class for further details.
+
+        :param: bridge_name Name of ros_gz_bridge  node
+        :param: config_file YAML config file.
+        :param: container_name Name of container that nodes will load in if use composition.
+        :param: namespace Top-level namespace.
+        :param: use_composition Use composed bringup if True.
+        :param: use_respawn Whether to respawn if a node crashes (when composition is disabled).
+        :param: log_level Log level.
+        """
+        super().__init__(**kwargs)
+        self.__bridge_name = bridge_name
+        self.__config_file = config_file
+        self.__container_name = container_name
+        self.__namespace = namespace
+        self.__use_composition = use_composition
+        self.__use_respawn = use_respawn
+        self.__log_level = log_level
+
+    @classmethod
+    def parse(cls, entity: Entity, parser: Parser):
+        """Parse ros_gz_bridge."""
+        _, kwargs = super().parse(entity, parser)
+
+        bridge_name = entity.get_attr(
+            'bridge_name', data_type=str,
+            optional=False)
+
+        config_file = entity.get_attr(
+            'config_file', data_type=str,
+            optional=False)
+
+        container_name = entity.get_attr(
+            'container_name', data_type=str,
+            optional=True)
+
+        namespace = entity.get_attr(
+            'namespace', data_type=str,
+            optional=True)
+
+        use_composition = entity.get_attr(
+            'use_composition', data_type=str,
+            optional=True)
+
+        use_respawn = entity.get_attr(
+            'use_respawn', data_type=str,
+            optional=True)
+
+        log_level = entity.get_attr(
+            'log_level', data_type=str,
+            optional=True)
+
+        if isinstance(bridge_name, str):
+            bridge_name = parser.parse_substitution(bridge_name)
+            kwargs['bridge_name'] = bridge_name
+
+        if isinstance(config_file, str):
+            config_file = parser.parse_substitution(config_file)
+            kwargs['config_file'] = config_file
+
+        if isinstance(container_name, str):
+            container_name = parser.parse_substitution(container_name)
+            kwargs['container_name'] = container_name
+
+        if isinstance(namespace, str):
+            namespace = parser.parse_substitution(namespace)
+            kwargs['namespace'] = namespace
+
+        if isinstance(use_composition, str):
+            use_composition = parser.parse_substitution(use_composition)
+            kwargs['use_composition'] = use_composition
+
+        if isinstance(use_respawn, str):
+            use_respawn = parser.parse_substitution(use_respawn)
+            kwargs['use_respawn'] = use_respawn
+
+        if isinstance(log_level, str):
+            log_level = parser.parse_substitution(log_level)
+            kwargs['log_level'] = log_level
+
+        return cls, kwargs
+
+    def execute(self, context: LaunchContext) -> Optional[List[Action]]:
+        """Execute the action."""
+        ros_gz_bridge_description = IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                [PathJoinSubstitution([FindPackageShare('ros_gz_bridge'),
+                                       'launch',
+                                       'ros_gz_bridge.launch.py'])]),
+            launch_arguments=[('bridge_name', self.__bridge_name),
+                              ('config_file', self.__config_file),
+                              ('container_name', self.__container_name),
+                              ('namespace',   self.__namespace),
+                              ('use_composition',  self.__use_composition),
+                              ('use_respawn',  self.__use_respawn),
+                              ('log_level',   self.__log_level), ])
+
+        return [ros_gz_bridge_description]

ros_gz_bridge/setup.cfg

@@ -0,0 +1,3 @@
+[options.entry_points]
+launch.frontend.launch_extension =
+  ros_gz_bridge = ros_gz_bridge

ros_gz_bridge/src/factory.hpp

@@ -22,6 +22,7 @@
 #include <type_traits>
 
 #include <gz/transport/Node.hh>
+#include <gz/transport/SubscribeOptions.hh>
 
 // include ROS 2
 #include <rclcpp/rclcpp.hpp>
@@ -120,18 +121,16 @@ class Factory : public FactoryInterface
     rclcpp::PublisherBase::SharedPtr ros_pub,
     bool override_timestamps_with_wall_time)
   {
-    std::function<void(const GZ_T &,
-      const gz::transport::MessageInfo &)> subCb =
-      [this, ros_pub, override_timestamps_with_wall_time](const GZ_T & _msg,
-      const gz::transport::MessageInfo & _info)
+    std::function<void(const GZ_T &)> subCb =
+      [this, ros_pub, override_timestamps_with_wall_time](const GZ_T & _msg)
       {
-        // Ignore messages that are published from this bridge.
-        if (!_info.IntraProcess()) {
-          this->gz_callback(_msg, ros_pub, override_timestamps_with_wall_time);
-        }
+        this->gz_callback(_msg, ros_pub, override_timestamps_with_wall_time);
       };
 
-    node->Subscribe(topic_name, subCb);
+    // Ignore messages that are published from this bridge.
+    gz::transport::SubscribeOptions opts;
+    opts.SetIgnoreLocalMessages(true);
+    node->Subscribe(topic_name, subCb, opts);
   }
 
 protected: