WIP: Testing launch with gz_server and ros_gz_bridge composable nodes

Signed-off-by: Addisu Z. Taddese <[email protected]>

ros_gz_bridge/launch/ros_gz_bridge.launch.py

@@ -18,7 +18,7 @@
 from launch.actions import DeclareLaunchArgument, GroupAction
 from launch.conditions import IfCondition
 from launch.substitutions import LaunchConfiguration, PythonExpression
-from launch_ros.actions import ComposableNodeContainer, Node
+from launch_ros.actions import LoadComposableNodes, Node
 from launch_ros.descriptions import ComposableNode
 
 
@@ -74,22 +74,21 @@ def generate_launch_description():
         ],
     )
 
-    load_composable_nodes = ComposableNodeContainer(
+    load_composable_nodes = GroupAction(
         condition=IfCondition(use_composition),
-        name=container_name,
-        namespace=namespace,
-        package='rclcpp_components',
-        executable='component_container',
-        composable_node_descriptions=[
-            ComposableNode(
-                package='ros_gz_bridge',
-                plugin='ros_gz_bridge::RosGzBridge',
-                name='ros_gz_bridge',
-                parameters=[{'config_file': config_file}],
-                extra_arguments=[{'use_intra_process_comms': True}],
-            ),
-        ],
-        output='screen',
+        actions=[LoadComposableNodes(
+            target_container=container_name,
+            composable_node_descriptions=[
+                ComposableNode(
+                    package='ros_gz_bridge',
+                    plugin='ros_gz_bridge::RosGzBridge',
+                    name='ros_gz_bridge',
+                    parameters=[{'config_file': config_file}],
+                    extra_arguments=[{'use_intra_process_comms': True,
+                                      'output': 'screen'}],
+                ),
+            ],
+        )]
     )
 
     # Create the launch description and populate

ros_gz_bridge/src/bridge_handle_gz_to_ros.cpp

@@ -65,6 +65,7 @@ bool BridgeHandleGzToRos::HasSubscriber() const
 
 void BridgeHandleGzToRos::StartSubscriber()
 {
+  std::cout << "Creating gz subscriber " << this->config_.gz_topic_name << std::endl;
   // Start Gazebo subscriber
   this->factory_->create_gz_subscriber(
     this->gz_node_,

ros_gz_bridge/src/factory.hpp

@@ -110,9 +110,9 @@ class Factory : public FactoryInterface
       [this, ros_pub](const GZ_T & _msg, const gz::transport::MessageInfo & _info)
       {
         // Ignore messages that are published from this bridge.
-        if (!_info.IntraProcess()) {
+        /* if (!_info.IntraProcess()) { */
           this->gz_callback(_msg, ros_pub);
-        }
+        /* } */
       };
 
     node->Subscribe(topic_name, subCb);

ros_gz_sim/launch/ros_gz_sim.launch.py

@@ -15,10 +15,12 @@
 """Launch gzsim + ros_gz_bridge in a component container."""
 
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, TextSubstitution
-from launch_ros.substitutions import FindPackageShare
+from launch.actions import DeclareLaunchArgument, GroupAction
+from launch.conditions import IfCondition
+from launch.substitutions import (
+    LaunchConfiguration, TextSubstitution, PythonExpression)
+from launch_ros.actions import ComposableNodeContainer, Node
+from launch_ros.descriptions import ComposableNode
 
 
 def generate_launch_description():
@@ -71,27 +73,54 @@ def generate_launch_description():
         description='SDF world string'
     )
 
-    bridge_description = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            [PathJoinSubstitution([FindPackageShare('ros_gz_bridge'),
-                                   'launch',
-                                   'ros_gz_bridge.launch.py'])]),
-        launch_arguments=[('config_file', config_file),
-                          ('container_name', container_name),
-                          ('namespace', namespace),
-                          ('use_composition', use_composition),
-                          ('use_respawn', use_respawn),
-                          ('bridge_log_level', bridge_log_level)])
-
-    gz_server_description = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            [PathJoinSubstitution([FindPackageShare('ros_gz_sim'),
-                                   'launch',
-                                   'gz_server.launch.py'])]),
-        launch_arguments=[('world_sdf_file', world_sdf_file),
-                          ('world_sdf_string', world_sdf_string),
-                          ('use_composition', use_composition), ])
+    load_nodes = GroupAction(
+        condition=IfCondition(PythonExpression(['not ', use_composition])),
+        actions=[
+            Node(
+                package='ros_gz_bridge',
+                executable='bridge_node',
+                output='screen',
+                respawn=use_respawn,
+                respawn_delay=2.0,
+                parameters=[{'config_file': config_file}],
+            ),
+            Node(
+                condition=IfCondition(PythonExpression(['not ', use_composition])),
+                package='ros_gz_sim',
+                executable='gzserver',
+                output='screen',
+                parameters=[{'world_sdf_file': world_sdf_file,
+                            'world_sdf_string': world_sdf_string}],
+            )
+        ],
+    )
 
+    composable_nodes = ComposableNodeContainer(
+        condition=IfCondition(use_composition),
+        name=container_name,
+        namespace=namespace,
+        package='rclcpp_components',
+        executable='component_container_mt',
+        composable_node_descriptions=[
+            ComposableNode(
+                package='ros_gz_sim',
+                plugin='ros_gz_sim::GzServer',
+                name='gz_server',
+                parameters=[{'world_sdf_file': world_sdf_file,
+                             'world_sdf_string': world_sdf_string}],
+                extra_arguments=[{'use_intra_process_comms': True}],
+            ),
+            ComposableNode(
+                package='ros_gz_bridge',
+                plugin='ros_gz_bridge::RosGzBridge',
+                name='ros_gz_bridge',
+                parameters=[{'config_file': config_file}],
+                extra_arguments=[{'use_intra_process_comms': True,
+                                  'output': 'screen'}],
+            ),
+        ],
+        output='screen',
+    )
     # Create the launch description and populate
     ld = LaunchDescription()
 
@@ -105,7 +134,7 @@ def generate_launch_description():
     ld.add_action(declare_world_sdf_file_cmd)
     ld.add_action(declare_world_sdf_string_cmd)
     # Add the actions to launch all of the bridge + gz_server nodes
-    ld.add_action(bridge_description)
-    ld.add_action(gz_server_description)
+    ld.add_action(load_nodes)
+    ld.add_action(composable_nodes)
 
     return ld

ros_gz_sim_demos/CMakeLists.txt

@@ -15,6 +15,12 @@ install(
   DESTINATION share/${PROJECT_NAME}/launch
 )
 
+install(
+  DIRECTORY
+    config/
+  DESTINATION share/${PROJECT_NAME}/config
+)
+
 install(
   DIRECTORY
     rviz/

ros_gz_sim_demos/config/imu.yaml

@@ -0,0 +1,5 @@
+---
+- topic_name: "/imu"
+  ros_type_name: "sensor_msgs/msg/Imu"
+  gz_type_name: "gz.msgs.IMU"
+  direction: GZ_TO_ROS

ros_gz_sim_demos/config/rgbd_camera_bridge.yaml

@@ -0,0 +1,33 @@
+---
+
+# - ros_topic_name: "/camera/image"
+#   gz_topic_name: "/camera"
+#   ros_type_name: "sensor_msgs/msg/Image"
+#   gz_type_name: "gz.msgs.Image"
+#   direction: GZ_TO_ROS
+#
+# - ros_topic_name: "/camera/camera_info"
+#   gz_topic_name: "/camera_info"
+#   ros_type_name: "sensor_msgs/msg/Image"
+#   gz_type_name: "gz.msgs.Image"
+#   direction: GZ_TO_ROS
+#
+# - topic_name: "/rgbd_camera/image"
+#   ros_type_name: "sensor_msgs/msg/Image"
+#   gz_type_name: "gz.msgs.Image"
+#   direction: GZ_TO_ROS
+#
+# - topic_name: "/rgbd_camera/camera_info"
+#   ros_type_name: "sensor_msgs/msg/CameraInfo"
+#   gz_type_name: "gz.msgs.CameraInfo"
+#   direction: GZ_TO_ROS
+#
+# - topic_name: "/rgbd_camera/depth_image"
+#   ros_type_name: "sensor_msgs/msg/Image"
+#   gz_type_name: "gz.msgs.Image"
+#   direction: GZ_TO_ROS
+
+- topic_name: "/rgbd_camera/points"
+  ros_type_name: "sensor_msgs/msg/PointCloud2"
+  gz_type_name: "gz.msgs.PointCloudPacked"
+  direction: GZ_TO_ROS

ros_gz_sim_demos/launch/imu.launch.py

@@ -34,7 +34,7 @@ def generate_launch_description():
         PythonLaunchDescriptionSource(
             os.path.join(pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py')),
         launch_arguments={
-            'gz_args': '-r sensors.sdf'
+            'gz_args': '-s -r sensors.sdf'
         }.items(),
     )
 

ros_gz_sim_demos/launch/imu_composition.launch.py

@@ -0,0 +1,46 @@
+# Copyright 2019 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.
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+
+def generate_launch_description():
+
+    pkg_ros_gz_sim = get_package_share_directory('ros_gz_sim')
+    pkg_ros_gz_sim_demos = get_package_share_directory('ros_gz_sim_demos')
+
+    gz_sim = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_ros_gz_sim, 'launch', 'ros_gz_sim.launch.py')),
+        launch_arguments={
+            'world_sdf_file': 'sensors.sdf',
+            'config_file': os.path.join(pkg_ros_gz_sim_demos,
+                                        'config', 'imu.yaml'),
+        }.items(),
+    )
+
+    return LaunchDescription([
+        gz_sim,
+        DeclareLaunchArgument(
+            'rviz', default_value='true', description='Open RViz.'
+        ),
+        # rviz
+    ])

ros_gz_sim_demos/launch/rgbd_camera_bridge.launch.py

@@ -35,7 +35,7 @@ def generate_launch_description():
         PythonLaunchDescriptionSource(
             os.path.join(pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py')),
         launch_arguments={
-            'gz_args': '-r sensors_demo.sdf'
+            'gz_args': '-s -r sensors_demo.sdf'
         }.items(),
     )
 

ros_gz_sim_demos/launch/rgbd_camera_bridge_composition.launch.py

@@ -0,0 +1,59 @@
+# Copyright 2019 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.
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.actions import IncludeLaunchDescription
+from launch.conditions import IfCondition
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+
+    pkg_ros_gz_sim_demos = get_package_share_directory('ros_gz_sim_demos')
+    pkg_ros_gz_sim = get_package_share_directory('ros_gz_sim')
+
+    gz_sim = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_ros_gz_sim, 'launch', 'ros_gz_sim.launch.py')),
+        launch_arguments={
+            'world_sdf_file': 'sensors_demo.sdf',
+            'config_file': os.path.join(pkg_ros_gz_sim_demos,
+                                        'config', 'rgbd_camera_bridge.yaml'),
+        }.items(),
+    )
+
+    # RViz
+    rviz = Node(
+        package='rviz2',
+        executable='rviz2',
+        arguments=[
+            '-d', os.path.join(pkg_ros_gz_sim_demos, 'rviz', 'rgbd_camera_bridge.rviz')
+        ],
+        condition=IfCondition(LaunchConfiguration('rviz'))
+    )
+
+    return LaunchDescription([
+        gz_sim,
+        DeclareLaunchArgument('rviz', default_value='true',
+                              description='Open RViz.'),
+        rviz,
+    ])