From a188ab0fd5f561ed60d66c00671f2234f1f6fe15 Mon Sep 17 00:00:00 2001
From: "oceanpdoshi@g.ucla.edu" <oceanpdoshi@.ucla.edu>
Date: Mon, 1 Jul 2019 15:38:51 -0700
Subject: [PATCH] CoopJournal - updated to match new data analyzer
data_generator and Simlated Dataset Manager - Updated to allow for user
inputted velocity
Deleted redundant code in request part of SDM
(code extraneously copied value to msg sent out)
In find_measurent(), updated to differentiate
landmark over relative observations ( & prefer)
Need to figure out what's wrong with velocity
Then, need to test communication
---
...perative_Localization_Journal2_sim_data.py | 51 +++++++++----------
CoLo-AT/dataset_manager/data_generator.py | 7 +--
.../simulated_dataset_manager.py | 31 ++++++-----
3 files changed, 47 insertions(+), 42 deletions(-)
diff --git a/CoLo-AT/Cooperative_Localization_Journal2_sim_data.py b/CoLo-AT/Cooperative_Localization_Journal2_sim_data.py
index f792db7..5da60c1 100644
--- a/CoLo-AT/Cooperative_Localization_Journal2_sim_data.py
+++ b/CoLo-AT/Cooperative_Localization_Journal2_sim_data.py
@@ -1,14 +1,8 @@
-#!/usr/bin/env python3
-# -*- coding: utf-8 -*-
-"""
-Created on Thu May 16 15:41:51 2019
-
-@author: william
-"""
import os, sys
import getpass
from math import pi, sqrt
+import numpy as np
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,40 +23,45 @@ from ekf_gs_ci2 import EKF_GS_CI2
from gs_ci_bound import GS_CI_Bound
from ekf_gs_sci2 import EKF_GS_SCI2
+np.random.seed(1)
+
landmarks = {10: [0,5] , 11: [20,3], 12: [-30,0], 13: [-13, -12] }
robot_labels = [1,2,3,4,5]
boundary = 60
-duration = 180
+duration = 120
testing_dataset = SimulatedDataSetManager('testing', boundary, landmarks, duration, robot_labels,
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)
+robot_fov=2*pi/3, delta_t=0.2)
+start_time, starting_states, dataset_data, time_arr = testing_dataset.simulate_dataset('random', test=False, velocity=1, velocity_spread=0.5)
analyzer = Analyzer('analyzer', robot_labels)
+# TODO - implment ISSR 2017 topography
+robots_cant_observe_lm = None
+graph_name = 'Algo Comparison'
+
#############################################################################
loc_algo = EKF_GS_CI2('algo')
robot = RobotSystem('robot', robot_labels, loc_algo, distr_sys = True)
sim = SimulationManager('sim gs_ci')
-state_recorder_gs_ci = StatesRecorder('GS_CI', robot_labels)
+state_recorder_GS_CI2 = StatesRecorder('GS_CI', robot_labels)
-sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder_gs_ci, simple_plot = False)
-loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_gs_ci, plot_graphs = False)
+sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder_GS_CI2, simple_plot = False , robots_cant_observe_lm = robots_cant_observe_lm)
+loc_err_per_run, state_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_GS_CI2, plot_graphs = False)
##############################################################################
-
+'''
loc_algo = GS_CI_Bound('algo')
robot = RobotSystem('robot', robot_labels, loc_algo, distr_sys = True)
sim = SimulationManager('sim gs_ci_bound')
state_recorder_bound = StatesRecorder('GS_CI_bound', robot_labels, state_var_only = True)
-sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder_bound, simple_plot = False)
+sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder_bound, simple_plot = False, robots_cant_observe_lm = robots_cant_observe_lm)
loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_bound, plot_graphs = False)
-
-
+'''
##############################################################################
@@ -71,8 +70,8 @@ robot = RobotSystem('robot', robot_labels, loc_algo, distr_sys = False)
sim = SimulationManager('sim cen_ekf')
state_recorder_LS_cen = StatesRecorder('LS_cen', robot_labels)
-sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder_LS_cen, simple_plot = False)
-loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_LS_cen, plot_graphs = False)
+sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder_LS_cen, simple_plot = False, robots_cant_observe_lm = robots_cant_observe_lm)
+loc_err_per_run, state_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_LS_cen, plot_graphs = False)
##############################################################################
@@ -82,8 +81,8 @@ robot = RobotSystem('robot', robot_labels, loc_algo, distr_sys = False)
sim = SimulationManager('sim ls_bda')
state_recorder_LS_BDA = StatesRecorder('LS_BDA', robot_labels)
-sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder_LS_BDA, simple_plot = False)
-loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_LS_BDA, plot_graphs = False)
+sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder_LS_BDA, simple_plot = False, robots_cant_observe_lm = robots_cant_observe_lm)
+loc_err_per_run, state_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_LS_BDA, plot_graphs = False)
##############################################################################
@@ -94,8 +93,8 @@ robot = RobotSystem('robot', robot_labels, loc_algo, distr_sys = False)
sim = SimulationManager('sim ls_ci')
state_recorder_LS_CI = StatesRecorder('LS_CI', robot_labels)
-sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder_LS_CI, simple_plot = False)
-loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_LS_CI, plot_graphs = False)
+sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder_LS_CI, simple_plot = False, robots_cant_observe_lm = robots_cant_observe_lm)
+loc_err_per_run, state_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_LS_CI, plot_graphs = False)
##############################################################################
@@ -105,7 +104,7 @@ robot = RobotSystem('robot', robot_labels, loc_algo, distr_sys = True)
sim = SimulationManager('sim gs_sci')
state_recorder_GS_SCI = StatesRecorder('GS_SCI', robot_labels)
-sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder_GS_SCI, simple_plot = False)
-loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_GS_SCI, plot_graphs = False)
+sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder_GS_SCI, simple_plot = False, robots_cant_observe_lm = robots_cant_observe_lm)
+loc_err_per_run, state_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_GS_SCI, plot_graphs = False)
-comparison_plot.robot1_algo_comparison_plot([state_recorder_gs_ci, state_recorder_LS_cen, state_recorder_LS_BDA, state_recorder_LS_CI, state_recorder_GS_SCI, state_recorder_bound], only_trace = ['GS_CI_bound'])
\ No newline at end of file
+analyzer.algos_comparison([state_recorder_GS_CI2, state_recorder_LS_cen, state_recorder_LS_BDA, state_recorder_LS_CI, state_recorder_GS_SCI], graph_name = graph_name)
diff --git a/CoLo-AT/dataset_manager/data_generator.py b/CoLo-AT/dataset_manager/data_generator.py
index 13fc4c2..15105d1 100644
--- a/CoLo-AT/dataset_manager/data_generator.py
+++ b/CoLo-AT/dataset_manager/data_generator.py
@@ -37,7 +37,6 @@ class DataGenerator():
if landmark_id < 5:
raise Exception("Invalid landmark ID: landmark IDs must be bigger than 5.")
- # TODO - user specified sample rate
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):
@@ -133,7 +132,7 @@ class DataGenerator():
# 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):
+ def generate_random_data(self, test, velocity, velocity_spread):
# Generate ground truth data
for i, label in enumerate(self.robot_labels, 0):
@@ -148,7 +147,9 @@ 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)))*self.delta_t # units of distance change are in meters, if delta t is one second... v = m/s
+
+ rand_dist = (velocity + np.random.normal(0, velocity_spread))*self.delta_t
+ # np.random.normal(0,sqrt(0.25))
next_x = curr_x + rand_dist*cos(next_orientation)
next_y = curr_y + rand_dist*sin(next_orientation)
diff --git a/CoLo-AT/dataset_manager/simulated_dataset_manager.py b/CoLo-AT/dataset_manager/simulated_dataset_manager.py
index f31dccb..5aa76d0 100644
--- a/CoLo-AT/dataset_manager/simulated_dataset_manager.py
+++ b/CoLo-AT/dataset_manager/simulated_dataset_manager.py
@@ -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, delta_t=1.0):
+ robot_fov = 2*pi, delta_t=1.0):
self.name = dataset_name
self.boundary = float(boundary)
@@ -69,7 +69,7 @@ class SimulatedDataSetManager:
# Input: robot_path (line, circle, etc.)
# Output: Generates simulated data (gt, odo, meas)
- def simulate_dataset(self, path='random', test=False):
+ def simulate_dataset(self, path='random', test=False, velocity=1, velocity_spread = 0):
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
@@ -77,7 +77,7 @@ class SimulatedDataSetManager:
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.time_arr, self.groundtruth_data, self.odometry_data, self.measurement_data, self.dataset_data = self.generator.generate_random_data(test, velocity, velocity_spread)
return self.start_time, self.starting_states, self.dataset_data, self.time_arr
else:
raise Exception("Unsupported robot path specified.")
@@ -125,9 +125,7 @@ class SimulatedDataSetManager:
valid_respond, message, req_type, robot_idx = self.get_dataline(req, current_time)
if (valid_respond):
- req.set_time(current_time)
req.set_message(message)
- req.set_robot_idx(robot_idx)
req.set_type(req_type)
@@ -149,7 +147,6 @@ class SimulatedDataSetManager:
if req.get_type() == None:
req_type = self.curr_request
robot_idx = self.curr_robot_idx
- req.set_type(req_type)
# alternate odometry and measurement requests
if (self.curr_request == 'odometry'):
self.curr_request = 'measurement'
@@ -170,14 +167,15 @@ class SimulatedDataSetManager:
if req_type == 'odometry':
odo_time_idx = find_nearest_time_idx(self.time_arr[req_type][robot_idx], current_time)
message['data'] = self.odometry_data[robot_idx][odo_time_idx]
- # if no measurement data available (ex: no landmarks were in range) returns
- # {'time': -1, 'subject_ID': None, 'measurment_range': None,'bearing': None}
+ # consider case of no measurements
elif req_type == 'measurement':
message['data'] = self.find_measurement(current_time, robot_idx)
gt_time_idx = find_nearest_time_idx(self.time_arr['groundtruth'][robot_idx], current_time)
message['groundtruth'] = self.groundtruth_data[robot_idx][gt_time_idx]
+ message['time'] = current_time
+
return valid_dataline, message, req_type, robot_idx
# Inputs: current time, robot_idx
@@ -188,15 +186,22 @@ class SimulatedDataSetManager:
available_measurements = []
measurement_indices = []
+ # landmark measurements
for data_idx, measurement in enumerate(self.measurement_data[robot_idx]):
- if (abs(measurement['time'] - current_time) < 0.001):
+ if (abs(measurement['time'] - current_time) < 0.001 and measurement['subject_ID'] > 5):
available_measurements.append(measurement['measurment_range'])
measurement_indices.append(data_idx)
- # No exact matches at that time
- if (len(available_measurements)== 0):
- time_idx = find_nearest_time_idx(self.time_arr['measurement'][robot_idx], current_time)
- return self.measurement_data[robot_idx][time_idx]
+ # Relative measurements
+ if (len(available_measurements) == 0):
+ for data_idx, measurement in enumerate(self.measurement_data[robot_idx]):
+ if (abs(measurement['time'] - current_time) < 0.001 and measurement['subject_ID'] <= 5): # relative observation
+ available_measurements.append(measurement['measurment_range'])
+ measurement_indices.append(data_idx)
+
+ # No measurements -> invalid subject id "-1"
+ if (len(available_measurements) == 0):
+ return {'time': current_time,'subject_ID': -1, 'measurment_range': 0, 'bearing': 0}
least_index = np.argmin(available_measurements)
idx = measurement_indices[least_index]
--
GitLab