Skip to content
Snippets Groups Projects
Commit 3ff0ca2e authored by oceanpdoshi@g.ucla.edu's avatar oceanpdoshi@g.ucla.edu
Browse files

fixed assumptions of delta_t being 1.0 s 

Replaced hard-coded assumption with
find_nearest_time_idx()

Updated journal 2 sim parameters
Still debugging!
parent 78887950
Branches
No related merge requests found
Hide whitespace changes
Inline Side-by-side
CoLo-AT/Cooperative_Localization_Journal2_sim_data.py
+ 6
− 5
View file @ 3ff0ca2e
......@@ -8,7 +8,7 @@ Created on Thu May 16 15:41:51 2019
import os, sys
import getpass
from math import pi
from math import pi, sqrt
sys.path.append(os.path.join(os.path.dirname(__file__), "."))
from dataset_manager.realworld_dataset_manager import RW_Dataset_Manager
from dataset_manager.simulated_dataset_manager import SimulatedDataSetManager
......@@ -29,13 +29,14 @@ from ekf_gs_ci2 import EKF_GS_CI2
from gs_ci_bound import GS_CI_Bound
from ekf_gs_sci2 import EKF_GS_SCI2
landmarks = {10: [0,0] }
landmarks = {10: [0,5] , 11: [20,3], 12: [-30,0], 13: [-13, -12] }
robot_labels = [1,2,3,4,5]
boundary = 200
duration = 30
boundary = 60
duration = 180
testing_dataset = SimulatedDataSetManager('testing', boundary, landmarks, duration, robot_labels,
velocity_noise=0.0125, angular_velocity_noise=0.0, measurement_range_noise=0.1, bearing_noise=0.0349, robot_fov=2*pi)
velocity_noise=sqrt(0.0125), angular_velocity_noise=0.001, measurement_range_noise=sqrt(0.1), bearing_noise=sqrt(2)*pi/180,
robot_fov=2*pi, delta_t=0.2)
start_time, starting_states, dataset_data, time_arr = testing_dataset.simulate_dataset('random', test=False)
analyzer = Analyzer('analyzer', robot_labels)
......
CoLo-AT/dataset_manager/data_generator.py
+ 41
− 31
View file @ 3ff0ca2e
......@@ -148,7 +148,7 @@ class DataGenerator():
# rotate some random number of radians between pi/3 and -pi/3
next_orientation = curr_orientation + np.random.uniform(-pi/3, pi/3)
next_orientation = self.converge_to_angle_range(next_orientation)
rand_dist = 1 + np.random.normal(0, sqrt(0.25)) # units of distance change are in meters, if delta t is one second... v = m/s
rand_dist = (1 + np.random.normal(0, sqrt(0.25)))*self.delta_t # units of distance change are in meters, if delta t is one second... v = m/s
next_x = curr_x + rand_dist*cos(next_orientation)
next_y = curr_y + rand_dist*sin(next_orientation)
......@@ -175,14 +175,17 @@ class DataGenerator():
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)
t1 = self.groundtruth_data[i][time_idx-1]['time']
t2 = self.groundtruth_data[i][time_idx]['time']
velocity = self.calc_distance(loc1, loc2)/(t2-t1) + np.random.normal(loc=0.0,scale=self.velocity_noise)
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)
angular_velocity = (theta_2-theta_1)/(t2-t1) + np.random.normal(loc=0.0,scale=self.angular_velocity_noise)
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})
'orientation' : self.groundtruth_data[i][time_idx]['orientation'], 'delta_t' : (t2-t1)})
def generate_measurement_data(self):
......@@ -199,16 +202,16 @@ class DataGenerator():
robot_orientation = self.groundtruth_data[robot_idx][time_idx]['orientation']
robot_loc = [robot_loc_x, robot_loc_y]
# Relative (other robots) observations
# Relative (other robots) observations
other_robot_locs = {label : [self.groundtruth_data[i][time_idx]['x_pos'], self.groundtruth_data[i][time_idx]['y_pos']] for i, label in enumerate(self.robot_labels, 0) if i != robot_idx }
for robotID, other_robot_loc in other_robot_locs.items():
for robotidx, other_robot_loc in other_robot_locs.items():
if (self.within_vision(self.robot_fov, robot_loc, other_robot_loc, robot_orientation)):
measurement_range = self.calc_distance(robot_loc, other_robot_loc) + np.random.normal(loc=0.0,scale=self.measurement_range_noise)
bearing = atan2((other_robot_loc[1]-robot_loc[1]), (other_robot_loc[0]-robot_loc[0])) + np.random.normal(loc=0.0,scale=self.bearing_noise) # global coordinates
bearing = bearing - robot_orientation # NOTE - local = global - orientation, all between 0 and 2*pi
bearing = self.converge_to_angle_range(bearing)
self.measurement_data[robot_idx].append({'time': time, 'subject_ID': robotID, 'measurment_range':measurement_range, 'bearing':bearing})
self.measurement_data[robot_idx].append({'time': time, 'subject_ID': self.robot_labels[robotidx], 'measurment_range':measurement_range, 'bearing':bearing})
time_arr.append(time) # we expect repeats/skipped times in time array
# Landmark (absolute) observations
......@@ -281,7 +284,7 @@ class DataGenerator():
generated_bearing = measurement['bearing']
meas_diff = actual_meas - generated_measurement
bearing_diff = actual_bearing - generated_bearing
bearing_diff = abs(actual_bearing) - abs(generated_bearing)
# double check for edge cases where both are approximately pi (one may be (-), the other may be positive
if (abs(bearing_diff) > 1):
......@@ -301,7 +304,7 @@ class DataGenerator():
generated_bearing = measurement['bearing']
meas_diff = actual_meas - generated_measurement
bearing_diff = actual_bearing - generated_bearing
bearing_diff = abs(actual_bearing) - abs(generated_bearing)
# double check for edge cases where both are approximately pi (one may be (-), the other may be positive
if (abs(bearing_diff) > 1):
......@@ -309,15 +312,21 @@ class DataGenerator():
meas_test_arr[i].append({'meas_diff': meas_diff, 'bearing_diff': bearing_diff, 'time': curr_time})
fig, ax_arr = plt.subplots(6)
fig, ax_arr = plt.subplots(8)
plt.xlabel('t [s]')
plt.title('Odometry & Measurement Data Verification', position=(0.5,7))
ax_arr[0].set_ylabel('v_diff [m/s]')
ax_arr[1].set_ylabel('w_diff [r/s]')
ax_arr[2].set_ylabel('o_diff [r]')
ax_arr[3].set_ylabel('delta_t_diff [s]')
ax_arr[4].set_ylabel('meas_diff [m]')
ax_arr[5].set_ylabel('bearing_diff [r]')
# plt.title('Odometry & Measurement Data Verification', position=(0.5,7))
ax_arr[0].set_xlabel('v_diff [m/s]')
ax_arr[1].set_xlabel('w_diff [r/s]')
ax_arr[2].set_xlabel('o_diff [r]')
ax_arr[3].set_xlabel('delta_t_diff [s]')
ax_arr[4].set_xlabel('meas_diff [m]')
ax_arr[5].set_xlabel('bearing_diff [r]')
ax_arr[6].set_xlabel('t')
ax_arr[7].set_xlabel('t')
ax_arr[6].set_ylabel('meas_diff [m]')
ax_arr[7].set_ylabel('bearing_diff [r]')
labels = ['robot{}'.format(i) for i, lablel in enumerate(self.robot_labels,1,)]
......@@ -326,29 +335,30 @@ class DataGenerator():
w_diff_arr = [d['w_diff'] for d in diff_list]
o_diff_arr = [d['o_diff'] for d in diff_list]
delta_t_diff_arr = [d['delta_t_diff'] for d in diff_list]
time_arr = [d['time'] for d in diff_list]
# time_arr = [d['time'] for d in diff_list]
n_bins = 60
ax_arr[0].plot(time_arr, v_diff_arr)
ax_arr[1].plot(time_arr, w_diff_arr)
ax_arr[2].plot(time_arr, o_diff_arr)
ax_arr[3].plot(time_arr, delta_t_diff_arr)
ax_arr[0].hist(v_diff_arr, n_bins)
ax_arr[1].hist(w_diff_arr, n_bins)
ax_arr[2].hist(o_diff_arr, n_bins)
ax_arr[3].hist(delta_t_diff_arr, n_bins)
for diff_list in meas_test_arr:
meas_diff_arr = [d['meas_diff'] for d in diff_list]
bearing_diff_arr = [d['bearing_diff'] for d in diff_list]
meas_time_arr = [d['time'] for d in diff_list]
ax_arr[4].scatter(meas_time_arr, meas_diff_arr)
ax_arr[5].scatter(meas_time_arr, bearing_diff_arr)
ax_arr[4].hist(meas_diff_arr, n_bins)
ax_arr[5].hist(bearing_diff_arr, n_bins)
ax_arr[6].scatter(meas_time_arr, meas_diff_arr)
ax_arr[7].scatter(meas_time_arr, bearing_diff_arr)
fig.legend(labels, prop={'size': 12})
# ax_arr[0].set_ylim([-10*self.velocity_noise, 10*self.velocity_noise])
# ax_arr[1].set_ylim([-10*self.angular_velocity_noise, 10*self.angular_velocity_noise])
# ax_arr[4].set_ylim([-10*self.measurement_range_noise, 10*self.measurement_range_noise])
# ax_arr[5].set_ylim([-10*self.bearing_noise, 10*self.bearing_noise])
plt.show()
plt.close('all')
plt.subplots_adjust(left=.07, bottom=.08, right=.94, top=.89, wspace=.2, hspace=.75)
# plt.show()
# plt.close('all')
print('******Verification Complete******')
......
CoLo-AT/dataset_manager/simulated_dataset_manager.py
+ 9
− 11
View file @ 3ff0ca2e
......@@ -28,7 +28,7 @@ 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_fov = 2*pi/3):
robot_fov = 2*pi/3, delta_t=1.0):
self.name = dataset_name
self.boundary = float(boundary)
......@@ -44,7 +44,7 @@ class SimulatedDataSetManager:
self.curr_robot_idx = 0
# time step of the simulation (secs) TODO - user specified
self.delta_t = 1.0
self.delta_t = delta_t
# landmarks = {landmark ID: [x,y]} (m)
self.landmark_map = landmarks
......@@ -69,7 +69,7 @@ class SimulatedDataSetManager:
# Input: robot_path (line, circle, etc.)
# Output: Generates simulated data (gt, odo, meas)
def simulate_dataset(self, path='line', test=False):
def simulate_dataset(self, path='random', test=False):
if (path == 'line'):
self.time_arr, self.groundtruth_data, self.odometry_data, self.measurement_data, self.dataset_data = self.generator.generate_straight_line_data(test)
return self.start_time, self.starting_states, self.dataset_data, self.time_arr
......@@ -103,12 +103,9 @@ class SimulatedDataSetManager:
def get_time_arr(self, data_category):
return self.time_arr[data_category]
# TODO - issues with simulation process communication module:
# issue appears to be time index is out of boutds
def get_robot_groundtruth(self, gt_time, robot_index):
while (gt_time >= self.end_time):
gt_time -= self.delta_t
gt_index = np.where(gt_time == self.time_arr['groundtruth'][robot_index])[0][0]
gt_time_arr = [ groundtruth['time'] for groundtruth in self.groundtruth_data[robot_index] ]
gt_index = find_nearest_time_idx(gt_time_arr, gt_time)
gt = self.groundtruth_data[robot_index][gt_index]
return gt
......@@ -133,9 +130,10 @@ class SimulatedDataSetManager:
req.set_robot_idx(robot_idx)
req.set_type(req_type)
# TODO - perhaps this is the sourceo f the issue?
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
......@@ -165,10 +163,9 @@ class SimulatedDataSetManager:
message['robot_index'] = robot_idx
# Past the total time of simulation or no further measurements
valid_dataline = current_time <= self.end_time
valid_dataline = current_time < self.end_time or (abs(current_time - self.end_time) < 0.001) # <= w/floats
# Select message data
# NOTE - use difference rather than inequality because of float difficulties
if valid_dataline:
if req_type == 'odometry':
odo_time_idx = find_nearest_time_idx(self.time_arr[req_type][robot_idx], current_time)
......@@ -202,5 +199,6 @@ class SimulatedDataSetManager:
idx = measurement_indices[least_index]
return self.measurement_data[robot_idx][idx]
# No measurement data available (ex: no landmarks in vision at given time)
return {'time': current_time, 'subject_ID': None, 'measurment_range': None,'bearing': None}
\ No newline at end of file
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment