Added handling for multiple robots

- internal robot_ctr that resets correctly
Order is (assuming 2 robots):
1: odo, meas
2: odo, meas
1: odo, meas
...

CoLo-AT/dataset_manager/simulated_dataset_manager.py

@@ -37,9 +37,9 @@ class SimulatedDataSetManager:
         self.robot_fov = robot_fov
         
         # Alternates between 'odometry' and 'measurement'
-        self.next_request = 'odometry'
-        # cycles through robot_labels
-        self.current_robot_idx = 0
+        self.curr_request = 'odometry'
+        # cycles through indices of robot_labels
+        self.curr_robot_idx = 0
         
         # Internal time index/counter
         self.time_idx = 0
@@ -204,6 +204,12 @@ class SimulatedDataSetManager:
         gt = self.groundtruth_data[robot_index][gt_time]
         return gt
 
+    def reset_robot_ctr(self):
+        if (self.curr_robot_idx > self.num_robots-1):
+            self.curr_robot_idx = 0
+            return True
+        return False
+
     def respond(self, req, current_time, need_specific_time = False):
 
         valid_respond, message, req_type, robot_idx = self.get_dataline(req, current_time)
@@ -215,40 +221,39 @@ class SimulatedDataSetManager:
             req.set_type(req_type)
 
         # TODO - add in update parameters for multiple robots (and robot_ctr about to reset)
-        if (req_type == 'measurement'):
+        if (req_type == 'measurement' and self.reset_robot_ctr()):
             current_time += self.delta_t
+            self.reset_robot_idx = False
 
         return valid_respond, current_time, req
 
-    # Alternates odometry and measurement updates
-    # Cycles through multiple robots
+    # Alternates odometry and measurement updates and cycles through multiple robots
     def get_dataline(self, req, current_time):
         message = req.get_message()
 
-        # Selection of req_type, robot_idx
         if message['robot_index'] == None:
+            
+            # select req_type, robot_idx
             if req.get_type() == None:
-                # Alternate between measurement and odometry requests
-                # TODO - change method of selecting robot indices - Suggestion (use a counter maintained within simulation)
-                req_type = self.next_request
+                req_type = self.curr_request
+                robot_idx = self.curr_robot_idx
                 req.set_type(req_type)
-                if (self.next_request == 'odometry'):
-                    robot_idx = np.argmin(self.get_time_arr('odometry'))
-                    self.next_request = 'measurement'
+                # alternate odometry and measurement requests
+                if (self.curr_request == 'odometry'):
+                    self.curr_request = 'measurement'
                 else:
-                    robot_idx = np.argmin(self.get_time_arr('measurement'))
-                    self.next_request = 'odometry'
+                    self.curr_request = 'odometry'
+                    # go to next robot
+                    self.curr_robot_idx += 1
             else:
                 req_type = req.get_type()
-                robot_idx = np.argmin(self.time_arr[req_type])
-            
-            message['robot_index'] = robot_idx
 
-        # Selection of time_idx for measurement or odometry and assosciated groundtruth
+            message['robot_index'] = robot_idx
         
-        # Past the total time of simulation or no further measurements TODO - ask william about when to stop simulation
-        valid_dataline = current_time <= self.end_time # and current_time <= max(self.time_arr['measurement'][robot_idx])
+        # Past the total time of simulation or no further measurements 
+        valid_dataline = current_time <= self.end_time
 
+        # Select message data
         if valid_dataline:
             if req_type == 'odometry':
                 odo_time_idx = np.where(self.time_arr[req_type][robot_idx] == current_time)[0][0] 
@@ -260,6 +265,7 @@ class SimulatedDataSetManager:
 
         gt_time_idx = np.where(self.time_arr['groundtruth'][robot_idx] == current_time)[0][0]
         message['groundtruth'] = self.groundtruth_data[robot_idx][gt_time_idx]
+        
         return valid_dataline, message, req_type, robot_idx
         
     # Check if a given loc [x,y] is within map
@@ -291,8 +297,9 @@ class SimulatedDataSetManager:
         
         return abs(bearing) <= robot_fov/2
 
-    # TODO - ask: should we not update the time and keep providing measurements to different landmarks, or just take the best
-    # TODO - ask: handling for no measurement available (but not at end of simulation) (see robot_system print statement)
+    # Inputs: current time, robot_idx
+    # Outputs:  closest landmark measurement for given time and robot, if available
+    #           {'time': current_time, 'subject_ID': None, 'measurment_range': None,'bearing': None} if no measurement available
     def find_measurement(self, current_time, robot_idx):
         
         # {'idx': idx, 'distance': measurment_range}