From c46c2287c550aee2c018e491bb1b0b913b14429d Mon Sep 17 00:00:00 2001
From: "oceanpdoshi@g.ucla.edu" <oceanpdoshi@.ucla.edu>
Date: Mon, 22 Apr 2019 21:24:49 -0700
Subject: [PATCH] Added my own test_simulation file for future reference Added
custom noises to simulated_dataset_manager.py Thorough review of
simulated_dataset_manager reveals that it works correctly! fixed delta_t with
regards to alternation between measurement and odometry updates -> scheduling
of this will change in next commit will add multiple robots
---
.../simulated_dataset_manager.py | 89 ++++++++++---------
CoLo-AT/test_simulation_sagar.py | 68 ++++++++++++++
2 files changed, 113 insertions(+), 44 deletions(-)
create mode 100644 CoLo-AT/test_simulation_sagar.py
diff --git a/CoLo-AT/dataset_manager/simulated_dataset_manager.py b/CoLo-AT/dataset_manager/simulated_dataset_manager.py
index 8656977..2651dc5 100644
--- a/CoLo-AT/dataset_manager/simulated_dataset_manager.py
+++ b/CoLo-AT/dataset_manager/simulated_dataset_manager.py
@@ -17,31 +17,32 @@ from requests.request_response import Request_response
# TODO - incorporate communcation (LQI, RSS) as a threshold/cutoff for communication
class SimulatedDataSetManager:
- def __init__(self, dataset_name, boundary, landmarks, duration, robot_labels,
- odometry_noise, communication_noise, measurement_noise, robot_vision = pi/3):
+ 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):
self.name = dataset_name
- # Used for request scheduling
+ # Alternates between 'odometry' and 'measurement'
self.next_request = 'odometry'
self.duration = duration
+ self.boundary = boundary
self.robot_labels = robot_labels
self.num_robots = len(robot_labels)
# Internal time "counter"
self.time_idx = 0
- self.delta_t = 1
+ 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
- # TODO - may need to add more choice in the future (Ex: specify angular velocity noise)
- self.odometry_noise = odometry_noise
+ # User-specified noises, if none given: defaulted to 0
+ self.velocity_noise = velocity_noise
+ self.angular_velocity_noise = angular_velocity_noise
+ self.measurement_range_noise = measurement_range_noise
+ self.bearing_noise = bearing_noise
self.communication_noise = communication_noise
- self.measurement_noise = measurement_noise
- self.boundary = boundary
# All robots begin moving at t = 0s
self.start_time = 20.0
@@ -53,16 +54,15 @@ class SimulatedDataSetManager:
self.landmark_map = landmarks
# starting_states = {robot label: [time, x_pos, y_pos, orientation], ... }
- # TODO - Update this to be user-specified on how to initialize robot locations.
- self.starting_states = {label : [self.start_time, i, i, 0] for i,label in enumerate(robot_labels)}
+ # 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 i in range(self.num_robots)], 'measurement': [[] for i in range(self.num_robots)],
- 'groundtruth': [[] for i in range(self.num_robots)]}
+ 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+1)
+ self.time_arr[data_type][i] = np.arange(self.start_time, self.end_time)
# Measurement Data Format
@@ -134,11 +134,10 @@ class SimulatedDataSetManager:
# 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']]
- velocity = self.calc_distance(loc1, loc2)/self.delta_t + np.random.normal(loc=0.0,scale=self.odometry_noise)
-
+ 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']
- angular_velocity = (theta_2-theta_1)/self.delta_t + np.random.normal(loc=0.0,scale=self.odometry_noise)
+ 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})
@@ -150,9 +149,9 @@ class SimulatedDataSetManager:
robot_loc_x = self.groundtruth_data[i][j]['x_pos']
robot_loc_y = self.groundtruth_data[i][j]['y_pos']
robot_loc = [robot_loc_x, robot_loc_y]
- if (self.within_vision(2*pi, robot_loc, landmark_loc)):
- measurement_range = self.calc_distance(robot_loc, landmark_loc) + np.random.normal(loc=0.0,scale=self.measurement_noise)
- bearing = atan2((landmark_loc[1]-robot_loc[1]), (landmark_loc[0]-robot_loc[0])) + np.random.normal(loc=0.0,scale=0.1)
+ if (True):
+ 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.dataset_data = {'odometry': self.odometry_data, 'measurement': self.measurement_data, 'groundtruth': self.groundtruth_data}
@@ -183,9 +182,6 @@ class SimulatedDataSetManager:
gt = self.groundtruth_data[robot_index][gt_time]
return gt
- def updateTime(self):
- self.time_idx += 1
-
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)
@@ -198,9 +194,10 @@ class SimulatedDataSetManager:
req.set_robot_idx(robot_idx)
req.set_type(req_type)
- print(req.get_message())
+ if (req_type == 'measurement'):
+ current_time += self.delta_t
- return valid_respond, current_time+self.delta_t, req
+ return valid_respond, current_time, req
# Alternates odometry and measurement updates
def get_dataline(self, req, current_time):
@@ -224,14 +221,21 @@ class SimulatedDataSetManager:
message['robot_index'] = robot_idx
-
time_idx = np.where(self.time_arr[req_type][robot_idx] == current_time)[0][0]
- if self.dataset_data[req_type][robot_idx][time_idx]['time'] > self.end_time or time_idx == -1:
+ 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
- else:
- valid_dataline= True
- message['data'] = self.dataset_data[req_type][robot_idx][time_idx]
- message['groundtruth'] = self.groundtruth_data[robot_idx][time_idx]
+
+ if valid_dataline:
+ if req_type == 'odometry':
+ message['data'] = self.odometry_data[robot_idx][time_idx]
+ 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['groundtruth'] = self.groundtruth_data[robot_idx][time_idx]
return valid_dataline, message, req_type, robot_idx, time_idx
# Check if a given loc [x,y] is within map
@@ -254,21 +258,18 @@ 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):
- # arctan() returns between [-pi/2, pi/2] => Correct for our F.O.V application with respect to the x-axis as realistically F.O.V will not exceed pi/3
- bearing = 0
- # Same point
- if (landmark_loc[1] == robot_loc[1] and landmark_loc[0] == robot_loc[0]):
- bearing = 0
- # either + or - pi/2 (delta_x = 0)
- elif (landmark_loc[0] == robot_loc[0]):
- if (landmark_loc[1] < robot_loc[1]):
- bearing = -1*pi/2
- else:
- bearing = pi/2
- else:
- bearing = np.arctan((landmark_loc[1]-robot_loc[1])/(landmark_loc[0]-robot_loc[0])) + np.random.normal(loc=0.0,scale=0.1)
+ 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
+
+ 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)
+ return None
\ No newline at end of file
diff --git a/CoLo-AT/test_simulation_sagar.py b/CoLo-AT/test_simulation_sagar.py
new file mode 100644
index 0000000..d87d160
--- /dev/null
+++ b/CoLo-AT/test_simulation_sagar.py
@@ -0,0 +1,68 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+Created on Apr 17 2019
+
+@author: Sagar
+"""
+
+import os, sys
+import getpass
+import IPython
+from matplotlib import pyplot as plt
+from matplotlib import animation
+
+# 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
+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
+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
+from ekf_ls_bda import EKF_LS_BDA
+from ekf_ls_ci import EKF_LS_CI
+from ekf_gs_bound import EKF_GS_BOUND
+from ekf_gs_ci2 import EKF_GS_CI2
+
+compname = getpass.getuser()
+#dataset_path = "/home/"+ compname +"/CoLo/CoLo-D/UTIAS-Datasets/MRCLAM_Dataset3/"
+
+# landmark IDs must be bigger than 5
+landmarks = {10: [2,5]}
+
+robot_labels = [1]
+boundary = 100 # size of circular map
+duration = 120 # duration for the simulation in sec
+
+# NOTE - noise significantly affects the results
+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 )
+start_time, starting_states, dataset_data, time_arr = testing_dataset.simulate_dataset()
+
+
+# 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)
+
+
+analyzer = Analyzer('analyzer', robot_labels)
+
+loc_algo = Centralized_EKF('CentralizedEKF')
+robot = RobotSystem('robot', robot_labels, loc_algo, distr_sys = False)
+
+sim = SimulationManager('sim')
+state_recorder_bda = StatesRecorder('CentralizedEKF', robot_labels)
+
+sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder_bda, simple_plot = True, comm=False, simulated_comm = False)
+
+loc_err_per_run, trace_per_run, t_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder_bda, plot_graphs = True)
+
--
GitLab