From d8d2feb3a2ed1d864b8e635c5532f4c611d507ec Mon Sep 17 00:00:00 2001
From: "oceanpdoshi@g.ucla.edu" <oceanpdoshi@.ucla.edu>
Date: Tue, 23 Apr 2019 14:11:02 -0700
Subject: [PATCH] added selection of closest landmark simulation now supports
multiple landmarks added handling for no measurement data available at a
given time (sim not over)
---
.../simulated_dataset_manager.py | 31 +++++++++++++------
1 file changed, 21 insertions(+), 10 deletions(-)
diff --git a/CoLo-AT/dataset_manager/simulated_dataset_manager.py b/CoLo-AT/dataset_manager/simulated_dataset_manager.py
index 3155991..965253a 100644
--- a/CoLo-AT/dataset_manager/simulated_dataset_manager.py
+++ b/CoLo-AT/dataset_manager/simulated_dataset_manager.py
@@ -27,7 +27,7 @@ class SimulatedDataSetManager:
self.boundary = boundary
self.robot_labels = robot_labels
self.num_robots = len(robot_labels)
- self.start_time = 20.0
+ self.start_time = 0.0
self.end_time = self.start_time + self.duration
self.start_moving_times = [self.start_time for label in robot_labels]
@@ -244,16 +244,17 @@ class SimulatedDataSetManager:
message['robot_index'] = robot_idx
- # Selection of time_idx
+ # Selection of time_idx for measurement or odometry and assosciated groundtruth
- # Stop simulation when you are the end of the dataset (no more measurements or groundtruths/odometry)
- 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 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])
if valid_dataline:
if req_type == 'odometry':
odo_time_idx = np.where(self.time_arr[req_type][robot_idx] == current_time)[0][0]
message['data'] = self.odometry_data[robot_idx][odo_time_idx]
- # if no measurement data available (ex: no landmarks were in range) message['data'] = None
+ # if no measurement data available (ex: no landmarks were in range) returns
+ # {'time': -1, 'subject_ID': None, 'measurment_range': None,'bearing': None}
elif req_type == 'measurement':
message['data'] = self.find_measurement(current_time, robot_idx)
@@ -290,12 +291,22 @@ class SimulatedDataSetManager:
return abs(bearing) <= robot_fov/2
- # TODO - add handling for multiple landmarks
- # TODO - add handling for no measurement available (but not at end of simulation)
+ # 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)
def find_measurement(self, current_time, robot_idx):
- for measurement in self.measurement_data[robot_idx]:
+
+ # {'idx': idx, 'distance': measurment_range}
+ available_measurements = []
+
+ for data_idx, measurement in enumerate(self.measurement_data[robot_idx]):
if (measurement['time'] == current_time):
- return measurement
+ available_measurements.append({'idx': data_idx, 'distance': measurement['measurment_range']})
+
+ # Select the "best"=shortest distance measurement
+ if (len(available_measurements) != 0):
+ sorted_measurements = sorted(available_measurements, key=lambda k: k['distance'])
+ min_distance_idx = sorted_measurements[0]['idx']
+ return self.measurement_data[robot_idx][min_distance_idx]
# No measurement data available (ex: no landmarks in vision at given time)
- return None
\ No newline at end of file
+ return {'time': current_time, 'subject_ID': None, 'measurment_range': None,'bearing': None}
\ No newline at end of file
--
GitLab