From c6daabcc8b898dc6256d59f1db981d3b31035517 Mon Sep 17 00:00:00 2001
From: "oceanpdoshi@g.ucla.edu" <oceanpdoshi@.ucla.edu>
Date: Tue, 14 May 2019 23:40:32 -0700
Subject: [PATCH] Added random data generator Currently testing communication
---
CoLo-AT/dataset_manager/data_generator.py | 54 +++++++++++++++----
.../simulated_dataset_manager.py | 17 ++++--
CoLo-AT/test_simulation_sagar.py | 32 ++++++-----
3 files changed, 77 insertions(+), 26 deletions(-)
diff --git a/CoLo-AT/dataset_manager/data_generator.py b/CoLo-AT/dataset_manager/data_generator.py
index 182bcd5..207210c 100644
--- a/CoLo-AT/dataset_manager/data_generator.py
+++ b/CoLo-AT/dataset_manager/data_generator.py
@@ -2,6 +2,7 @@ import numpy as np
from math import pi, sqrt, atan2, hypot, sin, cos
import matplotlib.pyplot as plt
+# TODO - add collision/boundary checking for circle/random data
class DataGenerator():
def __init__(self, boundary, landmarks, duration, robot_labels, starting_states, start_time, delta_t,
velocity_noise = 0, angular_velocity_noise = 0, measurement_range_noise = 0, bearing_noise = 0, communication_noise = 0,
@@ -86,8 +87,6 @@ class DataGenerator():
# Generates simulated of robots moving in a circle
def generate_circular_data(self, test):
- # TODO - add boundary, collision checking
-
# NOTE - this method of generating a circular path goes from polygons (low duration) to rounder as duration increases
path_radius = self.boundary/(self.num_robots + 3)
path_circumference = 2*pi*path_radius
@@ -112,7 +111,7 @@ class DataGenerator():
# Converts all orientations to be between pi and pi
if (next_orientation > pi or next_orientation < -pi):
- next_orientation = self.converge_to_bearing_range(next_orientation)
+ next_orientation = self.converge_to_angle_range(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})
@@ -127,6 +126,43 @@ class DataGenerator():
return self.time_arr, self.groundtruth_data, self.odometry_data, self.measurement_data, self.dataset_data
+ # Inputs: None
+ # Outputs: time_arr, groundtruth_data, self.odometry_data, measurement_data, self.dataset_data
+ # Generates simulated of robots moving randomly
+ def generate_random_data(self, test):
+
+ # Generate ground truth data
+ 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 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']
+
+ # rotate some random number of radians between pi/4 and -pi/4
+ next_orientation = curr_orientation + np.random.uniform(-pi/3, pi/3)
+ next_orientation = self.converge_to_angle_range(next_orientation)
+ rand_dist = np.random.uniform(0, 0.5) # units of distance change are in
+ next_x = curr_x + rand_dist*cos(next_orientation)
+ next_y = curr_y + rand_dist*sin(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})
+
+ self.generate_odometry_data()
+ self.generate_measurement_data()
+
+ self.dataset_data = {'odometry': self.odometry_data, 'measurement': self.measurement_data, 'groundtruth': self.groundtruth_data}
+
+ if (test):
+ self.verify_generated_data()
+
+ return self.time_arr, self.groundtruth_data, self.odometry_data, self.measurement_data, self.dataset_data
+
+ # Inputs: None
+ # Outputs: time_arr, groundtruth_data, self.odometry_data, measurement_data, self.dataset_data
+ # Generates simulated data for robots moving completely randomly (within realism)
def generate_odometry_data(self):
for i, label in enumerate(self.robot_labels, 0):
# Initial robot state
@@ -144,7 +180,7 @@ class DataGenerator():
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})
- # TODO - add relative robot measurements
+
def generate_measurement_data(self):
# Generate Measurement Data
for robot_idx, label in enumerate(self.robot_labels, 0):
@@ -167,7 +203,7 @@ class DataGenerator():
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_bearing_range(bearing)
+ bearing = self.converge_to_angle_range(bearing)
self.measurement_data[robot_idx].append({'time': time, 'subject_ID': robotID, 'measurment_range':measurement_range, 'bearing':bearing})
time_arr.append(time) # we expect repeats/skipped times in time array
@@ -178,7 +214,7 @@ class DataGenerator():
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) # global coordinates
bearing = bearing - robot_orientation # NOTE - local = global - orientation, all between 0 and 2*pi
- bearing = self.converge_to_bearing_range(bearing)
+ bearing = self.converge_to_angle_range(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
@@ -237,7 +273,7 @@ class DataGenerator():
actual_meas = self.calc_distance(curr_loc, landmark_loc)
generated_measurement = measurement['measurment_range']
- actual_bearing = self.converge_to_bearing_range(atan2(landmark_loc[1]- y2, landmark_loc[0]-x2) - actual_o) # global - orientation = local, -pi to pi
+ actual_bearing = self.converge_to_angle_range(atan2(landmark_loc[1]- y2, landmark_loc[0]-x2) - actual_o) # global - orientation = local, -pi to pi
generated_bearing = measurement['bearing']
meas_diff = actual_meas - generated_measurement
@@ -253,7 +289,7 @@ class DataGenerator():
actual_meas = self.calc_distance(curr_loc, other_robot_loc)
generated_measurement = measurement['measurment_range']
- actual_bearing = self.converge_to_bearing_range(atan2(other_robot_loc[1]- y2, other_robot_loc[0]-x2) - actual_o) # global - orientation = local, -pi to pi
+ actual_bearing = self.converge_to_angle_range(atan2(other_robot_loc[1]- y2, other_robot_loc[0]-x2) - actual_o) # global - orientation = local, -pi to pi
generated_bearing = measurement['bearing']
meas_diff = actual_meas - generated_measurement
@@ -331,7 +367,7 @@ class DataGenerator():
return abs(bearing) <= robot_fov/2
# get angle between -pi and pi
- def converge_to_bearing_range(self, theta):
+ def converge_to_angle_range(self, theta):
while (theta < -pi):
theta += 2*pi
while (theta > pi):
diff --git a/CoLo-AT/dataset_manager/simulated_dataset_manager.py b/CoLo-AT/dataset_manager/simulated_dataset_manager.py
index 18ba1ab..e51247b 100644
--- a/CoLo-AT/dataset_manager/simulated_dataset_manager.py
+++ b/CoLo-AT/dataset_manager/simulated_dataset_manager.py
@@ -1,6 +1,8 @@
from math import pi
import numpy as np
+import IPython
+
# Import from other directories in repository
import sys
import os
@@ -44,7 +46,7 @@ class SimulatedDataSetManager:
raise Exception("Invalid landmark ID: landmark IDs must be bigger than 5.")
# starting_states = {robot label: [time, x_pos, y_pos, orientation], ... }
- # TODO - create a generate starting states function
+ # TODO - create a generate starting states function (potentially randomized to fit boundary)
self.starting_states = {label : [self.start_time, -boundary + float(i+1)*boundary/self.num_robots, 0, 0.0] for i,label in enumerate(self.robot_labels)}
# groundtruth & odometry are of fixed length=duration
@@ -64,10 +66,13 @@ class SimulatedDataSetManager:
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
- # TODO
elif(path=='circle'):
self.time_arr, self.groundtruth_data, self.odometry_data, self.measurement_data, self.dataset_data = self.generator.generate_circular_data(test)
return self.start_time, self.starting_states, self.dataset_data, self.time_arr
+ elif(path == 'random'):
+ self.time_arr, self.groundtruth_data, self.odometry_data, self.measurement_data, self.dataset_data = self.generator.generate_random_data(test)
+ self.measurement_data[0] = [] # TODO - to test simulated communication, trace of state variance should increase unless a communication update
+ return self.start_time, self.starting_states, self.dataset_data, self.time_arr
else:
raise Exception("Unsupported robot path specified.")
@@ -92,10 +97,16 @@ 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):
- gt = self.groundtruth_data[robot_index][gt_time]
+ 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 = self.groundtruth_data[robot_index][gt_index]
return gt
+ # Used to cycle through all robots in native simulation
def reset_robot_ctr(self):
if (self.curr_robot_idx > self.num_robots-1):
self.curr_robot_idx = 0
diff --git a/CoLo-AT/test_simulation_sagar.py b/CoLo-AT/test_simulation_sagar.py
index 4304428..54e066c 100644
--- a/CoLo-AT/test_simulation_sagar.py
+++ b/CoLo-AT/test_simulation_sagar.py
@@ -26,6 +26,9 @@ from data_analysis.realtime_plot import animate_plot
from pprint import pprint
# load algorithms
+# NOTE
+# GS = global state -> distributed system
+# LS = localized state -> centralized system.
sys.path.append(os.path.join(os.path.dirname(__file__), "localization_algos"))
from centralized_ekf import Centralized_EKF # works
from simple_ekf import Simple_EKF
@@ -38,30 +41,31 @@ compname = getpass.getuser()
#dataset_path = "/home/"+ compname +"/CoLo/CoLo-D/UTIAS-Datasets/MRCLAM_Dataset3/"
# landmark IDs must be bigger than 5
-landmarks = {10: [0,5], 11: [3,3], 12: [-3,0], 13: [-3, -2] }
+landmarks = {10: [0,5] , 11: [3,3], 12: [-3,0], 13: [-3, -2] }
-robot_labels = [1]
+robot_labels = [1,2]
boundary = 5 # radius of circular map centered at 0,0
-duration = 150 # (seconds)
-
-# testing_dataset = SimulatedDataSetManager('testing', boundary, landmarks, duration, robot_labels,
-# velocity_noise=0.01, angular_velocity_noise=0.01, measurement_range_noise=0.01, bearing_noise=0.01, robot_fov=2*pi)
-# start_time, starting_states, dataset_data, time_arr = testing_dataset.simulate_dataset('circle', test=True)
+duration = 45 # (seconds)
+# NOTE - adjust noise if need to see drastic convergence/divergence
+testing_dataset = SimulatedDataSetManager('testing', boundary, landmarks, duration, robot_labels,
+velocity_noise=0.01, angular_velocity_noise=0.01, measurement_range_noise=0.01, bearing_noise=0.01, robot_fov=2*pi)
+start_time, starting_states, dataset_data, time_arr = testing_dataset.simulate_dataset('random', test=False)
# Real-World DataSet
-dataset_path = "D:/LEMUR/CoLo/CoLo-D/CoLo-Datasets/official_dataset1/"
-testing_dataset = RW_Dataset_Manager('testing')
-start_time, starting_states, dataset_data, time_arr = testing_dataset.load_datasets(dataset_path, robot_labels, duration,delay_start=20)
+# dataset_path = "D:/LEMUR/CoLo/CoLo-D/CoLo-Datasets/official_dataset1/"
+# testing_dataset = RW_Dataset_Manager('testing')
+# start_time, starting_states, dataset_data, time_arr = testing_dataset.load_datasets(dataset_path, robot_labels, duration,delay_start=20)
analyzer = Analyzer('analyzer', robot_labels)
-loc_algo = Simple_EKF('Simple_EKF')
-robot = RobotSystem('robot', robot_labels, loc_algo, distr_sys = False)
+loc_algo = EKF_GS_BOUND('EKF_GS_BOUND')
+robot = RobotSystem('robot', robot_labels, loc_algo, distr_sys = True)
sim = SimulationManager('sim')
-state_recorder = StatesRecorder('Simple_EKF', robot_labels)
+state_recorder = StatesRecorder('EKF_GS_BOUND', robot_labels)
-sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder, simple_plot = False, comm=False, simulated_comm = False)
+# TODO - 3 robots, but one CANNOT take measurements and must rely on communication, see if convergent.
+sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder, simple_plot = False, comm=True, simulated_comm = True)
loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder, plot_graphs = False)
--
GitLab