#24 fixed publish at clock

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

@@ -28,8 +28,6 @@ namespace RosSharp.RosBridgeClient
 
         protected override void Start()
         {
-            Debug.Log(GetComponent<RosConnector>());
-            Debug.Log(GetComponent<RosConnector>().RosSocket);
             base.Start();
             InitializeMessage();
         }

Assets/MainAssets/Robots/Scripts/Sensors/Lidar/LidarPublisher.cs

@@ -5,7 +5,7 @@ using RosSharp;
 using System;
 
 namespace RosSharp.RosBridgeClient{
-public class LidarPublisher : Publisher<Messages.Sensor.LaserScan>
+public class LidarPublisher : Publisher<Messages.Sensor.LaserScan>, Publisher
 {
     [SerializeField]
     [Tooltip ("Sensor providing the data to be published")]
@@ -43,7 +43,7 @@ public class LidarPublisher : Publisher<Messages.Sensor.LaserScan>
     {
         // if (Time.realtimeSinceStartup >= previousScanTime + scanPeriod)
         // {
-            UpdateMessage();
+            // UpdateMessage();
             // previousScanTime = Time.realtimeSinceStartup;
         // }
     }

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

@@ -161,6 +161,7 @@ public class SimManager {
     public void doStep(float deltaTime, List<Vector3> posList, Agent playerGoalState, List<Agent> agentsGoalState, List<Robot> robots = null)
     {
         int i = 0;
+        bool player_in_sim = true;
 
         // Update simulators state
         foreach (ControlSim sim in simulators)
@@ -175,6 +176,7 @@ public class SimManager {
                 else
                 {
                     i = -1;
+                    player_in_sim = false;
                 }
             }
 
@@ -195,8 +197,16 @@ public class SimManager {
 
         i = 0;
         int simId = agentToSim[i];
-        if (simId >= 0)
-            playerGoalState.simOverride(simulators[simId].getAgentPos2d(i), simulators[simId].getAgentSpeed2d(i));
+
+        if(player_in_sim)
+        {
+            if (simId >= 0)
+                playerGoalState.simOverride(simulators[simId].getAgentPos2d(i), simulators[simId].getAgentSpeed2d(i));
+        }
+        else
+        {
+            i = -1;
+        }
 
         foreach(Agent robot in robots){
             ++i;

Assets/MainAssets/Scripts/Managers/DataGen_TrialManager.cs

@@ -18,7 +18,7 @@ public class DataGen_TrialManager : MonoBehaviour, TrialManager {
 
     public GameObject ROS;
 
-    MonoBehaviour[] Ros_Scripts;
+    Publisher[] Ros_Publishers;
 
     private bool player_in_sim;
     // protected ToolsCamRecord camRecord;
@@ -160,9 +160,9 @@ public class DataGen_TrialManager : MonoBehaviour, TrialManager {
         }
 
         ROS = GameObject.FindGameObjectWithTag("Ros");
-        Ros_Scripts = ROS.GetComponents<MonoBehaviour>();
+        Ros_Publishers = ROS.GetComponents<Publisher>();
 
-        foreach(MonoBehaviour script in Ros_Scripts)
+        foreach(Publisher script in Ros_Publishers)
         {
             if(script.GetType() == typeof(TwistArrayStampedPublisher) )
             {
@@ -231,26 +231,27 @@ public class DataGen_TrialManager : MonoBehaviour, TrialManager {
     /// </summary>
     public virtual void doStep()
     {
-        foreach(MonoBehaviour script in Ros_Scripts)
-        {
-            if(script.GetType() == typeof(Publisher))
-            {
-                ((Publisher)script).UpdateMessage();
-            }
-        }
-
         if(!player_in_sim)
             player.doStep();
 
         if (ToolsTime.DeltaTime == 0)
             return;
+            
+        foreach(Publisher script in Ros_Publishers)
+        {
+            if(((MonoBehaviour)script).enabled == true)
+                script.UpdateMessage();
+        }
+
 
         List<Vector3> currPos = new List<Vector3>();
 
         // Do regular step
-        currPos.Add(player.Position);
         if(player_in_sim)
+        {
+            currPos.Add(player.Position);
             player.doStep();
+        }
 
         foreach (Robot r in robots)
         {