From f12ac22766dbc901b4d10c3015a69d5b568dab89 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Argentina=20Ortega=20S=C3=A1inz?=
 <argenos@users.noreply.github.com>
Date: Fri, 19 Jul 2024 16:06:29 +0200
Subject: [PATCH 1/2] Add support for ROS2 launch files and Gazebo worlds

---
 src/fpm/cli.py                                | 24 +++++++++++++++--
 src/fpm/generators/ros.py                     |  4 +--
 ...d.launch.jinja => world.ros1.launch.jinja} |  1 +
 src/fpm/templates/ros/world.ros2.launch.jinja | 27 +++++++++++++++++++
 4 files changed, 52 insertions(+), 4 deletions(-)
 rename src/fpm/templates/ros/{world.launch.jinja => world.ros1.launch.jinja} (97%)
 create mode 100644 src/fpm/templates/ros/world.ros2.launch.jinja

diff --git a/src/fpm/cli.py b/src/fpm/cli.py
index 5ad20c2..0c37019 100644
--- a/src/fpm/cli.py
+++ b/src/fpm/cli.py
@@ -65,10 +65,15 @@ def gazebo_world_model(g, model_name, base_path, **kwargs):
     model = {"instances": instances, "name": model_name}
 
     output_path = get_output_path(base_path, "gazebo/worlds")
+    if kwargs.get("ros_version", "ros2") == "ros2":
+        file_name = "{name}.sdf".format(name=model_name)
+    else:
+        file_name = "{name}.world".format(name=model_name)
+
     generate_sdf_file(
         model,
         output_path,
-        "{name}.world".format(name=model_name),
+        file_name,
         template_name="gazebo/world.sdf.jinja",
         template_path=template_path,
     )
@@ -77,10 +82,19 @@ def gazebo_world_model(g, model_name, base_path, **kwargs):
 def gazebo_world_launch(model_name, base_path, **kwargs):
     template_path = kwargs.get("template_path")
     output_path = get_output_path(base_path, "ros/launch")
+
+    if kwargs.get("ros_version", "ros2") == "ros2":
+        file_name = "{name}.ros2.launch".format(name=model_name)
+        template_name = "ros/world.ros2.launch.jinja"
+    else:
+        file_name = "{name}.ros1.launch".format(name=model_name)
+        template_name = "ros/world.ros1.launch.jinja"
+
     generate_launch_file(
         model_name,
         output_path,
-        template_name="ros/world.launch.jinja",
+        file_name,
+        template_name=template_name,
         template_path=template_path,
     )
 
@@ -127,7 +141,13 @@ def get_output_path(base_path, subfolder, model_name=None):
     type=click.Path(exists=True, resolve_path=True),
     default=os.path.join("."),
 )
+@click.option(
+    "--ros-version",
+    type=click.STRING,
+    default="ros2",
+)
 def generate(configfile, inputs, output_path, **kwargs):
+    print(kwargs)
     config = load_config_file(configfile)
 
     g = build_graph_from_directory(inputs)
diff --git a/src/fpm/generators/ros.py b/src/fpm/generators/ros.py
index d0ff2b3..8a64aa5 100644
--- a/src/fpm/generators/ros.py
+++ b/src/fpm/generators/ros.py
@@ -1,7 +1,7 @@
 from fpm.utils import load_template, save_file
 
 
-def generate_launch_file(model_name, output_path, **custom_args):
+def generate_launch_file(model_name, output_path, file_name, **custom_args):
     template_name = custom_args.get("template_name", "world.launch.jinja")
     template_path = custom_args.get("template_path")
     template = load_template(template_name, template_path)
@@ -10,4 +10,4 @@ def generate_launch_file(model_name, output_path, **custom_args):
 
     output = template.render(pkg_path=ros_pkg, model_name=model_name)
 
-    save_file(output_path, "{name}.launch".format(name=model_name), output)
+    save_file(output_path, file_name, output)
diff --git a/src/fpm/templates/ros/world.launch.jinja b/src/fpm/templates/ros/world.ros1.launch.jinja
similarity index 97%
rename from src/fpm/templates/ros/world.launch.jinja
rename to src/fpm/templates/ros/world.ros1.launch.jinja
index 8b0d052..d8bfa65 100644
--- a/src/fpm/templates/ros/world.launch.jinja
+++ b/src/fpm/templates/ros/world.ros1.launch.jinja
@@ -1,5 +1,6 @@
 <?xml version="1.0"?>
 
+<!-- ROS1 Launch File -->
 <launch>
     
     <!-- GAZEBO SETUP ARGUMENTS -->
diff --git a/src/fpm/templates/ros/world.ros2.launch.jinja b/src/fpm/templates/ros/world.ros2.launch.jinja
new file mode 100644
index 0000000..c77d02c
--- /dev/null
+++ b/src/fpm/templates/ros/world.ros2.launch.jinja
@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+
+<!-- ROS2 Launch File - Supports ROS2 Jazzy -->
+<launch>
+    
+
+    <!-- GAZEBO SETUP ARGUMENTS -->
+    <arg name="use_sim_time" default="true"/>
+    <arg name="gazebo_gui" default="false"/>
+    <arg name="headless" default="true"/>
+    <arg name="debug" default="false"/>
+    <arg name="paused" default="false"/>
+    <arg name="world_name" default="worlds/{{model_name}}.world"/>
+
+    <set_env name="GAZEBO_MODEL_PATH" value="$(find-pkg-prefix floorplan_models)/models:$(find-pkg-prefix floorplan_models)/worlds:$(env GAZEBO_MODEL_PATH)"/>
+
+    <!-- launch gazebo with empty world -->
+    <include file="$(find floorplan_models)/launch/gazebo_world.launch">
+        <let name="world_name" value="$(var world_name)"/>
+        <let name="paused" value="$(var paused)"/>
+        <let name="use_sim_time" value="$(var use_sim_time)"/>
+        <let name="gui" value="$(var gazebo_gui)"/>
+        <let name="headless" value="$(var headless)"/>
+        <let name="debug" value="$(var debug)"/>
+    </include>
+
+</launch>

From 8d8532a6effc1b6648754fbccc7df4aef80df79d Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Argentina=20Ortega=20S=C3=A1inz?=
 <argenos@users.noreply.github.com>
Date: Fri, 19 Jul 2024 16:18:10 +0200
Subject: [PATCH 2/2] Fix .world -> .sdf

---
 src/fpm/templates/ros/world.ros2.launch.jinja | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/fpm/templates/ros/world.ros2.launch.jinja b/src/fpm/templates/ros/world.ros2.launch.jinja
index c77d02c..0fb08cd 100644
--- a/src/fpm/templates/ros/world.ros2.launch.jinja
+++ b/src/fpm/templates/ros/world.ros2.launch.jinja
@@ -10,7 +10,7 @@
     <arg name="headless" default="true"/>
     <arg name="debug" default="false"/>
     <arg name="paused" default="false"/>
-    <arg name="world_name" default="worlds/{{model_name}}.world"/>
+    <arg name="world_name" default="worlds/{{model_name}}.sdf"/>
 
     <set_env name="GAZEBO_MODEL_PATH" value="$(find-pkg-prefix floorplan_models)/models:$(find-pkg-prefix floorplan_models)/worlds:$(env GAZEBO_MODEL_PATH)"/>