From 992706cd9539e29b1c9dc7d45d64c2f6ae6ae5ec Mon Sep 17 00:00:00 2001
From: Rohan <rohanchandra30@gmail.com>
Date: Wed, 16 Aug 2023 17:13:11 -0500
Subject: [PATCH] multi env scenario

---
 .../configs/1_31_23/door/sacadrl.json         |   4 +-
 .../multi-scenario.navigation.json            | 173 ++++++++++++++++++
 .../multi-scenario.navigation.txt             |   0
 .../multi-scenario.vectormap.txt              |  17 ++
 scripts/create_env_template.py                |   2 -
 src/config_run.py                             |   8 +-
 src/environment/scenarios/scenario.py         |   1 -
 src/environment/scenarios/types/graph_nav.py  |   7 +-
 .../tmp/multi-scenario/all_launch.launch      |  41 +++++
 .../tmp/multi-scenario/config_launch.launch   |  12 ++
 .../tmp/multi-scenario/greedy_launch.launch   |  18 ++
 src/templates/tmp/multi-scenario/humans.lua   |  31 ++++
 .../tmp/multi-scenario/launch.launch          |  31 ++++
 .../multi-scenario.navigation.json            | 173 ++++++++++++++++++
 .../multi-scenario.navigation.txt             |   0
 .../multi-scenario.vectormap.json             |   1 +
 .../multi-scenario.vectormap.txt              |  17 ++
 .../tmp/multi-scenario/pedsim_launch.launch   |  30 +++
 .../tmp/multi-scenario/pips_launch.launch     |  21 +++
 .../tmp/multi-scenario/ref_launch.launch      |  21 +++
 src/templates/tmp/multi-scenario/scene.xml    |  47 +++++
 .../tmp/multi-scenario/sim_config.lua         | 104 +++++++++++
 22 files changed, 749 insertions(+), 10 deletions(-)
 create mode 100644 docker/vectordisplay/maps/tmp/multi-scenario/multi-scenario.navigation.json
 create mode 100644 docker/vectordisplay/maps/tmp/multi-scenario/multi-scenario.navigation.txt
 create mode 100644 docker/vectordisplay/maps/tmp/multi-scenario/multi-scenario.vectormap.txt
 create mode 100644 src/templates/tmp/multi-scenario/all_launch.launch
 create mode 100644 src/templates/tmp/multi-scenario/config_launch.launch
 create mode 100644 src/templates/tmp/multi-scenario/greedy_launch.launch
 create mode 100644 src/templates/tmp/multi-scenario/humans.lua
 create mode 100644 src/templates/tmp/multi-scenario/launch.launch
 create mode 100644 src/templates/tmp/multi-scenario/multi-scenario.navigation.json
 create mode 100644 src/templates/tmp/multi-scenario/multi-scenario.navigation.txt
 create mode 100644 src/templates/tmp/multi-scenario/multi-scenario.vectormap.json
 create mode 100644 src/templates/tmp/multi-scenario/multi-scenario.vectormap.txt
 create mode 100644 src/templates/tmp/multi-scenario/pedsim_launch.launch
 create mode 100644 src/templates/tmp/multi-scenario/pips_launch.launch
 create mode 100644 src/templates/tmp/multi-scenario/ref_launch.launch
 create mode 100644 src/templates/tmp/multi-scenario/scene.xml
 create mode 100644 src/templates/tmp/multi-scenario/sim_config.lua

diff --git a/config_runner/configs/1_31_23/door/sacadrl.json b/config_runner/configs/1_31_23/door/sacadrl.json
index b0e10783..388d9bfd 100644
--- a/config_runner/configs/1_31_23/door/sacadrl.json
+++ b/config_runner/configs/1_31_23/door/sacadrl.json
@@ -1,5 +1,5 @@
 {
-  "num_agents": [[0, 3], [35, 4], [70, 5]],
+  "num_agents": 2,
   "eval_num_agents": [3, 4, 5, 7, 10, 20],
   "train_length": 250000,
   "ending_eval_trials": 25,
@@ -11,7 +11,7 @@
   "policy_algo_kwargs": {"n_steps":  4096},
   "monitor": true,
 
-  "experiment_names": ["envs_door"],
+  "experiment_names": ["envs_multi"],
 
   "run_name": "1_31_23/door/sacadrl_1",
   "run_type": "SACADRL",
diff --git a/docker/vectordisplay/maps/tmp/multi-scenario/multi-scenario.navigation.json b/docker/vectordisplay/maps/tmp/multi-scenario/multi-scenario.navigation.json
new file mode 100644
index 00000000..abf84545
--- /dev/null
+++ b/docker/vectordisplay/maps/tmp/multi-scenario/multi-scenario.navigation.json
@@ -0,0 +1,173 @@
+{
+    "edges": [
+        {
+            "has_automated_door": false,
+            "has_door": false,
+            "has_elevator": false,
+            "has_stairs": false,
+            "max_clearance": 1.0,
+            "max_speed": 2.0,
+            "s0_id": 0,
+            "s1_id": 1
+        },
+        {
+            "has_automated_door": false,
+            "has_door": false,
+            "has_elevator": false,
+            "has_stairs": false,
+            "max_clearance": 1.0,
+            "max_speed": 2.0,
+            "s0_id": 1,
+            "s1_id": 2
+        },
+        {
+            "has_automated_door": false,
+            "has_door": false,
+            "has_elevator": false,
+            "has_stairs": false,
+            "max_clearance": 1.0,
+            "max_speed": 2.0,
+            "s0_id": 2,
+            "s1_id": 3
+        },
+        {
+            "has_automated_door": false,
+            "has_door": false,
+            "has_elevator": false,
+            "has_stairs": false,
+            "max_clearance": 1.0,
+            "max_speed": 2.0,
+            "s0_id": 3,
+            "s1_id": 4
+        },
+        {
+            "has_automated_door": false,
+            "has_door": false,
+            "has_elevator": false,
+            "has_stairs": false,
+            "max_clearance": 1.0,
+            "max_speed": 2.0,
+            "s0_id": 4,
+            "s1_id": 5
+        },
+        {
+            "has_automated_door": false,
+            "has_door": false,
+            "has_elevator": false,
+            "has_stairs": false,
+            "max_clearance": 1.0,
+            "max_speed": 2.0,
+            "s0_id": 5,
+            "s1_id": 6
+        },
+        {
+            "has_automated_door": false,
+            "has_door": false,
+            "has_elevator": false,
+            "has_stairs": false,
+            "max_clearance": 1.0,
+            "max_speed": 2.0,
+            "s0_id": 7,
+            "s1_id": 8
+        },
+        {
+            "has_automated_door": false,
+            "has_door": false,
+            "has_elevator": false,
+            "has_stairs": false,
+            "max_clearance": 1.0,
+            "max_speed": 2.0,
+            "s0_id": 8,
+            "s1_id": 9
+        },
+        {
+            "has_automated_door": false,
+            "has_door": false,
+            "has_elevator": false,
+            "has_stairs": false,
+            "max_clearance": 1.0,
+            "max_speed": 2.0,
+            "s0_id": 9,
+            "s1_id": 10
+        }
+    ],
+    "nodes": [
+        {
+            "id": 0,
+            "loc": {
+                "x": -84.42088317871094,
+                "y": 36.796165466308594
+            }
+        },
+        {
+            "id": 1,
+            "loc": {
+                "x": -65.49714660644531,
+                "y": 35.95743942260742
+            }
+        },
+        {
+            "id": 2,
+            "loc": {
+                "x": -64.39631652832031,
+                "y": 13.678800582885742
+            }
+        },
+        {
+            "id": 3,
+            "loc": {
+                "x": -70.1101303100586,
+                "y": 13.573960304260254
+            }
+        },
+        {
+            "id": 4,
+            "loc": {
+                "x": -70.16255187988281,
+                "y": 7.860144138336182
+            }
+        },
+        {
+            "id": 5,
+            "loc": {
+                "x": -57.47683334350586,
+                "y": 8.01740550994873
+            }
+        },
+        {
+            "id": 6,
+            "loc": {
+                "x": -57.73893737792969,
+                "y": 37.844573974609375
+            }
+        },
+        {
+            "id": 7,
+            "loc": {
+                "x": -85.10234069824219,
+                "y": 6.602056503295898
+            }
+        },
+        {
+            "id": 8,
+            "loc": {
+                "x": -70.1101303100586,
+                "y": 6.287534236907959
+            }
+        },
+        {
+            "id": 9,
+            "loc": {
+                "x": -55.43243408203125,
+                "y": 6.339954853057861
+            }
+        },
+        {
+            "id": 10,
+            "loc": {
+                "x": -56.06148147583008,
+                "y": 38.001834869384766
+            }
+        }
+    ]
+}
diff --git a/docker/vectordisplay/maps/tmp/multi-scenario/multi-scenario.navigation.txt b/docker/vectordisplay/maps/tmp/multi-scenario/multi-scenario.navigation.txt
new file mode 100644
index 00000000..e69de29b
diff --git a/docker/vectordisplay/maps/tmp/multi-scenario/multi-scenario.vectormap.txt b/docker/vectordisplay/maps/tmp/multi-scenario/multi-scenario.vectormap.txt
new file mode 100644
index 00000000..a0f2327c
--- /dev/null
+++ b/docker/vectordisplay/maps/tmp/multi-scenario/multi-scenario.vectormap.txt
@@ -0,0 +1,17 @@
+-87.844826, 40.172413, -88.017242, -13.965517
+-19.000624, 26.448141, -19.000624, 26.448141
+-20.026054, 26.319963, -20.026054, 26.319963
+-87.704407, 30.549860, -68.605782, 30.549860
+-60.658703, 39.906906, -60.530525, 9.913089
+-68.477600, 30.678040, -68.093063, 18.501062
+-68.033943, 18.292671, -79.927086, 18.129751
+-79.927086, 18.129751, -79.764160, 10.309603
+-79.764160, 10.146683, -73.573212, 10.146683
+-60.621090, 10.146683, -66.893501, 10.228143
+-88.073074, 5.014711, -73.654671, 4.933251
+-73.654671, 4.933251, -73.736130, -13.476682
+-66.730583, 4.770331, -66.486206, -13.639602
+-66.730583, 4.444491, -53.126781, 4.444491
+-53.126781, 4.281572, -54.104301, 40.042458
+-88.073074, -13.721062, -66.404739, -13.558142
+-87.828690, 40.205379, -60.702553, 39.716621
diff --git a/scripts/create_env_template.py b/scripts/create_env_template.py
index d8bd6b32..1908246f 100644
--- a/scripts/create_env_template.py
+++ b/scripts/create_env_template.py
@@ -67,8 +67,6 @@ def create_new_env(name: str, template: str = None):
         open(str(vectormap_file), 'w').close()
     if not navigation_file.exists():
         open(str(navigation_file), 'w').close()
-    if not navigation_json_file.exists():
-        open(str(navigation_json_file), 'w').close()
 
     # Build vectormap (geometry of the env)
     cmd = [str(VECTOR_DISPLAY_FOLDER / 'vd.sh'), str(environment_map_path.parent), environment_map_path.name, "--edit_localization"]
diff --git a/src/config_run.py b/src/config_run.py
index ea82c0b8..1f26213e 100644
--- a/src/config_run.py
+++ b/src/config_run.py
@@ -34,7 +34,7 @@
 
 from src.environment.scenarios.common_scenarios import envs_door, envs_hallway, envs_intersection, envs_round_about, \
   envs_open
-from src.environment.scenarios import CycleScenario
+from src.environment.scenarios import CycleScenario, GraphNavScenario
 from src.environment.utils.utils import DATA_FOLDER
 from src.environment.utils.evaluate_policy import evaluate_policy
 import datetime
@@ -319,6 +319,10 @@ def run(
                                                all_config=monitor and not local)
     scenarios.append(scenario)
     zones.append(conflict_zone)
+  if 'envs_multi' in experiment_names:
+    scenario = GraphNavScenario("tmp/multi-scenario")
+    scenarios.append(scenario)
+
 
   observations = []
 
@@ -387,7 +391,7 @@ def run(
     ENV_CLASS = partial(ManualZoneEnv, zones)
   else:
     ENV_CLASS = partial(RosSocialEnv)
-
+  
   # nav_map_vis = NavMapViz(scenario.nav_map, scenario.nav_lines)
   env = ENV_CLASS(observer=observer, rewarder=rewarder, scenarios=scenarios, num_humans=0, num_agents=num_agents if isinstance(num_agents, int) else max(num_agents[-1][1], eval_num_agents[-1]), debug=debug)
 
diff --git a/src/environment/scenarios/scenario.py b/src/environment/scenarios/scenario.py
index 1a7c24db..2f89b2f3 100644
--- a/src/environment/scenarios/scenario.py
+++ b/src/environment/scenarios/scenario.py
@@ -29,7 +29,6 @@ def __init__(
 
         self.config_scene_path = Path(f'{ROOT_FOLDER}/src/templates/{env_name}/')
         self.config_nav_path = self.config_scene_path / f'{self.config_scene_path.name}.navigation.json'
-
         assert Path(self.config_nav_path).is_file(), \
             f'The env_name ({env_name}) does not have a navigation file at {self.config_nav_path}.'
         assert Path(self.config_scene_path).is_dir(), \
diff --git a/src/environment/scenarios/types/graph_nav.py b/src/environment/scenarios/types/graph_nav.py
index b7481d11..20a4a3c4 100644
--- a/src/environment/scenarios/types/graph_nav.py
+++ b/src/environment/scenarios/types/graph_nav.py
@@ -21,7 +21,6 @@
 from pathlib import Path
 from typing import Union, Tuple, List
 
-
 from src.environment.scenarios import Scenario
 
 robot_positions = [
@@ -178,7 +177,8 @@ def generate_scenario(
         if self.allowed_agent_goal_positions:
             robot_ends = [random.sample(list(set(self.allowed_human_goal_positions) - {x}), 1) for x in enumerate(robot_starts)]
         else:
-            robot_ends = [random.sample(list(set(list(range(0, len(self.robot_positions)))) - {x}), 1) for x in enumerate(robot_starts)]
+            # robot_ends = [random.sample(list(set(list(range(0, len(self.robot_positions)))) - {x}), 1) for x in enumerate(robot_starts)]
+            robot_ends = [random.sample(list(set(list(range(0, len(self.robot_positions)))) - {x}), 1)[0] for x in enumerate(robot_starts)]
 
         human_positions = []
 
@@ -224,6 +224,7 @@ def generate_scenario(
             'dev': human_dev,
             'positions': self.robot_positions,
             'human_positions': human_positions,
-            'nav_map': self.nav_map
+            'nav_map': self.nav_map,
+            'robot_count': num_agents
         }
         self.make_scenario(config)
diff --git a/src/templates/tmp/multi-scenario/all_launch.launch b/src/templates/tmp/multi-scenario/all_launch.launch
new file mode 100644
index 00000000..8652214e
--- /dev/null
+++ b/src/templates/tmp/multi-scenario/all_launch.launch
@@ -0,0 +1,41 @@
+
+        <launch>
+            <arg name="outfile" default="screen" />
+        
+            <node pkg="ut_multirobot_sim" type="simulator_link" name="simulator" cwd="node"
+                output="screen"
+                args="-sim_config $(find social_gym)/config/gym_gen/sim_config.lua -scene_config $(find social_gym)/config/gym_gen/scene.xml -speedup_factor 1.0 --localize --use_pedsim" />
+        
+            <group unless="$(optenv DOCKER false)">
+                <node pkg="rviz" type="rviz" name="rviz" args="-d $(find ut_multirobot_sim)/visualization.rviz" />
+            </group>
+        
+                <group ns="camera1">
+                <node pkg="tf" type="static_transform_publisher" name="camera_broadcaster"
+                  args="-1 7 12 15 0 0 1 map camera1 10" />
+                <node name="camera_info" pkg="rostopic" type="rostopic"
+                  args="pub camera_info sensor_msgs/CameraInfo
+                 '{header: {seq: 0, stamp: {secs: 0, nsecs: 0}, frame_id: 'camera1'},
+                  height: 480, width: 640, distortion_model: 'plumb_bob',
+                  D: [0],
+                  K: [500.0, 0.0, 320, 0.0, 500.0, 240.0, 0.0, 0.0, 1.0],
+                  R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0],
+                  P: [500.0, 0.0, 320, 0.0, 0.0, 500, 240, 0.0, 0.0, 0.0, 1.0, 0.0],
+                  binning_x: 0, binning_y: 0,
+                  roi: {x_offset: 0, y_offset: 0, height: 480, width: 640, do_rectify: false}}' -r 2"
+                  output="screen"/>
+            </group>
+        
+            <group ns="rviz1/camera1/image">
+              <rosparam param="disable_pub_plugins">
+                - 'image_transport/compressed'
+                - 'image_transport/compressedDepth'
+                - 'image_transport/theora'
+              </rosparam>
+            </group>
+        
+            <node pkg="graph_navigation" type="social_nav" name="graph_navigation" output="screen" args="-service_mode=true -map=tmp/multi-scenario"  /><include file="$(find social_gym)/config/gym_gen/pedsim_launch.launch" />
+        
+            <param name="enable_statistics" value="true" />
+        </launch>
+        
\ No newline at end of file
diff --git a/src/templates/tmp/multi-scenario/config_launch.launch b/src/templates/tmp/multi-scenario/config_launch.launch
new file mode 100644
index 00000000..eedc6ee2
--- /dev/null
+++ b/src/templates/tmp/multi-scenario/config_launch.launch
@@ -0,0 +1,12 @@
+<launch>
+    <arg name="outfile" default="screen" />
+
+    <node pkg="ut_multirobot_sim" type="simulator_link" name="simulator" cwd="node"
+        output="screen"
+        args="-sim_config $(find social_gym)/config/gym_gen/sim_config.lua -scene_config $(find social_gym)/config/gym_gen/scene.xml -speedup_factor 1.0 --localize --use_pedsim" />
+
+    <node pkg="graph_navigation" type="social_nav" name="graph_navigation" output="screen" args="-service_mode=true -map=tmp/multi-scenario"  /><include file="$(find social_gym)/config/gym_gen/pedsim_launch.launch" />
+
+    <param name="enable_statistics" value="true" />
+</launch>
+
diff --git a/src/templates/tmp/multi-scenario/greedy_launch.launch b/src/templates/tmp/multi-scenario/greedy_launch.launch
new file mode 100644
index 00000000..600762b7
--- /dev/null
+++ b/src/templates/tmp/multi-scenario/greedy_launch.launch
@@ -0,0 +1,18 @@
+<launch>
+    <arg name="outfile" default="screen" />
+
+    <node pkg="ut_multirobot_sim" type="simulator_link" name="simulator" cwd="node"
+        output="screen"
+        args="-sim_config $(find ut_multirobot_sim)/config/gym_gen/sim_config.lua -scene_config $(find ut_multirobot_sim)/config/gym_gen/scene.xml -speedup_factor 1.0 --localize --use_pedsim" />
+
+    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find ut_multirobot_sim)/visualization.rviz" />
+
+    <node pkg="cpp-pips" type="social_greed" name="social_greed" output="screen" />
+
+    <node pkg="graph_navigation" type="social_nav" name="graph_navigation" output="screen"
+        args="-service_mode=true -social_mode=true -map=tmp/multi-scenario" />
+
+    <include file="$(find ut_multirobot_sim)/config/gym_gen/pedsim_launch.launch" />
+
+    <param name="enable_statistics" value="true" />
+</launch>
diff --git a/src/templates/tmp/multi-scenario/humans.lua b/src/templates/tmp/multi-scenario/humans.lua
new file mode 100644
index 00000000..ff63c602
--- /dev/null
+++ b/src/templates/tmp/multi-scenario/humans.lua
@@ -0,0 +1,31 @@
+function Vector2(x, y)
+    return {x = x, y = y}
+end
+
+-- Human shape information
+hu_radius = 0.2
+hu_num_segments = 20
+
+-- Human speed information
+hu_max_speed = 0.8 --
+hu_avg_speed = 0.8 --
+hu_max_omega = 0.2
+hu_avg_omega = 0.
+hu_reach_goal_threshold = 0.1
+
+-- Human walking mode
+local HumanMode = {
+    Singleshot=0,
+    Repeat=1,
+    Controlled=2,
+    Cycle=3,
+}
+
+hu_mode = HumanMode.Controlled
+hu_control_topic = "/command"
+
+{% for i in range(human_count) %}
+    hu{{ i }}_waypoints = {
+        { {{ human_positions[i][0] }}, {{ human_positions[i][1] }}, {{ human_positions[i][2] }} }
+    }
+{% endfor %}
diff --git a/src/templates/tmp/multi-scenario/launch.launch b/src/templates/tmp/multi-scenario/launch.launch
new file mode 100644
index 00000000..95089823
--- /dev/null
+++ b/src/templates/tmp/multi-scenario/launch.launch
@@ -0,0 +1,31 @@
+<launch>
+    <arg name="outfile" default="screen" />
+
+    <group ns="camera1">
+        <node pkg="tf" type="static_transform_publisher" name="camera_broadcaster"
+          args="-1 7 12 15 0 0 1 map camera1 10" />
+        <node name="camera_info" pkg="rostopic" type="rostopic"
+          args="pub camera_info sensor_msgs/CameraInfo
+         '{header: {seq: 0, stamp: {secs: 0, nsecs: 0}, frame_id: 'camera1'},
+          height: 480, width: 640, distortion_model: 'plumb_bob',
+          D: [0],
+          K: [500.0, 0.0, 320, 0.0, 500.0, 240.0, 0.0, 0.0, 1.0],
+          R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0],
+          P: [500.0, 0.0, 320, 0.0, 0.0, 500, 240, 0.0, 0.0, 0.0, 1.0, 0.0],
+          binning_x: 0, binning_y: 0,
+          roi: {x_offset: 0, y_offset: 0, height: 480, width: 640, do_rectify: false}}' -r 2"
+          output="screen"/>
+    </group>
+
+    <group ns="rviz1/camera1/image">
+      <rosparam param="disable_pub_plugins">
+        - 'image_transport/compressed'
+        - 'image_transport/compressedDepth'
+        - 'image_transport/theora'
+      </rosparam>
+    </group>
+
+    <include file="$(find social_gym)/config/gym_gen/pedsim_launch.launch" />
+
+    <param name="enable_statistics" value="true" />
+</launch>
diff --git a/src/templates/tmp/multi-scenario/multi-scenario.navigation.json b/src/templates/tmp/multi-scenario/multi-scenario.navigation.json
new file mode 100644
index 00000000..abf84545
--- /dev/null
+++ b/src/templates/tmp/multi-scenario/multi-scenario.navigation.json
@@ -0,0 +1,173 @@
+{
+    "edges": [
+        {
+            "has_automated_door": false,
+            "has_door": false,
+            "has_elevator": false,
+            "has_stairs": false,
+            "max_clearance": 1.0,
+            "max_speed": 2.0,
+            "s0_id": 0,
+            "s1_id": 1
+        },
+        {
+            "has_automated_door": false,
+            "has_door": false,
+            "has_elevator": false,
+            "has_stairs": false,
+            "max_clearance": 1.0,
+            "max_speed": 2.0,
+            "s0_id": 1,
+            "s1_id": 2
+        },
+        {
+            "has_automated_door": false,
+            "has_door": false,
+            "has_elevator": false,
+            "has_stairs": false,
+            "max_clearance": 1.0,
+            "max_speed": 2.0,
+            "s0_id": 2,
+            "s1_id": 3
+        },
+        {
+            "has_automated_door": false,
+            "has_door": false,
+            "has_elevator": false,
+            "has_stairs": false,
+            "max_clearance": 1.0,
+            "max_speed": 2.0,
+            "s0_id": 3,
+            "s1_id": 4
+        },
+        {
+            "has_automated_door": false,
+            "has_door": false,
+            "has_elevator": false,
+            "has_stairs": false,
+            "max_clearance": 1.0,
+            "max_speed": 2.0,
+            "s0_id": 4,
+            "s1_id": 5
+        },
+        {
+            "has_automated_door": false,
+            "has_door": false,
+            "has_elevator": false,
+            "has_stairs": false,
+            "max_clearance": 1.0,
+            "max_speed": 2.0,
+            "s0_id": 5,
+            "s1_id": 6
+        },
+        {
+            "has_automated_door": false,
+            "has_door": false,
+            "has_elevator": false,
+            "has_stairs": false,
+            "max_clearance": 1.0,
+            "max_speed": 2.0,
+            "s0_id": 7,
+            "s1_id": 8
+        },
+        {
+            "has_automated_door": false,
+            "has_door": false,
+            "has_elevator": false,
+            "has_stairs": false,
+            "max_clearance": 1.0,
+            "max_speed": 2.0,
+            "s0_id": 8,
+            "s1_id": 9
+        },
+        {
+            "has_automated_door": false,
+            "has_door": false,
+            "has_elevator": false,
+            "has_stairs": false,
+            "max_clearance": 1.0,
+            "max_speed": 2.0,
+            "s0_id": 9,
+            "s1_id": 10
+        }
+    ],
+    "nodes": [
+        {
+            "id": 0,
+            "loc": {
+                "x": -84.42088317871094,
+                "y": 36.796165466308594
+            }
+        },
+        {
+            "id": 1,
+            "loc": {
+                "x": -65.49714660644531,
+                "y": 35.95743942260742
+            }
+        },
+        {
+            "id": 2,
+            "loc": {
+                "x": -64.39631652832031,
+                "y": 13.678800582885742
+            }
+        },
+        {
+            "id": 3,
+            "loc": {
+                "x": -70.1101303100586,
+                "y": 13.573960304260254
+            }
+        },
+        {
+            "id": 4,
+            "loc": {
+                "x": -70.16255187988281,
+                "y": 7.860144138336182
+            }
+        },
+        {
+            "id": 5,
+            "loc": {
+                "x": -57.47683334350586,
+                "y": 8.01740550994873
+            }
+        },
+        {
+            "id": 6,
+            "loc": {
+                "x": -57.73893737792969,
+                "y": 37.844573974609375
+            }
+        },
+        {
+            "id": 7,
+            "loc": {
+                "x": -85.10234069824219,
+                "y": 6.602056503295898
+            }
+        },
+        {
+            "id": 8,
+            "loc": {
+                "x": -70.1101303100586,
+                "y": 6.287534236907959
+            }
+        },
+        {
+            "id": 9,
+            "loc": {
+                "x": -55.43243408203125,
+                "y": 6.339954853057861
+            }
+        },
+        {
+            "id": 10,
+            "loc": {
+                "x": -56.06148147583008,
+                "y": 38.001834869384766
+            }
+        }
+    ]
+}
diff --git a/src/templates/tmp/multi-scenario/multi-scenario.navigation.txt b/src/templates/tmp/multi-scenario/multi-scenario.navigation.txt
new file mode 100644
index 00000000..e69de29b
diff --git a/src/templates/tmp/multi-scenario/multi-scenario.vectormap.json b/src/templates/tmp/multi-scenario/multi-scenario.vectormap.json
new file mode 100644
index 00000000..4742616e
--- /dev/null
+++ b/src/templates/tmp/multi-scenario/multi-scenario.vectormap.json
@@ -0,0 +1 @@
+[{"p0": {"x": "-87.844826", "y": "40.172413"}, "p1": {"x": "-88.017242", "y": "-13.965517"}}, {"p0": {"x": "-19.000624", "y": "26.448141"}, "p1": {"x": "-19.000624", "y": "26.448141"}}, {"p0": {"x": "-20.026054", "y": "26.319963"}, "p1": {"x": "-20.026054", "y": "26.319963"}}, {"p0": {"x": "-87.704407", "y": "30.549860"}, "p1": {"x": "-68.605782", "y": "30.549860"}}, {"p0": {"x": "-60.658703", "y": "39.906906"}, "p1": {"x": "-60.530525", "y": "9.913089"}}, {"p0": {"x": "-68.477600", "y": "30.678040"}, "p1": {"x": "-68.093063", "y": "18.501062"}}, {"p0": {"x": "-68.033943", "y": "18.292671"}, "p1": {"x": "-79.927086", "y": "18.129751"}}, {"p0": {"x": "-79.927086", "y": "18.129751"}, "p1": {"x": "-79.764160", "y": "10.309603"}}, {"p0": {"x": "-79.764160", "y": "10.146683"}, "p1": {"x": "-73.573212", "y": "10.146683"}}, {"p0": {"x": "-60.621090", "y": "10.146683"}, "p1": {"x": "-66.893501", "y": "10.228143"}}, {"p0": {"x": "-88.073074", "y": "5.014711"}, "p1": {"x": "-73.654671", "y": "4.933251"}}, {"p0": {"x": "-73.654671", "y": "4.933251"}, "p1": {"x": "-73.736130", "y": "-13.476682"}}, {"p0": {"x": "-66.730583", "y": "4.770331"}, "p1": {"x": "-66.486206", "y": "-13.639602"}}, {"p0": {"x": "-66.730583", "y": "4.444491"}, "p1": {"x": "-53.126781", "y": "4.444491"}}, {"p0": {"x": "-53.126781", "y": "4.281572"}, "p1": {"x": "-54.104301", "y": "40.042458"}}, {"p0": {"x": "-88.073074", "y": "-13.721062"}, "p1": {"x": "-66.404739", "y": "-13.558142"}}, {"p0": {"x": "-87.828690", "y": "40.205379"}, "p1": {"x": "-60.702553", "y": "39.716621"}}]
\ No newline at end of file
diff --git a/src/templates/tmp/multi-scenario/multi-scenario.vectormap.txt b/src/templates/tmp/multi-scenario/multi-scenario.vectormap.txt
new file mode 100644
index 00000000..a0f2327c
--- /dev/null
+++ b/src/templates/tmp/multi-scenario/multi-scenario.vectormap.txt
@@ -0,0 +1,17 @@
+-87.844826, 40.172413, -88.017242, -13.965517
+-19.000624, 26.448141, -19.000624, 26.448141
+-20.026054, 26.319963, -20.026054, 26.319963
+-87.704407, 30.549860, -68.605782, 30.549860
+-60.658703, 39.906906, -60.530525, 9.913089
+-68.477600, 30.678040, -68.093063, 18.501062
+-68.033943, 18.292671, -79.927086, 18.129751
+-79.927086, 18.129751, -79.764160, 10.309603
+-79.764160, 10.146683, -73.573212, 10.146683
+-60.621090, 10.146683, -66.893501, 10.228143
+-88.073074, 5.014711, -73.654671, 4.933251
+-73.654671, 4.933251, -73.736130, -13.476682
+-66.730583, 4.770331, -66.486206, -13.639602
+-66.730583, 4.444491, -53.126781, 4.444491
+-53.126781, 4.281572, -54.104301, 40.042458
+-88.073074, -13.721062, -66.404739, -13.558142
+-87.828690, 40.205379, -60.702553, 39.716621
diff --git a/src/templates/tmp/multi-scenario/pedsim_launch.launch b/src/templates/tmp/multi-scenario/pedsim_launch.launch
new file mode 100644
index 00000000..70ac0248
--- /dev/null
+++ b/src/templates/tmp/multi-scenario/pedsim_launch.launch
@@ -0,0 +1,30 @@
+<launch>
+  <arg name="kbd_teleop" default="false"/>
+  <arg name="rqt_teleop" default="false"/>
+  <arg name="visualize" default="true"/>
+  <arg name="with_robot" default="true"/>
+
+  <arg name="simulation_factor" default="1.0"/> <!-- Speed up -->
+  <arg name="update_rate" default="40.0"/> <!-- Hz -->
+
+  <!-- Simulator -->
+  <include file="$(find pedsim_simulator)/launch/simulator.launch">
+    <arg name="kbd_teleop" value="$(arg kbd_teleop)"/>
+    <arg name="rqt_teleop" value="$(arg rqt_teleop)"/>
+    <arg name="scene_file" value="$(find social_gym)config/gym_gen/scene.xml"/>
+    <arg name="with_robot" value="$(arg with_robot)"/>
+    <arg name="simulation_factor" value="$(arg simulation_factor)"/>
+    <arg name="update_rate" value="$(arg update_rate)"/>
+    <arg name="default_queue_size" value="10"/>
+    <arg name="max_robot_speed" value="1.5"/>
+    <arg name="robot_mode" value="1"/>
+    <arg name="enable_groups" value="false"/>
+    <arg name="pose_initial_x" value="-1.45"/>
+    <arg name="pose_initial_y" value="10.35"/>
+    <arg name="pose_initial_theta" value="-1.5708"/>
+  </include>
+
+  <!-- <include file="$(find pedsim_visualizer)/launch/visualizer.launch"/> -->
+  <!-- <node pkg="rviz" type="rviz" name="rviz" args="-d $(find pedsim_simulator)/rviz/social_contexts_activities.rviz" if="$(arg visualize)"/> -->
+
+</launch>
diff --git a/src/templates/tmp/multi-scenario/pips_launch.launch b/src/templates/tmp/multi-scenario/pips_launch.launch
new file mode 100644
index 00000000..aeb25172
--- /dev/null
+++ b/src/templates/tmp/multi-scenario/pips_launch.launch
@@ -0,0 +1,21 @@
+<launch>
+    <arg name="docker" default="false"/>
+    <arg name="outfile" default="screen" />
+
+    <node pkg="ut_multirobot_sim" type="simulator_link" name="simulator" cwd="node"
+        output="screen"
+        args="-sim_config $(find ut_multirobot_sim)/config/gym_gen/sim_config.lua -scene_config $(find ut_multirobot_sim)/config/gym_gen/scene.xml -speedup_factor 1.0 --localize --use_pedsim" />
+
+    <group unless="$(optenv DOCKER false)">
+        <node pkg="rviz" type="rviz" name="rviz" args="-d $(find ut_multirobot_sim)/visualization.rviz" />
+    </group>
+
+    <node pkg="pips" type="social_interp" name="social_interp" output="screen" args="-ast_path $(find cpp-pips)/synthd/displ3" />
+
+    <node pkg="graph_navigation" type="social_nav" name="graph_navigation" output="screen"
+        args="-service_mode=true -social_mode=true -map=tmp/multi-scenario" />
+
+    <include file="$(find ut_multirobot_sim)/config/gym_gen/pedsim_launch.launch" />
+
+    <param name="enable_statistics" value="true" />
+</launch>
diff --git a/src/templates/tmp/multi-scenario/ref_launch.launch b/src/templates/tmp/multi-scenario/ref_launch.launch
new file mode 100644
index 00000000..e0d0735d
--- /dev/null
+++ b/src/templates/tmp/multi-scenario/ref_launch.launch
@@ -0,0 +1,21 @@
+<launch>
+    <arg name="outfile" default="screen" />
+    <arg name="docker" default="false"/>
+
+    <node pkg="ut_multirobot_sim" type="simulator_link" name="simulator" cwd="node"
+        output="screen"
+        args="-sim_config $(find ut_multirobot_sim)/config/gym_gen/sim_config.lua -scene_config $(find ut_multirobot_sim)/config/gym_gen/scene.xml -speedup_factor 1.0 --localize --use_pedsim" />
+
+    <group unless="$(optenv DOCKER false)">
+        <node pkg="rviz" type="rviz" name="rviz" args="-d $(find ut_multirobot_sim)/visualization.rviz" />
+    </group>
+
+    <node pkg="pips" type="social_ref" name="social_ref" output="screen" />
+
+    <node pkg="graph_navigation" type="social_nav" name="graph_navigation" output="screen"
+        args="-service_mode=true -social_mode=true -map=tmp/multi-scenario" />
+
+    <include file="$(find ut_multirobot_sim)/config/gym_gen/pedsim_launch.launch" />
+
+    <param name="enable_statistics" value="true" />
+</launch>
diff --git a/src/templates/tmp/multi-scenario/scene.xml b/src/templates/tmp/multi-scenario/scene.xml
new file mode 100644
index 00000000..2d721255
--- /dev/null
+++ b/src/templates/tmp/multi-scenario/scene.xml
@@ -0,0 +1,47 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<scenario>
+    <!--Obstacles-->
+	<obstacle x1="-87.844826" y1="40.172413" x2="-88.017242" y2="-13.965517"/>
+	<obstacle x1="-19.000624" y1="26.448141" x2="-19.000624" y2="26.448141"/>
+	<obstacle x1="-20.026054" y1="26.319963" x2="-20.026054" y2="26.319963"/>
+	<obstacle x1="-87.704407" y1="30.549860" x2="-68.605782" y2="30.549860"/>
+	<obstacle x1="-60.658703" y1="39.906906" x2="-60.530525" y2="9.913089"/>
+	<obstacle x1="-68.477600" y1="30.678040" x2="-68.093063" y2="18.501062"/>
+	<obstacle x1="-68.033943" y1="18.292671" x2="-79.927086" y2="18.129751"/>
+	<obstacle x1="-79.927086" y1="18.129751" x2="-79.764160" y2="10.309603"/>
+	<obstacle x1="-79.764160" y1="10.146683" x2="-73.573212" y2="10.146683"/>
+	<obstacle x1="-60.621090" y1="10.146683" x2="-66.893501" y2="10.228143"/>
+	<obstacle x1="-88.073074" y1="5.014711" x2="-73.654671" y2="4.933251"/>
+	<obstacle x1="-73.654671" y1="4.933251" x2="-73.736130" y2="-13.476682"/>
+	<obstacle x1="-66.730583" y1="4.770331" x2="-66.486206" y2="-13.639602"/>
+	<obstacle x1="-66.730583" y1="4.444491" x2="-53.126781" y2="4.444491"/>
+	<obstacle x1="-53.126781" y1="4.281572" x2="-54.104301" y2="40.042458"/>
+	<obstacle x1="-88.073074" y1="-13.721062" x2="-66.404739" y2="-13.558142"/>
+	<obstacle x1="-87.828690" y1="40.205379" x2="-60.702553" y2="39.716621"/>
+    <!--Way Points-->
+{% for i in range(position_count) %}
+    <waypoint id="{{ i }}" x="{{ positions[i][0] }}" y = "{{ positions[i][1] }}" r="1" b="0.1"/>
+{% endfor %}
+
+{% for i in range(nav_count) %}
+    <waypoint id="n{{ i }}" x="{{ nav_map[i][0] }}" y = "{{ nav_map[i][1] }}" r="1" b="0.1"/>
+{% endfor %}
+
+    <!-- This Robot Goal Doesn't Matter, but is Required -->
+  <waypoint id="robot_goal" x="{{ robot_end[0] }}" y="{{ robot_end[1] }}" r="2"/>
+  <waypoint id="robot_start" x="{{ robot_start[0] }}" y="{{ robot_start[1] }}" r="2"/>
+
+  <agent x="{{ robot_start[0] }}" y="{{ robot_start[1] }}" n="1" dx="0" dy="0" type="2">
+    <addwaypoint id="robot_start"/>
+    <addwaypoint id="robot_goal"/>
+  </agent>
+
+  {% for human_position in human_positions %}
+  <agent x="{{ human_position[0] }}" y="{{ human_position[1] }}" n="1" dx="{{ dev }}" dy="{{ dev }}" type="0">
+    {% for i in range(3, human_position|length) %}
+    <addwaypoint id="n{{human_position[i]}}" />
+    {% endfor %}
+  </agent>
+  {% endfor %}
+
+</scenario>
diff --git a/src/templates/tmp/multi-scenario/sim_config.lua b/src/templates/tmp/multi-scenario/sim_config.lua
new file mode 100644
index 00000000..179033a7
--- /dev/null
+++ b/src/templates/tmp/multi-scenario/sim_config.lua
@@ -0,0 +1,104 @@
+
+function Vector2(x, y)
+    return {x = x, y = y}
+  end
+
+  function Vector3(x, y, z)
+    return {x = x, y = y, z = z}
+  end
+
+  function DegToRad(d)
+    return math.pi * d / 180
+  end
+map_name =  "maps/tmp/multi-scenario/multi-scenario.vectormap.txt"
+nav_map_name =  "tmp/multi-scenario"
+-- Simulator starting location.
+start_poses = {
+    {% for i in range(robot_count) %}
+        {
+             {{ robot_start[i][0] }}, {{ robot_start[i][1] }}, {{ robot_start[i][2] }}
+        },
+    {% endfor %}
+}
+
+
+goal_poses = {
+    {% for i in range(robot_count) %}
+        {
+             {{ robot_end[i][0] }}, {{ robot_end[i][1] }}, {{ robot_end[i][2] }}
+        },
+    {% endfor %}
+}
+
+
+  num_humans = {{ human_count }}
+  human_config = "../../config/gym_gen/humans.lua"
+  
+  partially_observable = {{ partially_observable }}
+
+  door_config_list = {
+    -- "/home/jaholtz/code/amrl_maps/GDC1/door_list.lua"
+  }
+
+  -- Time-step for simulation.
+  delta_t = 0.025
+  -- max_steps = 12000
+  max_steps = 3200
+
+  -- Simulator TF publications
+  publish_tfs = true;
+  -- publish_foot_to_base = true;
+  -- publish_map_to_odom = true;
+
+  -- -- Car dimensions.
+  -- car_width = 0.281
+  -- car_length = 0.535
+  -- car_height = 0.15;
+
+  -- -- Location of the robot's rear wheel axle relative to the center of the body.
+  -- rear_axle_offset = -0.162
+  -- laser_loc = Vector3(0.2, 0, 0.15)
+
+  -- Kinematic and dynamic constraints for the car.
+  -- min_turn_radius = 0.98
+
+  -- Laser rangefinder parameters.
+  laser_noise_stddev = 0.01;
+  -- laser_angle_min = DegToRad(-135.0);
+  -- laser_angle_max = DegToRad(135.0);
+  -- laser_angle_increment = DegToRad(0.25);
+  -- laser_min_range = 0.02;
+  -- laser_max_range = 100.0;
+
+  -- Turning error simulation.
+  angular_error_bias = DegToRad(0);
+  angular_error_rate = 0.1;
+
+  -- Defining robot type enumerator
+  local RobotType = {
+      ACKERMANN_DRIVE="ACKERMANN_DRIVE",
+      OMNIDIRECTIONAL_DRIVE="OMNIDIRECTIONAL_DRIVE",
+      DIFF_DRIVE="DIFF_DRIVE"
+  }
+
+  -- robot_type = RobotType.ACKERMANN_DRIVE
+  -- robot_config = "config/ut_automata_config.lua"
+  -- robot_type = RobotType.DIFF_DRIVE
+  -- robot_config = "config/bwibot_config.lua"
+  -- robot_type = RobotType.OMNIDIRECTIONAL_DRIVE
+  -- robot_config = "config/cobot_config.lua"
+  robot_types = {
+    {% for i in range(robot_count) %}
+        {% if i+1 < robot_count %}
+                RobotType.DIFF_DRIVE,
+        {% else %}
+                RobotType.DIFF_DRIVE
+        {% endif %}
+    {% endfor %}
+  }
+
+  robot_config = "config/ut_jackal_config.lua"
+
+  laser_topic = "/Cobot/Laser"
+  laser_frame = "base_laser"
+  
\ No newline at end of file