From 52bd80658791b3c1060e881484e06011ac30241b Mon Sep 17 00:00:00 2001
From: bowski23 <golebiowski@campus.tu-berlin.de>
Date: Mon, 19 Feb 2024 23:05:00 +0100
Subject: [PATCH] added unity world launching

---
 arena_bringup/launch/start_arena.launch                   | 1 +
 arena_bringup/launch/testing/simulators/unity.launch      | 4 +++-
 task_generator/task_generator/manager/obstacle_manager.py | 3 ++-
 task_generator/task_generator/utils.py                    | 4 ++++
 utils/misc/unity_launcher/src/launch_unity_sim.py         | 7 +++++--
 5 files changed, 15 insertions(+), 4 deletions(-)

diff --git a/arena_bringup/launch/start_arena.launch b/arena_bringup/launch/start_arena.launch
index d43c920e8..3591e1542 100755
--- a/arena_bringup/launch/start_arena.launch
+++ b/arena_bringup/launch/start_arena.launch
@@ -136,6 +136,7 @@
     <!--<arg name="world" default="$(arg world_file)" />-->
     <arg name="development_mode" value="$(arg development_mode)" />
     <arg name="headless" value="$(arg headless_unity)" />
+    <arg name="map_file" value="$(arg map_file)" />
   </include>
 
   <!-- map server-->
diff --git a/arena_bringup/launch/testing/simulators/unity.launch b/arena_bringup/launch/testing/simulators/unity.launch
index 48aa77063..09902f556 100644
--- a/arena_bringup/launch/testing/simulators/unity.launch
+++ b/arena_bringup/launch/testing/simulators/unity.launch
@@ -21,10 +21,12 @@
 
   <arg name="development_mode" default="false" />
   <arg name="headless" default="false"/>
+  <arg name="map_file" default="map_empty" />
 
   <node name="unity_sim_node" pkg="unity_launcher" type="launch_unity_sim.py" output="screen">
     <param name="development_mode" value="$(arg development_mode)" />
     <param name="headless" value="$(arg headless)" />
+    <param name="map_file" value="$(arg map_file)" />
   </node>
 
-</launch>
\ No newline at end of file
+</launch>
diff --git a/task_generator/task_generator/manager/obstacle_manager.py b/task_generator/task_generator/manager/obstacle_manager.py
index fcbfa8f38..574412b15 100644
--- a/task_generator/task_generator/manager/obstacle_manager.py
+++ b/task_generator/task_generator/manager/obstacle_manager.py
@@ -9,6 +9,7 @@
 from task_generator.manager.world_manager import WorldManager
 from task_generator.shared import DynamicObstacle, Obstacle
 from task_generator.simulators.base_simulator import BaseSimulator
+from task_generator.utils import Utils
 
 
 class ObstacleManager:
@@ -38,7 +39,7 @@ def spawn_world_obstacles(self, world: World):
         self._entity_manager.spawn_walls(
             walls=world.entities.walls, heightmap=world.map)
         
-        if isinstance(self._simulator, UnitySimulator):
+        if isinstance(self._simulator, UnitySimulator) and not Utils.is_unity_map():
             self._simulator.spawn_walls(world.entities.walls)
 
         self._entity_manager.spawn_obstacles(
diff --git a/task_generator/task_generator/utils.py b/task_generator/task_generator/utils.py
index bb9259ffc..5a44c1a42 100644
--- a/task_generator/task_generator/utils.py
+++ b/task_generator/task_generator/utils.py
@@ -29,6 +29,10 @@ def get_arena_type() -> Constants.ArenaType:
     def is_synthetic_map() -> bool:
         return rosparam_get(str, "map_file") in ["dynamic_map"]
 
+    @staticmethod
+    def is_unity_map() -> bool:
+        return "_unity".casefold() in rosparam_get(str, "map_file").casefold()
+
     @staticmethod
     def generate_map_inner_border(free_space_indices, map_: nav_msgs.OccupancyGrid):
         """generate map border (four vertices of the map)
diff --git a/utils/misc/unity_launcher/src/launch_unity_sim.py b/utils/misc/unity_launcher/src/launch_unity_sim.py
index c484481e8..8a46b3ca2 100755
--- a/utils/misc/unity_launcher/src/launch_unity_sim.py
+++ b/utils/misc/unity_launcher/src/launch_unity_sim.py
@@ -12,6 +12,7 @@ def launch_unity():
     rospy.init_node('unity_sim_node', anonymous=True)
 
     dev = rospy.get_param('~development_mode', False)
+    map = rospy.get_param('~map_file', "map_empty")
     if isinstance(dev, bool) and dev:
         # Unity should be launched trough Unity Editor in dev mode
         return
@@ -33,8 +34,10 @@ def launch_unity():
 
     # test
     args += ["-force-vulkan"]
-
-
+    
+    
+    args += ["-map", map]
+    
     subprocess.run([unity_executable_path] + args, check=True)