multi env scenario

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",

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
+            }
+        }
+    ]
+}

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

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"]

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)
 

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(), \

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)

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>
+

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>
+

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>

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 %}