From 88eaa391bf076e603a45c6ab728e937bd11640da Mon Sep 17 00:00:00 2001
From: "oceanpdoshi@g.ucla.edu" <oceanpdoshi@.ucla.edu>
Date: Tue, 23 Apr 2019 13:37:33 -0700
Subject: [PATCH] -fixed robot_vision, -working on incorporating multiple
landmarks -working on incorporating multiple robots -working on handling of
end of simulation vs. no meas. data available
---
.../simulated_dataset_manager.py | 168 ++++++++++--------
1 file changed, 97 insertions(+), 71 deletions(-)
diff --git a/CoLo-AT/dataset_manager/simulated_dataset_manager.py b/CoLo-AT/dataset_manager/simulated_dataset_manager.py
index 2651dc5..3155991 100644
--- a/CoLo-AT/dataset_manager/simulated_dataset_manager.py
+++ b/CoLo-AT/dataset_manager/simulated_dataset_manager.py
@@ -18,25 +18,33 @@ from requests.request_response import Request_response
class SimulatedDataSetManager:
def __init__(self, dataset_name, boundary, landmarks, duration, robot_labels,
- velocity_noise = 0, angular_velocity_noise = 0, measurement_range_noise = 0, bearing_noise = 0, communication_noise = 0, robot_vision = pi/3):
+ velocity_noise = 0, angular_velocity_noise = 0, measurement_range_noise = 0, bearing_noise = 0, communication_noise = 0,
+ robot_fov = 2*pi/3):
- self.name = dataset_name
- # Alternates between 'odometry' and 'measurement'
- self.next_request = 'odometry'
-
+ self.name = dataset_name
self.duration = duration
self.boundary = boundary
self.robot_labels = robot_labels
self.num_robots = len(robot_labels)
+ self.start_time = 20.0
+ self.end_time = self.start_time + self.duration
+ self.start_moving_times = [self.start_time for label in robot_labels]
+
+ # angular width of robots's vision with respect to robot's x_axis (radians)
+ # robot_fov = angular width (radians) of robot's view in reference to x_axis
+ # ex: robot_fov = 2pi/3 means 60 degrees above and below x-axis are within vision
+ self.robot_fov = robot_fov
- # Internal time "counter"
+ # Alternates between 'odometry' and 'measurement'
+ self.next_request = 'odometry'
+ # cycles through robot_labels
+ self.current_robot_idx = 0
+
+ # Internal time index/counter
self.time_idx = 0
self.delta_t = 1.0
- # angular width of robots's vision with respect to the x_axis (radians)
- self.robot_vision = robot_vision
-
# User-specified noises, if none given: defaulted to 0
self.velocity_noise = velocity_noise
self.angular_velocity_noise = angular_velocity_noise
@@ -44,22 +52,19 @@ class SimulatedDataSetManager:
self.bearing_noise = bearing_noise
self.communication_noise = communication_noise
- # All robots begin moving at t = 0s
- self.start_time = 20.0
- self.end_time = self.start_time + self.duration
- self.start_moving_times = [0 for label in robot_labels]
-
# landmarks = {landmark ID: [x,y]}
- # NOTE - landmark IDs must be bigger than 5.
self.landmark_map = landmarks
+ for landmark_id in landmarks:
+ if landmark_id < 5:
+ raise Exception("Invalid landmark ID: landmark IDs must be bigger than 5.")
# starting_states = {robot label: [time, x_pos, y_pos, orientation], ... }
- # TODO - Update this to be user-specified on how to initialize robot locations. (esecially in more advanced movement patterns)
- self.starting_states = {label : [self.start_time, float(i), 1.0, 0.0] for i,label in enumerate(robot_labels)}
-
- # in the simulation all the times for all the measurements are the same.
- self.time_arr = {'odometry': [[] for robot in robot_labels], 'measurement': [[] for robot in robot_labels], 'groundtruth': [[] for robot in robot_labels]}
+ # TODO - modify generation of starting tates
+ self.starting_states = {label : [self.start_time, float(i), float(i), 0.0] for i,label in enumerate(robot_labels)}
+ # NOTE - length of odometry and groundtruth is fixed to be duration
+ # measurment can be greater or equal length depending on number of measurments made
+ self.time_arr = {'odometry': [[] for robot in robot_labels], 'measurement': [[] for robot in robot_labels], 'groundtruth': [[] for robot in robot_labels]}
for data_type in self.time_arr:
for i in range(self.num_robots):
self.time_arr[data_type][i] = np.arange(self.start_time, self.end_time)
@@ -77,7 +82,7 @@ class SimulatedDataSetManager:
'bearing': 0.29890824572449504
}
'''
- self.measurement_data = [[] for i in range(self.num_robots)]
+ self.measurement_data = [[] for robot in range(self.num_robots)]
# Odometry Data Format
'''
@@ -89,9 +94,9 @@ class SimulatedDataSetManager:
'delta_t': 0.09999990463256836
}
'''
- self.odometry_data = [[] for i in range(self.num_robots)]
+ self.odometry_data = [[] for robot in range(self.num_robots)]
- # ground_truth formatting
+ # Groundtruth format
# ORIENTATION DEFINED WITH RESPECT TO GLOBAL FRAME OF REFERENCE
'''
{
@@ -101,18 +106,28 @@ class SimulatedDataSetManager:
'orientation': 0.03251611228318508
}
'''
- self.groundtruth_data = [ [] for i in range(self.num_robots)]
+ self.groundtruth_data = [ [] for robot in range(self.num_robots)]
+
+ def simulate_dataset(self, path='line'):
+ if (path == 'line'):
+ return self.generate_straight_line_data()
+ # TODO
+ elif(path=='circle'):
+ pass
+ else:
+ raise Exception("Unsupported robot path specified.")
- def simulate_dataset(self):
+ # TODO - create a separate class file that does data generation
+ def generate_straight_line_data(self):
+
# Generate ground truth data
- # NOTE - FOR NOW THIS WILL JUST MOVE ROBOTS IN A STRAIGHT LINE UNTIL THEY REACH THE BOUNDARY - THEN THEY WILL BE STATIONARY
for i, label in enumerate(self.robot_labels, 0):
# append starting state
self.groundtruth_data[i].append({'time' : self.starting_states[label][0], 'x_pos': self.starting_states[label][1], 'y_pos': self.starting_states[label][2], 'orientation' : self.starting_states[label][3]})
- for j in range(1, len(self.time_arr['groundtruth'][i])):
- curr_x = self.groundtruth_data[i][j-1]['x_pos']
- curr_y = self.groundtruth_data[i][j-1]['y_pos']
- curr_orientation = self.groundtruth_data[i][j-1]['orientation']
+ for time_idx in range(1, len(self.time_arr['groundtruth'][i])):
+ curr_x = self.groundtruth_data[i][time_idx-1]['x_pos']
+ curr_y = self.groundtruth_data[i][time_idx-1]['y_pos']
+ curr_orientation = self.groundtruth_data[i][time_idx-1]['orientation']
next_x = curr_x
next_y = curr_y
@@ -122,38 +137,45 @@ class SimulatedDataSetManager:
next_x = curr_x + 1
next_y = curr_y
- self.groundtruth_data[i].append({'time' : self.time_arr['groundtruth'][i][j], 'x_pos': next_x, 'y_pos': next_y, 'orientation': next_orientation})
+ self.groundtruth_data[i].append({'time' : self.time_arr['groundtruth'][i][time_idx], 'x_pos': next_x, 'y_pos': next_y, 'orientation': next_orientation})
- # NOTE - modify incorporation of noise
# Generate Odometery Data
for i, label in enumerate(self.robot_labels, 0):
# Initial robot state
self.odometry_data[i].append({'time': self.start_time, 'velocity' : 0, 'angular velocity' : 0, 'orientation' : self.groundtruth_data[i][0]['orientation'],
'delta_t' : 0})
- for j in range(1, len(self.time_arr['odometry'][i])):
- # NOTE - delta_t is currently hard coded to 1
- loc1 = [self.groundtruth_data[i][j-1]['x_pos'], self.groundtruth_data[i][j-1]['y_pos']]
- loc2 = [self.groundtruth_data[i][j]['x_pos'], self.groundtruth_data[i][j]['y_pos']]
+ for time_idx in range(1, len(self.time_arr['odometry'][i])):
+ loc1 = [self.groundtruth_data[i][time_idx-1]['x_pos'], self.groundtruth_data[i][time_idx-1]['y_pos']]
+ loc2 = [self.groundtruth_data[i][time_idx]['x_pos'], self.groundtruth_data[i][time_idx]['y_pos']]
velocity = self.calc_distance(loc1, loc2)/self.delta_t + np.random.normal(loc=0.0,scale=self.velocity_noise)
- theta_1 = self.groundtruth_data[i][j-1]['orientation']
- theta_2 = self.groundtruth_data[i][j]['orientation']
+
+ theta_1 = self.groundtruth_data[i][time_idx-1]['orientation']
+ theta_2 = self.groundtruth_data[i][time_idx]['orientation']
angular_velocity = (theta_2-theta_1)/self.delta_t + np.random.normal(loc=0.0,scale=self.angular_velocity_noise)
- self.odometry_data[i].append({'time': self.time_arr['odometry'][i][j], 'velocity' : velocity, 'angular velocity' : angular_velocity,
- 'orientation' : self.groundtruth_data[i][j]['orientation'], 'delta_t' : self.delta_t})
+ self.odometry_data[i].append({'time': self.time_arr['odometry'][i][time_idx], 'velocity' : velocity, 'angular velocity' : angular_velocity,
+ 'orientation' : self.groundtruth_data[i][time_idx]['orientation'], 'delta_t' : self.delta_t})
# Generate Measurement Data
- for i, label in enumerate(self.robot_labels, 0):
- for j in range(0, len(self.time_arr['measurement'][i])):
- for landmarkID, landmark_loc in self.landmark_map.items():
- robot_loc_x = self.groundtruth_data[i][j]['x_pos']
- robot_loc_y = self.groundtruth_data[i][j]['y_pos']
+ for robot_idx, label in enumerate(self.robot_labels, 0):
+ # NOTE - time_arr is used to emplace a one-to-one index-to-time correspondence
+ # between measurement_data and time_arr['measurement'] to account for varing number of landmark observations
+ time_arr = []
+ # iterate over all possible times and (potentially) observable landmarks
+ for time_idx in range(0, len(self.time_arr['measurement'][i])):
+ for landmarkID, landmark_loc in self.landmark_map.items():
+
+ time = self.time_arr['measurement'][robot_idx][time_idx]
+ robot_loc_x = self.groundtruth_data[robot_idx][time_idx]['x_pos']
+ robot_loc_y = self.groundtruth_data[robot_idx][time_idx]['y_pos']
robot_loc = [robot_loc_x, robot_loc_y]
- if (True):
+
+ if (self.within_vision(self.robot_fov, robot_loc, landmark_loc)):
measurement_range = self.calc_distance(robot_loc, landmark_loc) + np.random.normal(loc=0.0,scale=self.measurement_range_noise)
bearing = atan2((landmark_loc[1]-robot_loc[1]), (landmark_loc[0]-robot_loc[0])) + np.random.normal(loc=0.0,scale=self.bearing_noise)
- self.measurement_data[i].append({'time': self.time_arr['measurement'][i][j], 'subject_ID': landmarkID, 'measurment_range':measurement_range, 'bearing':bearing})
-
+ self.measurement_data[robot_idx].append({'time': time, 'subject_ID': landmarkID, 'measurment_range':measurement_range, 'bearing':bearing})
+ time_arr.append(time) # we expect repeats/skipped times in time array
+ self.time_arr['measurement'][robot_idx] = time_arr
self.dataset_data = {'odometry': self.odometry_data, 'measurement': self.measurement_data, 'groundtruth': self.groundtruth_data}
return self.start_time, self.starting_states, self.dataset_data, self.time_arr
@@ -184,9 +206,7 @@ class SimulatedDataSetManager:
def respond(self, req, current_time, need_specific_time = False):
- valid_respond, message, req_type, robot_idx, time_idx = self.get_dataline(req, current_time)
-
- # current_time == self.time_arr[req_type][robot_idx][time_idx]
+ valid_respond, message, req_type, robot_idx = self.get_dataline(req, current_time)
if (valid_respond):
req.set_time(current_time)
@@ -194,15 +214,18 @@ class SimulatedDataSetManager:
req.set_robot_idx(robot_idx)
req.set_type(req_type)
+ # TODO - add in update parameters for multiple robots (and robot_ctr about to reset)
if (req_type == 'measurement'):
current_time += self.delta_t
return valid_respond, current_time, req
- # Alternates odometry and measurement updates
+ # Alternates odometry and measurement updates
+ # 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:
if req.get_type() == None:
# Alternate between measurement and odometry requests
@@ -221,22 +244,22 @@ class SimulatedDataSetManager:
message['robot_index'] = robot_idx
- time_idx = np.where(self.time_arr[req_type][robot_idx] == current_time)[0][0]
- valid_dataline= True
-
- # TODO - handling of case where no data is available
- if req_type == 'measurement' and (time_idx >= len (self.dataset_data[req_type][robot_idx])):
- valid_dataline = False
+ # Selection of time_idx
+
+ # 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])
if valid_dataline:
if req_type == 'odometry':
- message['data'] = self.odometry_data[robot_idx][time_idx]
+ 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
elif req_type == 'measurement':
- message['data'] = self.find_measurement(current_time, robot_idx) # there may exist a built-in function to do a linear search through a list of dictionaries
-
+ message['data'] = self.find_measurement(current_time, robot_idx)
- message['groundtruth'] = self.groundtruth_data[robot_idx][time_idx]
- return valid_dataline, message, req_type, robot_idx, time_idx
+ 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
def within_map(self, loc):
@@ -258,18 +281,21 @@ class SimulatedDataSetManager:
distance = sqrt((x1-x2)**2 + (y1-y2)**2)
return distance
- # TODO - Check this function
- # robot_vision = angular width of robot's view in reference to x_axis
- # robot_loc & landmark_loc = [x,y]
- def within_vision(self, robot_vision, robot_loc, landmark_loc):
-
- bearing = atan2((landmark_loc[1]-robot_loc[1]),(landmark_loc[0]-robot_loc[0])) + np.random.normal(loc=0.0,scale=0.1)
- return abs(bearing) < robot_vision/2
+ # TODO - add distance limitations to robot_vision
+ # [x,y] for landmark and robot_loc
+ def within_vision(self, robot_fov, robot_loc, landmark_loc):
+
+ # between pi and -pi with respect to the x axis
+ bearing = atan2((landmark_loc[1]-robot_loc[1]),(landmark_loc[0]-robot_loc[0]))
+
+ 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)
def find_measurement(self, current_time, robot_idx):
for measurement in self.measurement_data[robot_idx]:
if (measurement['time'] == current_time):
return measurement
- # TODO - handling of case where no measurmeent data available (ex: no landmarks in vision at given time)
+ # No measurement data available (ex: no landmarks in vision at given time)
return None
\ No newline at end of file
--
GitLab