From 2d78d5e5b4c207bb7264ea49981fcaaae45e1438 Mon Sep 17 00:00:00 2001
From: "oceanpdoshi@g.ucla.edu" <oceanpdoshi@.ucla.edu>
Date: Tue, 23 Apr 2019 14:40:33 -0700
Subject: [PATCH] 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 ...
---
.../simulated_dataset_manager.py | 53 +++++++++++--------
1 file changed, 30 insertions(+), 23 deletions(-)
diff --git a/CoLo-AT/dataset_manager/simulated_dataset_manager.py b/CoLo-AT/dataset_manager/simulated_dataset_manager.py
index 965253a..2638659 100644
--- a/CoLo-AT/dataset_manager/simulated_dataset_manager.py
+++ b/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}
--
GitLab