fixing scenario

config_runner/configs/1_31_23/door/sacadrl.json

@@ -1,5 +1,5 @@
 {
-  "num_agents": 2,
+  "num_agents": [[0,2]],
   "eval_num_agents": [3, 4, 5, 7, 10, 20],
   "train_length": 250000,
   "ending_eval_trials": 25,

config_runner/run_config.sh

@@ -11,7 +11,7 @@ docker run -d --name $1 -w /home/rosdev/social_gym/ --gpus all --privileged \
 -e DISPLAY=unix$DISPLAY \
 -v ${DIR}/data:/home/rosdev/social_gym/data \
 -v ${DIR}/config_runner/configs:/home/rosdev/social_gym/config_runner/configs \
--v ${DIR}/submodules/amrl_maps/envs:/home/rosdev/social_gym/submodules/amrl_maps/envs \
+-v ${DIR}/submodules/amrl_maps:/home/rosdev/social_gym/submodules/amrl_maps \
 -v ${DIR}/submodules/ut_multirobot_sim/maps:/home/rosdev/social_gym/submodules/ut_multirobot_sim/maps \
 --network host \
 -v ${DIR}/src:/home/rosdev/social_gym/src \

docker/vectordisplay/maps/multienv/multienv.navigation.json

@@ -0,0 +1,94 @@
+{
+    "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": 2
+        },
+        {
+            "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": 3
+        },
+        {
+            "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": 0,
+            "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": 2,
+            "s1_id": 1
+        }
+    ],
+    "nodes": [
+        {
+            "id": 0,
+            "loc": {
+                "x": -11.379310607910156,
+                "y": 15.17241382598877
+            }
+        },
+        {
+            "id": 1,
+            "loc": {
+                "x": 8.448275566101074,
+                "y": 15.0
+            }
+        },
+        {
+            "id": 2,
+            "loc": {
+                "x": -8.793103218078613,
+                "y": -6.724138259887695
+            }
+        },
+        {
+            "id": 3,
+            "loc": {
+                "x": 9.82758617401123,
+                "y": -8.103448867797852
+            }
+        }
+    ]
+}

docker/vectordisplay/maps/multienv/multienv.vectormap.txt

@@ -0,0 +1,3 @@
+-19.655172, 21.724140, -19.655172, -7.241378
+-19.827587, 22.241381, 13.275863, 22.586208
+13.275863, 22.586208, 13.793104, -5.689654

logs_multienv.txt

@@ -0,0 +1,38 @@
+/home/rosdev/social_gym
+/home/rosdev/social_gym
+Name: SuperSuit
+Version: 3.9.0
+Summary: Wrappers for Gymnasium and PettingZoo
+Home-page: None
+Author: None
+Author-email: Farama Foundation <contact@farama.org>
+License: MIT License
+Location: /usr/local/lib/python3.8/dist-packages
+Requires: tinyscaler, gymnasium, numpy
+Required-by: 
+Name: pettingzoo
+Version: 1.23.1
+Summary: Gymnasium for multi-agent reinforcement learning.
+Home-page: None
+Author: None
+Author-email: Farama Foundation <contact@farama.org>
+License: MIT License
+Location: /usr/local/lib/python3.8/dist-packages
+Requires: gymnasium, numpy
+Required-by: 
+Name: stable-baselines3
+Version: 2.1.0
+Summary: Pytorch version of Stable Baselines, implementations of reinforcement learning algorithms.
+Home-page: https://github.com/DLR-RM/stable-baselines3
+Author: Antonin Raffin
+Author-email: antonin.raffin@dlr.de
+License: MIT
+Location: /usr/local/lib/python3.8/dist-packages
+Requires: matplotlib, gymnasium, pandas, torch, cloudpickle, numpy
+Required-by: sb3-contrib, imitation
+Python 3.8.10
+[ INFO] [1693431990.500186587]: Updated sim with live config: Rate=40 incoming rate=40
+[ INFO] [1693431990.508012836]: Using default queue size of 10 for publisher queues... 
+[ INFO] [1693431990.520342905]: Loading scene [/home/rosdev/social_gym/config/gym_gen/scene.xml] for simulation
+[ INFO] [1693431990.540756953]: node initialized, now running 
+Service Mode

scripts/create_env_template.py

@@ -82,6 +82,8 @@ def create_new_env(name: str, template: str = None):
     out = subprocess.Popen(cmd, stdin=subprocess.PIPE, stdout=subprocess.PIPE, stderr=subprocess.PIPE)
     out.wait()
 
+    print("HI")
+
     subprocess.Popen(["sudo", "chmod", "-R", "a+rwX",  f"{ROOT_FOLDER}"], stdin=subprocess.PIPE, stdout=subprocess.PIPE, stderr=subprocess.PIPE)
     subprocess.Popen(["sudo", "chmod", "-R", "a+rwX",  f"{AMRL_MAPS_FOLDER}"], stdin=subprocess.PIPE, stdout=subprocess.PIPE, stderr=subprocess.PIPE)
     subprocess.Popen(["sudo", "chmod", "-R", "a+rwX",  f"{UT_MULTI_ROBOT_SIM_MAPS_FOLDER}"], stdin=subprocess.PIPE, stdout=subprocess.PIPE, stderr=subprocess.PIPE)

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, GraphNavScenario
+from src.environment.scenarios import CycleScenario, GraphNavScenario, ManualScenario
 from src.environment.utils.utils import DATA_FOLDER
 from src.environment.utils.evaluate_policy import evaluate_policy
 import datetime
@@ -320,8 +320,12 @@ def run(
     scenarios.append(scenario)
     zones.append(conflict_zone)
   if 'envs_multi' in experiment_names:
-    scenario = GraphNavScenario("tmp/multi-scenario")
+    # scenario = GraphNavScenario("multienv", partially_observable=partially_observable, config_runner=True if not monitor and not local else False, all_config=monitor and not local)
+    scenario = ManualScenario("multienv", agent_paths=[[0, 1], [1,0], [1, 2]], human_paths=[[0, 1]], partially_observable=partially_observable, config_runner=True if not monitor and not local else False, all_config=monitor and not local)
+
     scenarios.append(scenario)
+    # conflict_zone = (0, 1, 1)
+    # zones.append(conflict_zone)
 
 
   observations = []

src/environment/environment_types/manual_zone.py

@@ -153,6 +153,10 @@ def build_zone(self):
         start = np.array(scenario.robot_positions[start_point])
         end = np.array(scenario.robot_positions[end_point])
 
+        if start.shape[0] > 1:
+            start = start[0]
+            end = end[0]
+
         line = end - start
         length = np.linalg.norm(line)
         line /= length

src/environment/ros_social_gym.py

@@ -335,7 +335,10 @@ def reset(self, seed=None, return_info=True, options=None):
         self.last_reward_maps = []
 
         if not return_info:
-            return {agent: obs for agent, obs in zip(self.agents, observations)}
+            infos = {
+                agent: {} for agent in self.possible_agents if agent in self.agents
+            }
+            return {agent: obs for agent, obs in zip(self.agents, observations)}, infos
         else:
             infos = {
                 agent: {} for agent in self.possible_agents if agent in self.agents
@@ -423,20 +426,22 @@ def step(self, action_dict):
 
             } for agent, obs_map in zip(self.possible_agents, observation_maps) if agent in self.agents
         }
-        agent_infos["robot_data"] = agent_infos.pop(self.agents[0])
-        agent_infos["pedestrian_data"] = agent_infos.pop(self.agents[1])
+        agent_infos["robot_data"] = agent_infos[self.agents[0]]
+        agent_infos["pedestrian_data"] = agent_infos[self.agents[1]]
         agent_infos["pedestrian_data"] = [agent_infos["pedestrian_data"]] 
         agent_infos["collisions"] = 0
         agent_infos["success"] = False
         agent_infos["timestep"] = int(time.time())
 
+
         # if all([x.get('succeeded') for x in agent_infos.values()]):
         #     print('major reward')
         #     agent_rewards = {k: 100_000 for k in agent_rewards.keys()}
 
         self.terminations_ = list(agent_terminations.values())
 
         truncs = {agent: False if obs_map.get('success_observation', 0) == 0 else True for agent, obs_map in zip(self.agents, observation_maps)}
+        #print(agent_infos, flush=True)
 
         # return agent_observations, agent_rewards, agent_terminations, truncs, agent_infos
         return agent_observations, agent_rewards, agent_terminations, truncs, agent_infos

src/environment/scenarios/types/graph_nav.py

@@ -140,9 +140,12 @@ def __init__(
             allowed_agent_start_positions: List[int] = None,
             allowed_agent_goal_positions: List[int] = None,
             allowed_human_start_positions: List[int] = None,
-            allowed_human_goal_positions: List[int] = None
+            allowed_human_goal_positions: List[int] = None,
+            config_runner: bool = False,
+            all_config: bool = False,
+            partially_observable: bool = False
     ):
-        super().__init__(env_name=env_name)
+        super().__init__(env_name=env_name, config_runner=config_runner, all_config=all_config, partially_observable=partially_observable)
 
         self.allowed_agent_start_positions = allowed_agent_start_positions
         self.allowed_agent_goal_positions = allowed_agent_goal_positions

src/templates/multienv/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=multienv"  /><include file="$(find social_gym)/config/gym_gen/pedsim_launch.launch" />
+
+            <param name="enable_statistics" value="true" />
+        </launch>
+

src/templates/multienv/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=multienv"  /><include file="$(find social_gym)/config/gym_gen/pedsim_launch.launch" />
+
+    <param name="enable_statistics" value="true" />
+</launch>
+

src/templates/multienv/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=multienv" />
+
+    <include file="$(find ut_multirobot_sim)/config/gym_gen/pedsim_launch.launch" />
+
+    <param name="enable_statistics" value="true" />
+</launch>

src/templates/multienv/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 %}

src/templates/multienv/launch.launch

@@ -0,0 +1,37 @@
+<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" />
+
+
+    <node pkg="graph_navigation" type="social_nav" name="graph_navigation" output="screen" args="-service_mode=true -map=multienv"  />
+    <include file="$(find social_gym)/config/gym_gen/pedsim_launch.launch" />
+
+    <param name="enable_statistics" value="true" />
+
+    <param name="enable_statistics" value="true" />
+</launch>