diff --git a/CoLo-AT/.DS_Store b/CoLo-AT/.DS_Store
new file mode 100644
index 0000000000000000000000000000000000000000..0a170c81bddf9aed977bbe5e7f11711c1cb66f95
Binary files /dev/null and b/CoLo-AT/.DS_Store differ
diff --git a/CoLo-AT/README.md b/CoLo-AT/README.md
index 6b2426f087b18dd7bf00733ec2fbddbaac0bad05..22561165af0071a3b815cc345a8451e33e9521b0 100755
--- a/CoLo-AT/README.md
+++ b/CoLo-AT/README.md
@@ -32,9 +32,13 @@ git clone git@git.uclalemur.com:billyskc/Simulation-Environment-for-Cooperative-
1. Install all dependencies
2. Create a python script for the environment(ex: test_simulation.py)
+
a. Create a dataset_manager with proper setting
+
b. Load a localization algorithm in to robots
+
c. Create simulation manager, recorder and analyzer in CoLo
+
d. Put all these part in simulation manager
3. Run CoLo
diff --git a/CoLo-AT/data_analysis/__pycache__/__init__.cpython-36.pyc b/CoLo-AT/data_analysis/__pycache__/__init__.cpython-36.pyc
index c63692804c06816026ad58cdf8da4db3ee6706e3..a6c36afd728c74bcb0dfce9bb6db5c63562fdade 100644
Binary files a/CoLo-AT/data_analysis/__pycache__/__init__.cpython-36.pyc and b/CoLo-AT/data_analysis/__pycache__/__init__.cpython-36.pyc differ
diff --git a/CoLo-AT/data_analysis/__pycache__/data_analyzer.cpython-36.pyc b/CoLo-AT/data_analysis/__pycache__/data_analyzer.cpython-36.pyc
index c17ec2640ce6bff4e56915b4fe2430c236bdde2a..9b93370356b5023e51bb249b484f896ee3e248c1 100644
Binary files a/CoLo-AT/data_analysis/__pycache__/data_analyzer.cpython-36.pyc and b/CoLo-AT/data_analysis/__pycache__/data_analyzer.cpython-36.pyc differ
diff --git a/CoLo-AT/data_analysis/__pycache__/realtime_plot.cpython-36.pyc b/CoLo-AT/data_analysis/__pycache__/realtime_plot.cpython-36.pyc
index edb0d1c6d571e3816fea6931c4a2caf88f608c60..6b1a42fb26002c6c84ef44c593f5cfb6ffc15f86 100644
Binary files a/CoLo-AT/data_analysis/__pycache__/realtime_plot.cpython-36.pyc and b/CoLo-AT/data_analysis/__pycache__/realtime_plot.cpython-36.pyc differ
diff --git a/CoLo-AT/dataset_manager/__pycache__/__init__.cpython-36.pyc b/CoLo-AT/dataset_manager/__pycache__/__init__.cpython-36.pyc
index fd31344ff4309517f4f33b309e86875ec7027076..d15e459c547ee60f84d0fd9af17f5941c3593eb0 100644
Binary files a/CoLo-AT/dataset_manager/__pycache__/__init__.cpython-36.pyc and b/CoLo-AT/dataset_manager/__pycache__/__init__.cpython-36.pyc differ
diff --git a/CoLo-AT/dataset_manager/__pycache__/existing_dataset.cpython-36.pyc b/CoLo-AT/dataset_manager/__pycache__/existing_dataset.cpython-36.pyc
index 67e140af40210d6d0afaeb1ef2c0b4393be92044..c6504d54e2b103cc1de83064138ded363a911da3 100644
Binary files a/CoLo-AT/dataset_manager/__pycache__/existing_dataset.cpython-36.pyc and b/CoLo-AT/dataset_manager/__pycache__/existing_dataset.cpython-36.pyc differ
diff --git a/CoLo-AT/dataset_manager/__pycache__/realworld_dataset_manager.cpython-36.pyc b/CoLo-AT/dataset_manager/__pycache__/realworld_dataset_manager.cpython-36.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..018cc86e57a4288f0eae01ff5857953b2fb5c0b1
Binary files /dev/null and b/CoLo-AT/dataset_manager/__pycache__/realworld_dataset_manager.cpython-36.pyc differ
diff --git a/CoLo-AT/dataset_manager/existing_dataset.py b/CoLo-AT/dataset_manager/realworld_dataset_manager.py
similarity index 96%
rename from CoLo-AT/dataset_manager/existing_dataset.py
rename to CoLo-AT/dataset_manager/realworld_dataset_manager.py
index 966391e0bba272748ed330c4dbca0e4aaf25ba56..ea3221235f053b7913a3a1d73859f0191cc4976e 100755
--- a/CoLo-AT/dataset_manager/existing_dataset.py
+++ b/CoLo-AT/dataset_manager/realworld_dataset_manager.py
@@ -49,7 +49,7 @@ def linear_interpolation(end0, end1, x):
y = y0+(x-x0)*(y1-y0)/(x1-x0)
return y
-class Dataset:
+class RW_Dataset_Manager:
def __init__(self, dataset_name):
self.name = dataset_name
@@ -70,17 +70,17 @@ class Dataset:
return self.landmark_map
- def load_MRCLAMDatasets(self, dataset_path, dataset_labels, duration, adding_actifical_dataline = True, delay_start = 5):
+ def load_datasets(self, dataset_path, robot_lables, duration, adding_actifical_dataline = True, delay_start = 5):
print ('******** Initialization Started ********')
print ('add synthetic data: ', adding_actifical_dataline)
self.dataset_path = dataset_path
print("Absolute datapath: ")
print(self.dataset_path)
- self.dataset_labels = dataset_labels
+ self.robot_lables = robot_lables
self.adding_actifical_dataline = adding_actifical_dataline
self.create_landmark_map()
- self.num_robots = len(self.dataset_labels)
+ self.num_robots = len(self.robot_lables)
self.measurement_data = [[] for i in range(self.num_robots)]
self.odometry_data = [[] for i in range(self.num_robots)]
self.gt_data_odo = [[] for i in range(self.num_robots)]
@@ -94,7 +94,7 @@ class Dataset:
self.duration = duration # some more time to avoid index error
self.start_time_arr = []
#finding the starting time:
- for i, label in enumerate(self.dataset_labels):
+ for i, label in enumerate(self.robot_lables):
robot_num = str(label)
groundtruth_path = self.dataset_path+"Robot"+robot_num+"_Groundtruth.dat"
with open(groundtruth_path,'r+') as groundtruth_file:
@@ -108,7 +108,7 @@ class Dataset:
print('Staring time: ', self.start_time)
#finding starting states:
self.starting_states = {}
- for i, label in enumerate(self.dataset_labels):
+ for i, label in enumerate(self.robot_lables):
robot_num = str(label)
groundtruth_path = self.dataset_path+"Robot"+robot_num+"_Groundtruth.dat"
with open(groundtruth_path,'r+') as groundtruth_file:
@@ -124,7 +124,7 @@ class Dataset:
print(self.starting_states)
- for i, label in enumerate(self.dataset_labels):
+ for i, label in enumerate(self.robot_lables):
robot_num = str(label)
groundtruth_path = self.dataset_path+"Robot"+robot_num+"_Groundtruth.dat"
@@ -219,14 +219,14 @@ class Dataset:
def get_start_moving_times(self):
start_moving_times = []
time_idx = 0
- for i, label in enumerate(self.dataset_labels):
+ for i, label in enumerate(self.robot_lables):
start_moving_times.append(self.dataset_data['odometry'][i][time_idx]['time'])
return start_moving_times
def get_time_arr(self, data_catagory):
# retunr an array of time shows time for next dataline for each robot given data catagory
time_arr =[]
- for i, label in enumerate(self.dataset_labels):
+ for i, label in enumerate(self.robot_lables):
time_idx = self.data_trackers[data_catagory][i]
if time_idx == -1:
time_arr.append(self.start_time + self.duration + 10) # so that it will be out of range and not be selected
@@ -370,11 +370,11 @@ class Dataset:
if subject_ID > 5: # landmark
[lx, ly] = self.landmark_map[subject_ID]
else:
- if subject_ID not in self.dataset_labels:
- obj_index = (robot_idx+2)%(len(self.dataset_labels))
- subject_ID = self.dataset_labels[obj_index]
+ if subject_ID not in self.robot_lables:
+ obj_index = (robot_idx+2)%(len(self.robot_lables))
+ subject_ID = self.robot_lables[obj_index]
- obj_index = self.dataset_labels.index(subject_ID)
+ obj_index = self.robot_lables.index(subject_ID)
matched_gt_data = self.find_corresponding_ground_truth(obj_index, req_time)
lx = matched_gt_data['x_pos']
ly = matched_gt_data['y_pos']
diff --git a/CoLo-AT/localization_algos/.DS_Store b/CoLo-AT/localization_algos/.DS_Store
index 16a5a9ef2686ad5e79514819d39d7bf82f5ec619..227dcc92c9defae2fa37b52a15ca547ccacefdf7 100755
Binary files a/CoLo-AT/localization_algos/.DS_Store and b/CoLo-AT/localization_algos/.DS_Store differ
diff --git a/CoLo-AT/localization_algos/__pycache__/centralized_ekf.cpython-36.pyc b/CoLo-AT/localization_algos/__pycache__/centralized_ekf.cpython-36.pyc
index d947265cbc05ed085fa91c83a8c81e78989f7e8b..841baa9ca29af8beb5f008ea2df788530bc20823 100644
Binary files a/CoLo-AT/localization_algos/__pycache__/centralized_ekf.cpython-36.pyc and b/CoLo-AT/localization_algos/__pycache__/centralized_ekf.cpython-36.pyc differ
diff --git a/CoLo-AT/localization_algos/__pycache__/ekf_gs_bound.cpython-36.pyc b/CoLo-AT/localization_algos/__pycache__/ekf_gs_bound.cpython-36.pyc
index b2368b8f11cbbd40638748256f016fb4fe3099cf..900b3fb0362caaa3a65be460e2408d179bcbc86d 100644
Binary files a/CoLo-AT/localization_algos/__pycache__/ekf_gs_bound.cpython-36.pyc and b/CoLo-AT/localization_algos/__pycache__/ekf_gs_bound.cpython-36.pyc differ
diff --git a/CoLo-AT/localization_algos/__pycache__/ekf_gs_ci2.cpython-36.pyc b/CoLo-AT/localization_algos/__pycache__/ekf_gs_ci2.cpython-36.pyc
index 8df35967a6061613cd38380d1e83eb03ec81166c..8fcbaef524ac506c9b9676683c55297759c8b32a 100644
Binary files a/CoLo-AT/localization_algos/__pycache__/ekf_gs_ci2.cpython-36.pyc and b/CoLo-AT/localization_algos/__pycache__/ekf_gs_ci2.cpython-36.pyc differ
diff --git a/CoLo-AT/localization_algos/__pycache__/ekf_ls_bda.cpython-36.pyc b/CoLo-AT/localization_algos/__pycache__/ekf_ls_bda.cpython-36.pyc
index 7ca85c6006b5eaf6bedd40448862c238c2a09aaf..dfff1580319cd69bccc411af302e0a2a4aeb2dd0 100644
Binary files a/CoLo-AT/localization_algos/__pycache__/ekf_ls_bda.cpython-36.pyc and b/CoLo-AT/localization_algos/__pycache__/ekf_ls_bda.cpython-36.pyc differ
diff --git a/CoLo-AT/localization_algos/__pycache__/ekf_ls_ci.cpython-36.pyc b/CoLo-AT/localization_algos/__pycache__/ekf_ls_ci.cpython-36.pyc
index c46d670d002febc994b991f5e161624431c22975..7bbf05027bcebec06b2ffaf2f49d45c423207ea6 100644
Binary files a/CoLo-AT/localization_algos/__pycache__/ekf_ls_ci.cpython-36.pyc and b/CoLo-AT/localization_algos/__pycache__/ekf_ls_ci.cpython-36.pyc differ
diff --git a/CoLo-AT/localization_algos/__pycache__/localization_algo_framework.cpython-36.pyc b/CoLo-AT/localization_algos/__pycache__/localization_algo_framework.cpython-36.pyc
index ed8bf49bd81b702d10a4fd624404d3f372042198..eec902bc7e08e4b527973ba0b2a34ecf567d64a3 100644
Binary files a/CoLo-AT/localization_algos/__pycache__/localization_algo_framework.cpython-36.pyc and b/CoLo-AT/localization_algos/__pycache__/localization_algo_framework.cpython-36.pyc differ
diff --git a/CoLo-AT/localization_algos/__pycache__/simple_ekf.cpython-36.pyc b/CoLo-AT/localization_algos/__pycache__/simple_ekf.cpython-36.pyc
index 796da56e40283212099772fe8c64b1eab28ffb56..6b17fa54ed1708f61dd91c9565055c0c6ca63c18 100644
Binary files a/CoLo-AT/localization_algos/__pycache__/simple_ekf.cpython-36.pyc and b/CoLo-AT/localization_algos/__pycache__/simple_ekf.cpython-36.pyc differ
diff --git a/CoLo-AT/requests/__pycache__/request_response.cpython-36.pyc b/CoLo-AT/requests/__pycache__/request_response.cpython-36.pyc
index f7d7ac821b02e807490c880a52d3f6e990057904..02de2f22087bd952cd1f47b84adff5254474fbc2 100644
Binary files a/CoLo-AT/requests/__pycache__/request_response.cpython-36.pyc and b/CoLo-AT/requests/__pycache__/request_response.cpython-36.pyc differ
diff --git a/CoLo-AT/robots/__pycache__/__init__.cpython-36.pyc b/CoLo-AT/robots/__pycache__/__init__.cpython-36.pyc
index 968f4ea9f0609e66583b9f1a61b651c8274854be..9fbfa1b359d1539e86fc883a14c1eca9b609c242 100644
Binary files a/CoLo-AT/robots/__pycache__/__init__.cpython-36.pyc and b/CoLo-AT/robots/__pycache__/__init__.cpython-36.pyc differ
diff --git a/CoLo-AT/robots/__pycache__/robot_centralized.cpython-36.pyc b/CoLo-AT/robots/__pycache__/robot_centralized.cpython-36.pyc
index 50568596f2a6182b86836bd1966d5bab0f43e051..f398e784201849b2ada4d3786ab8e9f91f15f080 100644
Binary files a/CoLo-AT/robots/__pycache__/robot_centralized.cpython-36.pyc and b/CoLo-AT/robots/__pycache__/robot_centralized.cpython-36.pyc differ
diff --git a/CoLo-AT/robots/__pycache__/robot_distributive.cpython-36.pyc b/CoLo-AT/robots/__pycache__/robot_distributive.cpython-36.pyc
index 47a8be69a6c7981177a8129559bbf71301d46d2e..da90b600c29a7f15a8efa73b8285c7ec01ba6880 100644
Binary files a/CoLo-AT/robots/__pycache__/robot_distributive.cpython-36.pyc and b/CoLo-AT/robots/__pycache__/robot_distributive.cpython-36.pyc differ
diff --git a/CoLo-AT/robots/__pycache__/robot_parameters.cpython-36.pyc b/CoLo-AT/robots/__pycache__/robot_parameters.cpython-36.pyc
index b45792f8a1629a9f208525a179c2b464f765faf0..5c0a10a0b85351858e365c063ea53d687f40cbba 100644
Binary files a/CoLo-AT/robots/__pycache__/robot_parameters.cpython-36.pyc and b/CoLo-AT/robots/__pycache__/robot_parameters.cpython-36.pyc differ
diff --git a/CoLo-AT/robots/__pycache__/robot_system.cpython-36.pyc b/CoLo-AT/robots/__pycache__/robot_system.cpython-36.pyc
index 51373f8b840f6867913e1a6dffbd7dfe00f91edd..fdf4fa1b6761803dabb7c8be296bda7f1920a661 100644
Binary files a/CoLo-AT/robots/__pycache__/robot_system.cpython-36.pyc and b/CoLo-AT/robots/__pycache__/robot_system.cpython-36.pyc differ
diff --git a/CoLo-AT/simulation_process/__pycache__/__init__.cpython-36.pyc b/CoLo-AT/simulation_process/__pycache__/__init__.cpython-36.pyc
index 9d0372df15aae7b98875fd6917b810441fb19ca4..9911a68bb6d5909124222a8960179c2e0a77f33f 100644
Binary files a/CoLo-AT/simulation_process/__pycache__/__init__.cpython-36.pyc and b/CoLo-AT/simulation_process/__pycache__/__init__.cpython-36.pyc differ
diff --git a/CoLo-AT/simulation_process/__pycache__/sim_manager.cpython-36.pyc b/CoLo-AT/simulation_process/__pycache__/sim_manager.cpython-36.pyc
index b567d4d86a5ddcbd5ab83757cc5b2d448d6a3b98..d643fa9309460c81f3c936277613a0a4947439ee 100644
Binary files a/CoLo-AT/simulation_process/__pycache__/sim_manager.cpython-36.pyc and b/CoLo-AT/simulation_process/__pycache__/sim_manager.cpython-36.pyc differ
diff --git a/CoLo-AT/simulation_process/__pycache__/state_recorder.cpython-36.pyc b/CoLo-AT/simulation_process/__pycache__/state_recorder.cpython-36.pyc
index b4fff0660725a669b0823d3b42ca96f078fc881a..18718dff2454bc9ce43b6931a97894a27132023f 100644
Binary files a/CoLo-AT/simulation_process/__pycache__/state_recorder.cpython-36.pyc and b/CoLo-AT/simulation_process/__pycache__/state_recorder.cpython-36.pyc differ
diff --git a/CoLo-AT/simulation_process/sim_manager.py b/CoLo-AT/simulation_process/sim_manager.py
index d2f6e8d87d328d05dae1373b357096e051994eee..c4bf8c4b80802d0830f9728bf1f683f2f71e483b 100644
--- a/CoLo-AT/simulation_process/sim_manager.py
+++ b/CoLo-AT/simulation_process/sim_manager.py
@@ -14,11 +14,11 @@ class SimulationManager():
def __init__(self, name):
self.name = name
- def sim_process_native(self, dataset_labels, dm, robot_system, state_recorder, comm = True, simple_plot = False):
+ def sim_process_native(self, robot_labels, dm, robot_system, state_recorder, comm = True, simple_plot = False):
#dm: dataset manager
print('******** Simulation Process Started! ********')
print('communication: ', comm)
- self.dataset_labels = dataset_labels
+ self.robot_labels = robot_labels
start_time = dm.get_start_time()
duration = dm.get_duration()
@@ -57,7 +57,7 @@ class SimulationManager():
comm_rsp = request_response.Comm_req_resp(self.time, rbt_idx)
message = comm_rsp.get_message()
sender_idx = (rbt_idx+1)%(robot_system.num_robots)
- sender_id = dataset_labels[sender_idx]
+ sender_id = robot_labels[sender_idx]
message['data'] = {'subject_ID':sender_id}
message['groundtruth'] = rsp.get_groundtruth
comm_rsp.set_message(message)
@@ -73,14 +73,14 @@ class SimulationManager():
print('End time: ', self.time)
- return state_var
+ return self.time
- def sim_process_schedule(self, dataset_labels, dm, robot_system, state_recorder, freqs, simple_plot = False):
+ def sim_process_schedule(self, robot_labels, dm, robot_system, state_recorder, freqs, simple_plot = False):
#dm: dataset manager
print('******** Simulation Process Started! ********')
req_type_list = ['odometry', 'measurement', 'communication']
- self.dataset_labels = dataset_labels
+ self.robot_labels = robot_labels
start_time = dm.get_start_time()
duration = dm.get_duration()
@@ -94,7 +94,7 @@ class SimulationManager():
robot_system.set_starting_state(dm.get_starting_states())
# due to adding data
- robot_system.set_start_moving_times([start_time]*len(dataset_labels))
+ robot_system.set_start_moving_times([start_time]*len(robot_labels))
robot_system.load_map(dm.get_landmark_map())
next_possible_time_array= [ [ start_time + 1/freq for freq in f] for f in freqs]
diff --git a/CoLo-AT/test_simulation.py b/CoLo-AT/test_simulation.py
index 19d3ab9ac7a433d1b877ca9cf44cb43def8ed3e1..405984feb153998c5efc24efb195059a6215fad5 100644
--- a/CoLo-AT/test_simulation.py
+++ b/CoLo-AT/test_simulation.py
@@ -9,12 +9,13 @@ Created on Sun Apr 8 17:55:06 2018
import os, sys
import getpass
sys.path.append(os.path.join(os.path.dirname(__file__), "."))
-from dataset_manager.existing_dataset import Dataset
+from dataset_manager.realworld_dataset_manager import RW_Dataset_Manager
from simulation_process.sim_manager import SimulationManager
from robots.robot_system import RobotSystem
from simulation_process.state_recorder import StatesRecorder
from data_analysis.data_analyzer import Analyzer
from data_analysis.realtime_plot import animate_plot
+
from pprint import pprint
# load algorithms
@@ -29,32 +30,35 @@ from ekf_gs_ci2 import EKF_GS_CI2
compname = getpass.getuser()
#dataset_path = "/home/"+ compname +"/CoLo/CoLo-D/UTIAS-Datasets/MRCLAM_Dataset3/"
-dataset_path = "/home/"+ compname +"/CoLo/CoLo-D/CoLo-Datasets/official_dataset2/"
-dataset_labels = [1,2,3]
+
+dataset_path = "/Users/shengkangchen/Documents/CoLo/CoLo-D/CoLo-Datasets/official_dataset1/"
+robot_labels = [1,2,3]
duration = 120 # duration for the simulation in sec
-testing_dataset = Dataset('testing')
-analyzer = Analyzer('analyzer', dataset_labels)
+testing_dataset = RW_Dataset_Manager('testing')
+start_time, starting_states, dataset_data, time_arr = testing_dataset.load_datasets(dataset_path, robot_labels, duration)
+
+
+analyzer = Analyzer('analyzer', robot_labels)
-start_time, starting_states, dataset_data, time_arr = testing_dataset.load_MRCLAMDatasets(dataset_path, dataset_labels, duration)
loc_algo = EKF_LS_BDA('algo')
-robot = RobotSystem('robot', dataset_labels, loc_algo, distr_sys = False)
+robot = RobotSystem('robot', robot_labels, loc_algo, distr_sys = False)
sim = SimulationManager('sim LS-BDA')
-state_recorde_gs_ci = StatesRecorder('LS-BDA', dataset_labels)
-sim.sim_process_native(dataset_labels, testing_dataset, robot, state_recorde_gs_ci, simple_plot = True)
+state_recorde_gs_ci = StatesRecorder('LS-BDA', robot_labels)
+sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorde_gs_ci, simple_plot = True)
loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorde_gs_ci, plot_graphs = True)
##############################################################################
testing_dataset.dataset_reset()
loc_algo = Centralized_EKF('algo')
-robot = RobotSystem('robot', dataset_labels, loc_algo, distr_sys = False)
+robot = RobotSystem('robot', robot_labels, loc_algo, distr_sys = False)
sim = SimulationManager('Centralized_EKF')
-state_recorder_cen = StatesRecorder('Cen-EKF', dataset_labels)
-sim.sim_process_native(dataset_labels, testing_dataset, robot, state_recorder_cen, simple_plot = True)
+state_recorder_cen = StatesRecorder('Cen-EKF', robot_labels)
+sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder_cen, simple_plot = True)
loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_cen, plot_graphs = True)
analyzer.algos_comparison([state_recorde_gs_ci, state_recorder_cen])
diff --git a/CoLo-D/.DS_Store b/CoLo-D/.DS_Store
new file mode 100644
index 0000000000000000000000000000000000000000..112bb0c0e38fbe66bf4c642986eecbec3aaa79f9
Binary files /dev/null and b/CoLo-D/.DS_Store differ
diff --git a/CoLo-D/CoLo-Datasets/.DS_Store b/CoLo-D/CoLo-Datasets/.DS_Store
new file mode 100644
index 0000000000000000000000000000000000000000..d60266028d30bf24c0dcbba150ec026e642d33fc
Binary files /dev/null and b/CoLo-D/CoLo-Datasets/.DS_Store differ