CrowdBotSimulator crowd example

Assets/AddOns/RosSharp/Scripts/RosBridgeClient/RosCommuncation/TwistArrayStampedPublisher.cs

@@ -5,9 +5,11 @@ namespace RosSharp.RosBridgeClient
 {
     public class TwistArrayStampedPublisher : Publisher<Messages.crowdbotsim.TwistArrayStamped>
     {
+        [System.NonSerialized]
         public Rigidbody[] agent;
         private Messages.crowdbotsim.TwistArrayStamped message;
 
+        [System.NonSerialized]
         public bool started = false;
         private bool initialized = false;
 

Assets/AddOns/RosSharp/Scripts/RosBridgeClient/RosCommuncation/TwistArrayStampedSubscriber.cs

@@ -5,10 +5,13 @@ namespace RosSharp.RosBridgeClient
 {
     public class TwistArrayStampedSubscriber : Subscriber<Messages.crowdbotsim.TwistArrayStamped>
     {
+        [System.NonSerialized]
         public UnityTwist[] ut;
+        [System.NonSerialized]
         public Rigidbody[] agent;
         private bool isMessageReceived;
 
+        [System.NonSerialized]
         public bool started = false;
         
 

Assets/Demo/GroupExperiment/GroupExperiment.unity

@@ -329,7 +329,7 @@ MonoBehaviour:
   m_Script: {fileID: 11500000, guid: c8e7046e9f110da4a81262f9658c8514, type: 3}
   m_Name: 
   m_EditorClassIdentifier: 
-  Name: Vertical
+  Name: Vertical2
 --- !u!114 &1204470961
 MonoBehaviour:
   m_ObjectHideFlags: 0
@@ -342,7 +342,7 @@ MonoBehaviour:
   m_Script: {fileID: 11500000, guid: c8e7046e9f110da4a81262f9658c8514, type: 3}
   m_Name: 
   m_EditorClassIdentifier: 
-  Name: Horizontal
+  Name: Horizontal2
 --- !u!114 &1204470962
 MonoBehaviour:
   m_ObjectHideFlags: 0
@@ -433,8 +433,6 @@ MonoBehaviour:
   m_EditorClassIdentifier: 
   Topic: /crowd/vel
   TimeStep: 0
-  agent: []
-  started: 0
 --- !u!114 &1204470968
 MonoBehaviour:
   m_ObjectHideFlags: 0
@@ -462,8 +460,6 @@ MonoBehaviour:
   m_Name: 
   m_EditorClassIdentifier: 
   Topic: /crowd/pos
-  agent: []
-  started: 0
 --- !u!1001 &1616151443
 PrefabInstance:
   m_ObjectHideFlags: 0

Assets/Examples/CrowdBot.meta

+fileFormatVersion: 2
+guid: ba141974dd5040cafbd2adba06988335
+folderAsset: yes
+DefaultImporter:
+  externalObjects: {}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/Examples/CrowdBot/1D_flow_pepper_player.meta

+fileFormatVersion: 2
+guid: 29a02333a4442b7c1bbf2d1ad1cb55d2
+folderAsset: yes
+DefaultImporter:
+  externalObjects: {}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/MainAssets/Script/Agents/ControlSim/CrowdBot.meta

+fileFormatVersion: 2
+guid: 21aff979fe044a125b298e3fac15b62a
+folderAsset: yes
+DefaultImporter:
+  externalObjects: {}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/MainAssets/Script/Agents/ControlSim/CrowdBot/CrowdBotControlSim.cs

+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+
+public class CrowdBotControlSim : ControlSim
+{
+    int ConfigId;
+    CrowdBot.Simulator sim;
+
+    public CrowdBotControlSim(int id)
+    {
+        ConfigId = id;
+        sim = new CrowdBot.Simulator();
+    }
+
+    public void addAgent(Vector3 position, TrialControlSim infos)
+    {
+        CrowdBotControlSimConfig ConfigInfos = (CrowdBotControlSimConfig)infos;
+        sim.addAgent(position, ConfigInfos.neighborDist, ConfigInfos.maxNeighbors, ConfigInfos.timeHorizon, ConfigInfos.timeHorizonObst, ConfigInfos.radius, ConfigInfos.maxSpeed, new Vector3(0,0,0));
+    }
+
+    public void addNonResponsiveAgent(Vector3 position, float radius)
+    {
+        sim.addAgent(position, 0, 0, 0, 0, radius, 5, new Vector3(0,0,0));
+    }
+
+    public void addObstacles(Obstacles obst)
+    {
+        foreach (ObstCylinder pillar in obst.Pillars)
+        {
+            sim.addAgent(pillar.position, 0, 0, 0, 0, pillar.radius, 5, new Vector3(0,0,0));
+        }
+
+        foreach (ObstWall wall in obst.Walls)
+        {
+            sim.addWall(wall);
+        }
+    }
+
+    public void clear()
+    {
+        sim.Clear();
+    }
+
+    public void doStep(float deltaTime)
+    {
+        sim.setTimeStep(deltaTime);
+        sim.doStep();
+    }
+
+    public Vector3 getAgentPosition(int id)
+    {
+        return sim.getAgentPosition(id);
+    }
+
+    public Vector3 getAgentSpeed(int id)
+    {
+        return sim.getAgentVelocity(id);
+    }
+
+    public int getConfigId()
+    {
+        return ConfigId;
+    }
+
+    public void updateAgentState(int id, Vector3 position, Vector3 goal)
+    {
+        sim.setAgentPosition(id, position);
+        sim.setAgentPrefVelocity(id, goal);
+    }
+}

Assets/MainAssets/Script/Agents/ControlSim/CrowdBot/CrowdBotControlSim.cs.meta

+fileFormatVersion: 2
+guid: ff06c4ac162ece9c39eb42e7c9559368
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/MainAssets/Script/Agents/ControlSim/CrowdBot/CrowdBotControlSimConfig.cs

+using System.Collections;
+using System.Collections.Generic;
+using System.Xml.Serialization;
+using UnityEngine;
+
+public class CrowdBotControlSimConfig : TrialControlSim
+{
+    [XmlAttribute("SimulationID")]
+    public int id;
+    [XmlAttribute]
+    public float neighborDist;
+    [XmlAttribute]
+    public int maxNeighbors;
+    [XmlAttribute]
+    public float timeHorizon;
+    [XmlAttribute]
+    public float timeHorizonObst;
+    [XmlAttribute]
+    public float radius;
+    [XmlAttribute]
+    public float maxSpeed;
+
+    public int getConfigId()
+    {
+        return id;
+    }
+
+    public ControlSim createControlSim(int id)
+    {
+        
+        return new CrowdBotControlSim(id);
+    }
+
+    public CrowdBotControlSimConfig()
+    {
+        id = 0;
+        neighborDist = 5;
+        maxNeighbors = 3;
+        timeHorizon = 5;
+        timeHorizon = 2;
+        radius = 0.33f;
+        maxSpeed = 2;
+
+    }
+
+}

Assets/MainAssets/Script/Agents/ControlSim/CrowdBot/CrowdBotControlSimConfig.cs.meta

+fileFormatVersion: 2
+guid: 61a72843b2d47791bad5c05dfe1c6894
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/MainAssets/Script/Agents/ControlSim/CrowdBot/Lib.meta

+fileFormatVersion: 2
+guid: 30053fe63e65e4dc4aba81a71f49ee50
+folderAsset: yes
+DefaultImporter:
+  externalObjects: {}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/MainAssets/Script/Agents/ControlSim/CrowdBot/Lib/CrowdBotCrowdSimulator.cs

+using System;
+using System.Collections.Generic;
+using UnityEngine;
+
+namespace CrowdBot
+{
+    public class Simulator
+    {
+        List<Agent> agents_;
+        List<ObstWall> walls_;
+
+        float timeStep_ = 0.01f;
+
+        private float sigDca_ = 0.3f;
+        private float sigTtca_ = 0.3f;
+        float sigDist_ = 0.1f;
+        private float coefficient_ = 8.0f;
+        private float coefficientSpeed_ = 0.05f;
+        private float coefficientDist_ = 0.001f;
+        private float alpha = 0.15f;
+
+        private float eps = 0.001f;
+
+        public Simulator()
+        {        
+            agents_ = new List<Agent>();
+            walls_ = new List<ObstWall>();
+        }
+
+        public int addAgent(Vector3 position, float neighborDist, int maxNeighbors, float timeHorizon, float timeHorizonObst, float radius, float maxSpeed, Vector3 velocity)
+        {
+            Agent agent = new Agent();
+            agent.id_ = agents_.Count;
+            agent.maxNeighbors_ = maxNeighbors;
+            agent.maxSpeed_ = maxSpeed;
+            agent.neighborDist_ = neighborDist;
+            agent.setPosition(position);
+            agent.radius_ = radius;
+            agent.timeHorizon_ = timeHorizon;
+            agent.timeHorizonObst_ = timeHorizonObst;
+            agent.setVelocity(velocity);
+            agents_.Add(agent);
+
+            return agent.id_;
+        }
+        
+        public void Clear()
+        {
+            agents_.Clear();
+        }
+
+        public void setTimeStep(float deltaTime)
+        {
+            timeStep_ = deltaTime;
+        }
+
+        public void doStep()
+        {
+            Vector3[] new_vels = new Vector3[agents_.Count];
+            //Do the magic here
+            foreach(Agent a in agents_)
+            {
+                Vector3 Gradient = new Vector3();
+                Vector3 Vnorm = a.getVelocity().normalized;
+
+	            Matrix2x2 R = new Matrix2x2(new Vector3(Vnorm.z, -Vnorm.x), Vnorm);
+
+                float TotalCost = 0;
+            	float GradTh = 0;
+            	float GradS = 0;
+                Vector3 GradDist = Vector3.zero;
+                Vector3 GradWalls = Vector3.zero;
+    
+                //find the neighbors
+                List<int> neighbors_id = FindNeighbors(a.id_);
+
+             	int NumAgentsVisible = 0;
+
+                // for each agent of the neighbourhood
+                foreach(int n_id in neighbors_id)
+                {
+                    // Compute relative velocity and relative position
+                    Vector3 relPos = getAgentPosition(n_id) - a.getPosition();
+                    float radius_diff = agents_[n_id].radius_ + a.radius_;
+                    
+                    relPos = relPos.magnitude > radius_diff ? relPos - relPos.normalized * radius_diff : relPos.normalized * eps;
+
+                    Vector3 relVelocity = getAgentVelocity(n_id) - a.getVelocity();
+
+                    GradDist += coefficientDist_ * relPos / (2*sigDist_*sigDist_) * Mathf.Exp(-0.5f*(relPos.magnitude/sigDist_)*(relPos.magnitude/sigDist_)) * a.getVelocity().sqrMagnitude;
+
+                    //Only see agents in front of the direction of motion
+                    if (Vector3.Dot(relPos, a.getVelocity()) < 0) {
+                        continue;
+                    }
+
+                    float ttca = 0;
+                    float dca = 0;
+             		Vector3 vdca = new Vector3(0,0,0);
+
+                    // there is adaptation only if relative velocity is not zero
+                    if (relVelocity.sqrMagnitude > eps)
+                    {
+                        // computing ttc and dca
+                        ttca = -Vector3.Dot(relPos, relVelocity) / relVelocity.sqrMagnitude;
+                        vdca = (relPos + ttca * relVelocity);
+                        dca = vdca.magnitude;
+                        if (ttca < 0) continue;
+
+                        // saving the total cost
+                        float cost = Cost(ttca,dca);
+
+                        // cost = cost * (coefficient_ / relPos.sqrMagnitude); //simulate num of pixels of an agent in screen
+                        cost = cost * (coefficient_ / relPos.magnitude); //simulate num of pixels of an agent in screen
+                        TotalCost += cost;
+            
+                        // computing gradients of the cost regarding the speed and velocity of the main agent
+                        Vector3 velRot = new Vector3(a.getVelocity().z,0,-a.getVelocity().x);
+                        float gradTtcaAngle = -Vector3.Dot(relPos + 2*ttca*relVelocity,velRot) / (Vector3.Dot(relVelocity,relVelocity));
+                        float gradTtcaSpeed = Vector3.Dot(relPos + 2*ttca*relVelocity,a.getVelocity().normalized) / (Vector3.Dot(relVelocity,relVelocity));
+
+                        float gradDcaAngle, gradDcaSpeed;
+                        if(Mathf.Abs(dca) > eps){
+                            gradDcaAngle = Vector3.Dot(vdca, gradTtcaAngle*relVelocity + ttca*velRot) / dca;
+                            gradDcaSpeed = Vector3.Dot(vdca, gradTtcaSpeed*relVelocity - ttca * (a.getVelocity().normalized))/ dca;
+                        }
+                        else { 
+                            gradDcaAngle = 0;
+                            gradDcaSpeed = 0;
+                        }
+
+                        float gradCSpeed = -cost * ( gradTtcaSpeed * (ttca/(sigTtca_*sigTtca_)) +  gradDcaSpeed * (dca/(sigDca_*sigDca_)));
+                        float gradCAngle = -cost * ( gradTtcaAngle * (ttca/(sigTtca_*sigTtca_)) + gradDcaAngle * (dca/(sigDca_*sigDca_)));
+
+                        gradCSpeed += 0.5f*UnityEngine.Random.Range(-1.0f,1.0f);
+                        gradCAngle += 1.0f*UnityEngine.Random.Range(-1.0f,1.0f);
+                        gradCSpeed *= coefficientSpeed_;
+                        gradCAngle *= coefficient_;
+
+                        GradTh += gradCAngle;
+                        GradS += gradCSpeed;
+
+                        ++NumAgentsVisible;
+                    }
+                    // else{
+                    //     GradS = coefficientSpeed_*UnityEngine.Random.Range(-1.0f,1.0f);
+                    //     GradTh = coefficient_*UnityEngine.Random.Range(-1.0f,1.0f);
+                    // }
+                }
+                foreach(ObstWall wall in walls_)
+                {
+                    //TODO: better
+
+                    //find the longest part
+                    Vector3 L1 = wall.B - wall.A;
+                    Vector3 L2 = wall.D - wall.A;
+
+                    Vector3 AB = (L1.magnitude > L2.magnitude) ? L1 : L2;
+                    Vector3 AP = a.getPosition() - wall.A;
+                    Vector3 AM = Vector3.Dot(AP,AB)*AB.normalized;
+                    Vector3 PM = AP - AM;
+                    
+                    // 1000 factor to boost it
+                    GradWalls = PM.normalized * Mathf.Exp(-0.5f*(PM.sqrMagnitude))*1000.0f;
+                }
+                if(Mathf.Abs(GradS) > a.getVelocity().magnitude || a.getVelocity().magnitude < eps) GradS = 0;
+                // GradTh *= coefficient_;
+                // GradS *= coefficient_; 
+                // GradS = Mathf.Clamp(GradS,-0.5f,0.5f); 
+                // GradS = 0; 
+                // GradTh = Mathf.Clamp(GradTh, -0.5f, 0.5f);
+        
+                // Gradient = -1 * R.product(new Vector3(Mathf.Sin(GradTh), 0, 1 - Mathf.Cos(GradTh))*a.getVelocity().magnitude + new Vector3(0,0,GradS));
+                // Gradient = a.getVelocity().normalized - Quaternion.AngleAxis(GradTh, Vector3.up) * a.getVelocity().normalized * (1 + GradS);
+                // Gradient = -(a.getVelocity().normalized - Quaternion.AngleAxis(GradTh, Vector3.up) * a.getVelocity().normalized);
+                // Gradient = -(a.getVelocity().normalized - Quaternion.AngleAxis(GradTh, Vector3.up) * a.getVelocity().normalized*(1+GradS)) - GradDist - GradWalls;
+                Gradient = -(a.getVelocity().normalized - Quaternion.AngleAxis(GradTh, Vector3.up) * a.getVelocity().normalized);
+                Gradient -= a.getVelocity()*GradS;
+                Gradient -= GradDist;
+                if (NumAgentsVisible > 0) {
+                    Gradient = Gradient / (float)NumAgentsVisible;
+                    TotalCost = TotalCost / (float)NumAgentsVisible;
+                }
+
+
+                Vector3 new_delta_vel = new Vector3();
+
+                //TODO Debug this
+
+                // float CostAngle = -0.5f*Vector3.Angle(a.pref_velocity_, a.velocity_)*Mathf.Exp(-0.5f*( Mathf.Pow(Vector3.Angle(a.pref_velocity_, a.velocity_),2)));
+                // float CostSpeed = -0.5f*(a.velocity_.magnitude - a.pref_velocity_.magnitude)*Mathf.Exp(-0.5f*( Mathf.Pow(a.velocity_.magnitude - a.pref_velocity_.magnitude,2)));
+
+                // Vector3 CostMov = new Vector3(CostAngle, 0, CostSpeed);
+
+                // if(a.velocity_.magnitude > 0){
+                //     // new_vel = Quaternion.AngleAxis(CostMov.x, Vector3.up) * (a.velocity_.normalized * (a.velocity_.magnitude + CostMov.z));
+                //     new_vel = Quaternion.LookRotation(a.pref_velocity_, Vector3.up) * (a.velocity_.normalized * (a.velocity_.magnitude + CostMov.z));
+                // }
+                // else{
+                //     new_vel = Quaternion.LookRotation(a.pref_velocity_, Vector3.up) * (a.pref_velocity_.normalized * CostMov.z);
+                // }
+
+                //new_delta_vel = (a.pref_velocity_ - a.velocity_) + Gradient;
+                new_delta_vel = alpha * (a.pref_velocity_ - a.getVelocity()) + (1-alpha) * Gradient;
+                // if(new_delta_vel.magnitude >= 0.8f)
+                // {
+                //     new_delta_vel = new_delta_vel.normalized * 0.8f;
+                // }
+                new_vels[a.id_] = a.getVelocity() + new_delta_vel;
+                if(new_vels[a.id_].magnitude > 2.0f){
+                    new_vels[a.id_] = new_vels[a.id_].normalized*2.0f;
+                }
+                // a.setVelocity(new_vels[a.id_]);
+                // a.setPosition(a.getPosition() + a.getVelocity() * timeStep_);
+                // Debug.DrawLine(a.getPosition(), a.getPosition() + Gradient*10.0f);
+            }
+            
+            foreach(Agent a in agents_)
+            {
+                a.setVelocity(new_vels[a.id_]);
+                a.setPosition(a.getPosition() + a.getVelocity() * timeStep_);
+            }
+        }
+
+        private struct Matrix2x2 {
+            float a11_;
+            float a12_;
+            float a21_;
+            float a22_;
+            public Matrix2x2(Vector3 v1, Vector3 v2) {
+                a11_ = v1.x;
+                a21_ = v1.z;
+                a12_ = v2.x;
+                a22_ = v2.z;
+            }
+            public Vector3 product(Vector3 v)
+            {
+                return new Vector3(a11_*v.x + a12_*v.z, 0, a21_*v.x + a22_*v.z);
+            }
+        }
+
+        float Cost(float ttca, float dca){
+            return Mathf.Exp(-0.5f*( (ttca/sigTtca_)*(ttca/sigTtca_) + (dca/sigDca_)*(dca/sigDca_) ) );
+        }
+
+        public Vector3 getAgentPosition(int id)
+        {
+            return agents_[id].getPosition();
+        }
+        public Vector3 getAgentVelocity(int id)
+        {
+            return agents_[id].getVelocity();
+        }
+        public void setAgentPosition(int id, Vector3 position)
+        {
+            agents_[id].setPosition(position);
+
+        }
+        public void setAgentPrefVelocity(int id, Vector3 goal)
+        {
+            agents_[id].pref_velocity_ = goal;
+        }
+
+        private List<int> FindNeighbors(int a_id)
+        {
+            //TODO: KDTREE
+            List<int> n = new List<int>();
+            if(agents_[a_id].maxNeighbors_ == 0) return n;
+            for(int i = 0; i < agents_.Count; i++)
+            {
+                if(n.Count >= agents_[a_id].maxNeighbors_) 
+                    return n;
+                if(i == a_id) 
+                    continue;
+                if(Vector3.Distance(getAgentPosition(a_id), getAgentPosition(i)) < agents_[a_id].neighborDist_)
+                {
+                    n.Add(i);                
+                }
+            }
+            return n;
+        }
+
+        public void addWall(ObstWall wall)
+        {
+            walls_.Add(wall);
+        }
+    }
+}

Assets/MainAssets/Script/Agents/ControlSim/CrowdBot/Lib/CrowdBotCrowdSimulator.cs.meta

+fileFormatVersion: 2
+guid: 70fa961f7860f2e4da3c7d1faf88fe08
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/MainAssets/Script/Agents/ControlSim/CrowdBot/Lib/agent.cs

+using System;
+using System.Collections.Generic;
+using UnityEngine;
+
+namespace CrowdBot
+{
+    public class Agent
+    {
+        public int id_;
+        public int maxNeighbors_;
+        public float maxSpeed_;
+        public float neighborDist_;
+        private Vector3 position_;
+        public float radius_;
+        public float timeHorizon_;
+        public float timeHorizonObst_;
+        private Vector3 velocity_;
+        public Vector3 pref_velocity_;
+
+        public Agent()
+        {
+            id_ = -1;
+            maxNeighbors_ = 10;
+            maxSpeed_ = 2.0f;
+            neighborDist_ = 10.0f;
+            position_ = Vector3.zero;
+            radius_ = 0.3f;
+            timeHorizon_ = 10.0f;
+            timeHorizonObst_ = 10.0f;
+            velocity_ = Vector3.zero;
+            pref_velocity_ = Vector3.zero;
+        }
+
+        public void setVelocity(Vector3 v)
+        { 
+            velocity_ = Vector3.Lerp(getVelocity(),new Vector3(v.x,0,v.z),0.5f);
+        }
+        public void setPosition(Vector3 v)
+        { 
+            position_ = new Vector3(v.x,0,v.z);
+        }
+
+        public Vector3 getPosition(){ return new Vector3(position_.x, 0, position_.z); }
+        public Vector3 getVelocity()
+        { 
+            Vector3 vel = new Vector3(velocity_.x, 0, velocity_.z);
+            return vel;
+        }
+    }
+}
\ No newline at end of file

Assets/MainAssets/Script/Agents/ControlSim/CrowdBot/Lib/agent.cs.meta

+fileFormatVersion: 2
+guid: 043014237e87ec31d9fa6d2412127833
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/MainAssets/Script/Agents/ControlSim/Simulations.cs

@@ -39,12 +39,10 @@ public class SimManager {
 
         // Create simulators
         agentToSim.Add(getInternalId(LoaderConfig.playerInfo.getControlSimInfo()));
-        //tmp.Add(LoaderConfig.playerInfo.getControlSimInfo());
 
         foreach (TrialAgent a in LoaderConfig.agentsInfo)
         {
             agentToSim.Add(getInternalId(a.getControlSimInfo()));
-            //tmp.Add(a.getControlSimInfo());
         }
 
         // Fill simulators with agents and Obstacles

Assets/MainAssets/Script/Managers/CrowdBotSimMainManager.cs

@@ -69,7 +69,7 @@ public class CrowdBotSimMainManager : MainManager {
         ToolsTime.newLevel();
 
         // Initialize the trial manager
-        currentTrialManager = sceneGameObject.GetComponent<CrowdBotSimTrial>();
+        currentTrialManager = sceneGameObject.GetComponent<TrialManager>();
         if (currentTrialManager == null)
         {
             ToolsDebug.logFatalError("Trials " + LoaderConfig.xpCurrentTrial.ToString() + " does not have a manager");

Assets/MainAssets/Script/Managers/CrowdBotSimMainManager.cs.meta

@@ -4,7 +4,7 @@ MonoImporter:
   externalObjects: {}
   serializedVersion: 2
   defaultReferences: []
-  executionOrder: 0
+  executionOrder: -1
   icon: {instanceID: 0}
   userData: 
   assetBundleName: 

Assets/MainAssets/Script/Managers/DutraMainManager.cs

+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+using UnityEngine.UI;
+
+/// <summary>
+/// The main manager controlling the Experiment flow
+/// </summary>
+public class DutraMainManager : MainManager {
+
+    [System.NonSerialized]
+    public GameObject[] robotsModels;
+
+    // Use this for initialization
+    public override void Start () {
+
+
+        if (!initializeConstants())
+        {
+            ToolsDebug.logFatalError("Error on GUIManager constants initialisation");
+            Application.Quit();
+        }
+
+        isEndXP = false;
+        startTrial();
+        startTransition();
+    }
+    
+    /// <summary>
+    /// Load and setup current trial
+    /// </summary>
+    public override void startTrial()
+    {
+        // Load trial file
+        if (!LoaderConfig.LoadXP())
+        {
+            ToolsDebug.log("End of the trials", 2);
+            isEndXP = true;
+            //Application.Quit();
+            return;
+        }
+        
+        // Load the trial scene and unload the others
+        string sceneMeshName = LoaderConfig.sceneName;
+        foreach (GameObject currentGameObject in sceneObjectsModels)
+        {
+            /*Make every scene disapered exept the one given by the scenarioLoader*/
+            if (currentGameObject.name == sceneMeshName)
+            {
+                currentGameObject.SetActive(true);
+
+                if (currentGameObject.GetComponent<TrialManager>() == null)
+                {
+                    ToolsDebug.logFatalError("The trial need a Manager");
+                    return;
+                }
+
+
+                sceneGameObject = currentGameObject;
+                sceneGameObject.transform.position = LoaderConfig.scenePosition;
+                sceneGameObject.transform.rotation = Quaternion.Euler(LoaderConfig.sceneRotation);
+                break;
+            }
+        }
+
+        if (sceneGameObject == null)
+            ToolsDebug.logFatalError("Scene not found (" + sceneMeshName + ")");
+
+        // Reset trial time
+        ToolsTime.newLevel();
+
+        // Initialize the trial manager
+        currentTrialManager = sceneGameObject.GetComponent<TrialManager>();
+        if (currentTrialManager == null)
+        {
+            ToolsDebug.logFatalError("Trials " + LoaderConfig.xpCurrentTrial.ToString() + " does not have a manager");
+            Application.Quit();
+        }
+        currentTrialManager.initializeTrial(playerModels, agentModels);
+
+        trialStarted = false;
+    }
+
+    /// <summary>
+    /// Method used to initialize scene constants
+    /// </summary>
+    /// <returns>True if constant are well initialized, otherwise False</returns>
+    protected override bool initializeConstants()
+    {
+        playerModels =GameObject.FindGameObjectsWithTag("Player");
+        foreach (GameObject p in playerModels)
+        {
+            p.SetActive(false);
+        }
+        sceneObjectsModels = GameObject.FindGameObjectsWithTag("Stage");
+        foreach (GameObject scene in sceneObjectsModels)
+        {
+            scene.SetActive(false);
+        }
+
+        agentModels = GameObject.FindGameObjectsWithTag("AgentModels");
+        foreach (GameObject go in agentModels)
+        {
+            go.SetActive(false);
+        }
+
+        robotsModels = GameObject.FindGameObjectsWithTag("Robot");
+        foreach (GameObject go in robotsModels)
+        {
+            go.SetActive(false);
+        }
+
+        PluginManager[] plugins = gameObject.GetComponentsInChildren<PluginManager>(); 
+        foreach (PluginManager plugin in plugins)
+        {
+            plugin.LoadPlugin();
+        }
+
+        return true;
+    }
+
+    // Update is called once per frame
+    void Update () {
+        ToolsTime.updateTime();
+
+        if (currentTrialManager == null)
+            return;
+
+        if (currentTrialManager.hasEnded())
+            endTrial();
+        else
+            currentTrialManager.doStep();
+    }
+
+    // Handle key/button input in LateUpdate
+    protected override void LateUpdate()
+    {
+        if ((currentTrialManager==null || currentTrialManager.isReady()) && (ToolsInput.GetKeyDown(KeyCode.Space) || ToolsInput.GetButtonDown(ButtonCode.Fire1)))
+        {
+            if (!trialStarted)
+            {
+                trialStarted = true;
+                currentTrialManager.startTrial();
+            }
+
+            if (isEndXP)
+                Application.Quit();       
+            else
+                ToolsTime.tooglePause();
+        }
+
+        if (ToolsInput.GetKeyDown(KeyCode.N))
+        {
+            endTrial();
+        }
+
+        if (ToolsInput.GetKeyDown(KeyCode.P))
+        {
+            endTrial(-1);
+        }
+
+        if (ToolsInput.GetKeyDown(KeyCode.R))
+        {
+            endTrial(0);
+        }
+
+        if (ToolsInput.GetKeyDown(KeyCode.Escape))
+        {
+            Application.Quit();
+        }
+    }
+
+    /// <summary>
+    /// End the current trial change it
+    /// </summary>
+    void endTrial(int trialSwitch=1)
+    {
+        currentTrialManager.clear();
+        sceneGameObject.gameObject.SetActive(false);
+        sceneGameObject = null;
+        currentTrialManager = null;
+        LoaderConfig.ChangeTrial(trialSwitch);
+        startTrial();
+    }
+
+    /// <summary>
+    /// Update and load transition screen
+    /// </summary>
+    public override void startTransition()
+    {
+        if (isEndXP)
+        {
+            player.gameObject.SetActive(true);
+            return;
+        }
+    }
+}