Created Debug Report

Added Test Script & Debug Report to Documentation
Slight logic/variable scope modifications in:
DagaGenerator and SimulatedDatasetManager

CoLo-AT/Documentation/test_simulation_sagar.py

+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+Created on Apr 17 2019
+
+@author: Sagar
+"""
+
+# Import necessary Libraries
+import os, sys
+import numpy as np
+from math import pi, sqrt
+
+# Import other Colo-AT Modules
+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
+
+# Import Localization Algorithms
+sys.path.append(os.path.join(os.path.dirname(__file__), "localization_algos"))
+
+# LS -> centralized system.
+from centralized_ekf import Centralized_EKF
+from ekf_ls_bda import EKF_LS_BDA
+from ekf_ls_ci import EKF_LS_CI
+# GS -> distributed system
+from ekf_gs_bound import EKF_GS_BOUND
+from ekf_gs_ci2 import EKF_GS_CI2
+from ekf_gs_sci2 import EKF_GS_SCI2
+
+
+# Template code for running/debugging simulated data
+
+# Set a random seed for replication
+np.random.seed(1)
+
+# Initialize landmark map, landmark IDs must be larger than 5
+# Format {LandmarkID: [x,y]}
+landmarks = {11: [1,0], 12: [0,1], 13: [-1,0], 14: [0,-1]}
+
+# Initalize Robots, can be 1-5
+robot_labels = [1,2,3]
+
+# Specify simulation parameters:
+# Units are [m], [s], and [rad]
+duration = 120
+delta_t = 0.2
+
+v_noise = 0.05
+w_noise = 0.0001
+r_noise = 0.05 
+phi_noise = sqrt(2)*pi/180
+fov = 2*pi
+
+v = 0.1
+sigma_v = 0.0
+
+# Instantiate Simulated Dataset Manager
+testing_dataset = SimulatedDataSetManager('name', landmarks, duration, robot_labels, 
+velocity_noise=v_noise, angular_velocity_noise=w_noise, measurement_range_noise=r_noise, bearing_noise=phi_noise,
+robot_fov=fov, delta_t=delta_t)
+
+# Generate the data
+start_time, starting_states, dataset_data, time_arr = testing_dataset.simulate_dataset('random', test=True, velocity=v, velocity_spread=sqrt(sigma_v))
+
+# Create Data Analyzer
+analyzer = Analyzer('analyzer', robot_labels)
+
+# Specify Loc Algo and create RobotSystem
+loc_algo =  Centralized_EKF('Centralized_EKF')
+robot = RobotSystem('robot', robot_labels, loc_algo, distr_sys = False)
+
+# Create the simulation manager and state recorder
+sim = SimulationManager('sim')
+state_recorder = StatesRecorder('Centralized_EKF', robot_labels)
+
+# Run the simulation
+end_time = sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder, simple_plot = True, comm=False, simulated_comm = False)
+
+# Analyze the data
+loc_err_per_run, state_err_per_run, trace_per_run, time_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder, plot_graphs = False)
+
+# View an animated display of robot movement and loc algo performance
+animate_plot(robot_labels, state_recorder, analyzer, testing_dataset.get_landmark_map())
+
+
+
+
+'''
+# Input: loc1=[x1,y1], loc2=[x2,y2]
+# Output: Euclidean distance between two points
+def calc_distance(loc1, loc2):
+
+    x1 = loc1[0]
+    y1 = loc1[1]
+
+    x2 = loc2[0]
+    y2 = loc2[1]
+
+    distance = sqrt((x1-x2)**2 + (y1-y2)**2)
+    return distance
+
+np.random.seed(1)
+
+# landmark IDs must be bigger than 5
+# landmarks = {11: [120,2]}
+landmarks = {11: [0,-1], 12: [1,0], 13: [1,1], 14: [0,1]}
+# landmarks = {11: [1,0], 12: [0,1], 13: [-1,0], 14: [0,-1]}
+
+robot_labels = [1]
+duration = 120
+
+delta_t = 0.2
+
+# NOTE - for documentation: you need at least 3 robots to run communication
+
+
+testing_dataset = SimulatedDataSetManager('testing', landmarks, duration, robot_labels, 
+velocity_noise=.05, angular_velocity_noise=0.00001, measurement_range_noise=.05, bearing_noise=sqrt(2) *pi/180
+,robot_fov=2*pi, delta_t=delta_t)
+
+start_time, starting_states, dataset_data, time_arr = testing_dataset.simulate_dataset('circle', test=False, 
+velocity=.1, velocity_spread=0.0)
+
+analyzer = Analyzer('analyzer', robot_labels)
+
+loc_algo =  Centralized_EKF('Centralized_EKF')
+robot = RobotSystem('robot', robot_labels, loc_algo, distr_sys = False)
+
+sim = SimulationManager('sim')
+state_recorder = StatesRecorder('Centralized_EKF', robot_labels)
+
+end_time = sim.sim_process_native(robot_labels, testing_dataset, robot, state_recorder, simple_plot = True, comm=False, simulated_comm = False)
+
+loc_err_per_run, state_err_per_run, trace_per_run, time_arr = analyzer.calculate_loc_err_and_trace_state_variance_per_run(state_recorder, plot_graphs = False)
+
+# animate_plot(robot_labels, state_recorder, analyzer, testing_dataset.get_landmark_map())
+'''
+
+
+'''
+# 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)
+
+# Investigation of RMS/trace corresponding to meas_range
+x_diffs = diffs['x_diff']
+y_diffs = diffs['y_diff']
+
+times = np.arange(0, 120, delta_t)
+
+x_diffs = list(filter(lambda a: a != -1, x_diffs))
+x_diffs = [np.asscalar(x) for x in x_diffs]
+y_diffs = list(filter(lambda a: a != -1, y_diffs))
+y_diffs = [np.asscalar(y) for y in y_diffs]
+
+robot_x = [gt['x_pos'] for gt in dataset_data['groundtruth'][0]]
+robot_y = [gt['y_pos'] for gt in dataset_data['groundtruth'][0]]
+
+robot_locs = [ [robot_x[i], robot_y[i] ] for i in range(0, len(robot_x))]
+distances = [calc_distance(landmarks[11], robot_loc) for robot_loc in robot_locs]
+
+
+fig = plt.figure(2)
+
+plt.plot(times, x_diffs, times, y_diffs)
+
+plt.xlabel('t [s]')
+plt.ylabel('|z-z_hat| [m]')
+
+plt.legend(['x', 'y'], loc='upper right')
+
+fig = plt.figure(3)
+
+plt.plot(times, distances)
+plt.xlabel('t [s]')
+plt.ylabel('robot distance to landmark [m]')
+
+plt.show()
+'''
\ No newline at end of file

CoLo-AT/dataset_manager/data_generator.py

@@ -87,7 +87,7 @@ class DataGenerator():
     # Generates simulated of robots moving in a circle
     def generate_circular_data(self, test, velocity=0.1, velocity_spread=0):
 
-        radian_step = 2*pi/self.duration # can be hardcoded to be a certain amount of degrees (radians)
+        radian_step = pi/self.duration # can be hardcoded to be a certain amount of degrees (radians)
         # if it's a "perfect" circle, then velocity spread will be 0 
         distance_step = (velocity + np.random.normal(0, velocity_spread))*self.delta_t
 
@@ -353,7 +353,7 @@ class DataGenerator():
 
         fig.legend(labels, prop={'size': 12})
         plt.subplots_adjust(left=.07, bottom=.08, right=.94, top=.89, wspace=.2, hspace=.75)
-        # plt.show()
+        plt.show()
         # plt.close('all')
 
         print('******Verification Complete******')

CoLo-AT/dataset_manager/simulated_dataset_manager.py

@@ -123,10 +123,10 @@ class SimulatedDataSetManager:
         valid_respond, message, req_type, robot_idx = self.get_dataline(req, current_time)
 
         if (valid_respond):
-            req.set_message(message)
             req.set_type(req_type)
+            req.set_message(message)
 
-        if (req_type == 'measurement' and self.reset_robot_ctr()):
+        if (self.reset_robot_ctr()): # and req_type == 'measurement'
             current_time += self.delta_t
 
         return valid_respond, current_time, req
@@ -138,11 +138,10 @@ class SimulatedDataSetManager:
         message = req.get_message()
 
         if message['robot_index'] == None:
-            
+            robot_idx = self.curr_robot_idx
             # select req_type, robot_idx
             if req.get_type() == None:
                 req_type = self.curr_request
-                robot_idx = self.curr_robot_idx
                 # alternate odometry and measurement requests
                 if (self.curr_request == 'odometry'):
                     self.curr_request = 'measurement'
@@ -177,6 +176,7 @@ class SimulatedDataSetManager:
     # Inputs: current time, robot_idx
     # Outputs:  closest landmark measurement for given time and robot, if available
     #           {'time': current_time, 'subject_ID': None, 'measurment_range': None,'bearing': None} if no measurement available
+    # NOTE - prefers absolute observations over relative observations
     def find_measurement(self, current_time, robot_idx):
         
         available_measurements = []