From b6bc2a47a7d1223e73fa06716065425636941716 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= <caguero@openrobotics.org>
Date: Fri, 22 Nov 2024 17:47:12 +0100
Subject: [PATCH] Restore python launch file
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Signed-off-by: Carlos Agüero <caguero@openrobotics.org>
---
 ros_gz_sim_demos/README.md                 |  2 +-
 ros_gz_sim_demos/config/battery.yaml       | 14 ++---
 ros_gz_sim_demos/launch/battery.launch.py  | 64 ++++++++++++++++++++++
 ros_gz_sim_demos/launch/battery.launch.xml |  7 ---
 4 files changed, 69 insertions(+), 18 deletions(-)
 create mode 100644 ros_gz_sim_demos/launch/battery.launch.py
 delete mode 100644 ros_gz_sim_demos/launch/battery.launch.xml

diff --git a/ros_gz_sim_demos/README.md b/ros_gz_sim_demos/README.md
index f8409aa3..598f4558 100644
--- a/ros_gz_sim_demos/README.md
+++ b/ros_gz_sim_demos/README.md
@@ -32,7 +32,7 @@ And
 
 Get the current state of a battery.
 
-    ros2 launch ros_gz_sim_demos battery.launch.xml
+    ros2 launch ros_gz_sim_demos battery.launch.py
 
 Then send a command so the vehicle moves and drains the battery.
 
diff --git a/ros_gz_sim_demos/config/battery.yaml b/ros_gz_sim_demos/config/battery.yaml
index 61ad5ae9..9f6494bb 100644
--- a/ros_gz_sim_demos/config/battery.yaml
+++ b/ros_gz_sim_demos/config/battery.yaml
@@ -1,18 +1,12 @@
 # Battery configuration.
-- ros_topic_name: "/model/vehicle_blue/cmd_vel"
-  gz_topic_name: "/model/vehicle_blue/cmd_vel"
+- topic_name: "/model/vehicle_blue/cmd_vel"
   ros_type_name: "geometry_msgs/msg/Twist"
   gz_type_name: "gz.msgs.Twist"
-  subscriber_queue: 5
-  publisher_queue: 6
-  lazy: false
+  lazy: true
   direction: ROS_TO_GZ
 
-- ros_topic_name: "/model/vehicle_blue/battery/linear_battery/state"
-  gz_topic_name: "/model/vehicle_blue/battery/linear_battery/state"
+- topic_name: "/model/vehicle_blue/battery/linear_battery/state"
   ros_type_name: "sensor_msgs/msg/BatteryState"
   gz_type_name: "gz.msgs.BatteryState"
-  subscriber_queue: 5
-  publisher_queue: 6
-  lazy: false
+  lazy: true
   direction: GZ_TO_ROS
diff --git a/ros_gz_sim_demos/launch/battery.launch.py b/ros_gz_sim_demos/launch/battery.launch.py
new file mode 100644
index 00000000..a0da9fdf
--- /dev/null
+++ b/ros_gz_sim_demos/launch/battery.launch.py
@@ -0,0 +1,64 @@
+# 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
+from ros_gz_bridge.actions import RosGzBridge
+
+
+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')
+
+    # RQt
+    rqt = Node(
+        package='rqt_plot',
+        executable='rqt_plot',
+        # FIXME: Why isn't the topic being populated on the UI? RQt issue?
+        arguments=['--force-discover',
+                   '/model/vehicle_blue/battery/linear_battery/state/percentage'],
+        condition=IfCondition(LaunchConfiguration('rqt'))
+    )
+
+    # Gazebo
+    gz_sim = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py')),
+        launch_arguments={
+            'gz_args': '-r -z 1000000 linear_battery_demo.sdf'
+        }.items(),
+    )
+
+    # Bridge
+    ros_gz_bridge = RosGzBridge(
+        bridge_name='ros_gz_bridge',
+        config_file=os.path.join(pkg_ros_gz_sim_demos, 'config', 'battery.yaml'),
+    )
+
+    return LaunchDescription([
+        gz_sim,
+        DeclareLaunchArgument('rqt', default_value='true',
+                              description='Open RQt.'),
+        ros_gz_bridge,
+        rqt
+    ])
diff --git a/ros_gz_sim_demos/launch/battery.launch.xml b/ros_gz_sim_demos/launch/battery.launch.xml
deleted file mode 100644
index c93fcf9c..00000000
--- a/ros_gz_sim_demos/launch/battery.launch.xml
+++ /dev/null
@@ -1,7 +0,0 @@
-<launch>
-  <gz_server world_sdf_file="linear_battery_demo.sdf" use_composition="True" create_own_container="True" />
-  <ros_gz_bridge bridge_name="ros_gz_bridge" config_file="$(find-pkg-share ros_gz_sim_demos)/config/battery.yaml" use_composition="True" />
-  <!-- FIXME: Why isn't the topic being populated on the UI? RQt issue? -->
-  <node pkg="rqt_plot" exec="rqt_plot" args="--force-discover /model/vehicle_blue/battery/linear_battery/state/percentage" />
-  <executable cmd="gz sim -g" />
-</launch>