diff --git a/CoLo-AT/dataset_manager/data_generator.py b/CoLo-AT/dataset_manager/data_generator.py
index 0dee24728f69f14b9884809a81920dc907d1ee31..182bcd548c409399770798617a886baf3ae07512 100644
--- a/CoLo-AT/dataset_manager/data_generator.py
+++ b/CoLo-AT/dataset_manager/data_generator.py
@@ -2,7 +2,6 @@ import numpy as np
from math import pi, sqrt, atan2, hypot, sin, cos
import matplotlib.pyplot as plt
-
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,
@@ -145,22 +144,36 @@ 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):
# NOTE - time_arr is used to emplace a one-to-one index-to-time correspondence
# between measurement_data and time_arr['measurement'] to account for varing number of landmark observations
time_arr = []
- # iterate over all possible times and (potentially) observable landmarks
for time_idx in range(0, len(self.time_arr['measurement'][robot_idx])):
- for landmarkID, landmark_loc in self.landmark_map.items():
-
- time = self.time_arr['measurement'][robot_idx][time_idx]
- robot_loc_x = self.groundtruth_data[robot_idx][time_idx]['x_pos']
- robot_loc_y = self.groundtruth_data[robot_idx][time_idx]['y_pos']
- robot_orientation = self.groundtruth_data[robot_idx][time_idx]['orientation']
- robot_loc = [robot_loc_x, robot_loc_y]
+
+ time = self.time_arr['measurement'][robot_idx][time_idx]
+ robot_loc_x = self.groundtruth_data[robot_idx][time_idx]['x_pos']
+ robot_loc_y = self.groundtruth_data[robot_idx][time_idx]['y_pos']
+ robot_orientation = self.groundtruth_data[robot_idx][time_idx]['orientation']
+ robot_loc = [robot_loc_x, robot_loc_y]
+
+ # Relative (other robots) observations
+ other_robot_locs = {label : [self.groundtruth_data[i][time_idx]['x_pos'], self.groundtruth_data[i][time_idx]['y_pos']] for i, label in enumerate(self.robot_labels, 0) if i != robot_idx }
+ for robotID, other_robot_loc in other_robot_locs.items():
+ if (self.within_vision(self.robot_fov, robot_loc, other_robot_loc, robot_orientation)):
+ 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)
+ 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
+
+ # Landmark (absolute) observations
+ for landmarkID, landmark_loc in self.landmark_map.items():
+
if (self.within_vision(self.robot_fov, robot_loc, landmark_loc, robot_orientation)):
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
@@ -217,18 +230,36 @@ class DataGenerator():
measurements = [measurement for measurement in measurement_list if measurement['time']==curr_time]
curr_loc = [x2,y2]
for measurement in measurements:
- landmark_loc = self.landmark_map[measurement['subject_ID']]
-
- actual_meas = self.calc_distance(curr_loc, landmark_loc)
- generated_measurement = measurement['measurment_range']
+ # Verify a landmark measurement
+ if (measurement['subject_ID'] > 5):
+ landmark_loc = self.landmark_map[measurement['subject_ID']]
+
+ 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
+ generated_bearing = measurement['bearing']
+
+ meas_diff = actual_meas - generated_measurement
+ bearing_diff = actual_bearing - generated_bearing
+
+ meas_test_arr[i].append({'meas_diff': meas_diff, 'bearing_diff': bearing_diff, 'time': curr_time})
+ else:
+ other_robot_loc_x = self.groundtruth_data[self.robot_labels.index(measurement['subject_ID'])][g_idx]['x_pos']
+ other_robot_loc_y = self.groundtruth_data[self.robot_labels.index(measurement['subject_ID'])][g_idx]['y_pos']
+
+ other_robot_loc = [other_robot_loc_x, other_robot_loc_y]
+
+ actual_meas = self.calc_distance(curr_loc, other_robot_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
- generated_bearing = measurement['bearing']
+ 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
+ generated_bearing = measurement['bearing']
- meas_diff = actual_meas - generated_measurement
- bearing_diff = actual_bearing - generated_bearing
+ meas_diff = actual_meas - generated_measurement
+ bearing_diff = actual_bearing - generated_bearing
- meas_test_arr[i].append({'meas_diff': meas_diff, 'bearing_diff': bearing_diff, 'time': curr_time})
+ meas_test_arr[i].append({'meas_diff': meas_diff, 'bearing_diff': bearing_diff, 'time': curr_time})
fig, ax_arr = plt.subplots(6)
plt.xlabel('t [s]')
@@ -292,15 +323,12 @@ class DataGenerator():
return distance
# NOTE - add distance limitations to robot_vision?
- # TODO - FIX THIS
# Input: robot_loc, landmark_loc, [x,y]
# Output: boolean, a given point is with the field of view (sector of a circle) of a robot
def within_vision(self, robot_fov, robot_loc, landmark_loc, robot_orientation=0):
- # between pi and -pi with respect to the x axis
- bearing = atan2((landmark_loc[1]-robot_loc[1]),(landmark_loc[0]-robot_loc[0]))
- if bearing < 0:
- bearing += 2*pi
- return True # bearing <= robot_fov/2 + robot_orientation and bearing >= robot_orientation - robot_fov/2
+ # between pi and -pi with respect to LOCAL FRAME
+ bearing = atan2((landmark_loc[1]-robot_loc[1]),(landmark_loc[0]-robot_loc[0])) - robot_orientation
+ return abs(bearing) <= robot_fov/2
# get angle between -pi and pi
def converge_to_bearing_range(self, theta):
diff --git a/CoLo-AT/dataset_manager/simulated_dataset_manager.py b/CoLo-AT/dataset_manager/simulated_dataset_manager.py
index 738f22c3646ae88c660d3d10d04139378d760c21..18ba1ab36ef07cab8503268104815ef8166c2d69 100644
--- a/CoLo-AT/dataset_manager/simulated_dataset_manager.py
+++ b/CoLo-AT/dataset_manager/simulated_dataset_manager.py
@@ -13,9 +13,8 @@ from dataset_manager.data_generator import DataGenerator
# TODO
-# - Circular path data in DataGenerator class
-# - communication
-# - relative observations
+# communication
+# scheduling
class SimulatedDataSetManager:
def __init__(self, dataset_name, boundary, landmarks, duration, robot_labels,
diff --git a/CoLo-AT/simulation_process/data_generation.py b/CoLo-AT/simulation_process/data_generation.py
deleted file mode 100644
index 033695bc4ec8995fa28c552feaaba47a4fbb0d80..0000000000000000000000000000000000000000
--- a/CoLo-AT/simulation_process/data_generation.py
+++ /dev/null
@@ -1,56 +0,0 @@
-#!/usr/bin/env python3
-# -*- coding: utf-8 -*-
-"""
-Created on Tue Feb 13 16:11:36 2018
-
-@author: william
-"""
-
-class DataGenerator():
- def init_(self):
- pass
-
- def within_map(self, loc):
- '''
- For runtime self_generated data,
- Parameters:
- loc: array, shape = (2,1)
- robot's location for [x, y]
- Return: Boolean
- True if within the map, False if not
- '''
- center_point = [0,0]
- if sqrt(pow(loc[0]-center_point[0], 2)+pow(loc[1]-center_point[1], 2)) > d_max:
- return False
- else:
- return True
- def generate_propgation_data(self, current_loc, current_orientation):
-
- '''
- To generate propagtion data randomly so that it will run within predefined map *assuming orientation is perfect
- Parameters:
- current_loc: array, shape = (2,1)
- robot's current location for [x, y]
- current_orientation: float
- robot's current orientation in radian
- Return array, shape = (2,1)
- '''
- [meas_v, meas_a_v] = [random.uniform(-max_v,max_v), random.uniform(-max_a_v, max_a_v)]
- actual_v = meas_v + random.normal(0, sqrt(var_u_v))
- pre_update_position = [current_loc[0] + cos(current_orientation)*actual_v*delta_t, current_loc[1] + sin(current_orientation)*actual_v*delta_t]
-
- while(not within_map(pre_update_position)):
- [meas_v, meas_a_v] = [random.uniform(-max_v,max_v), random.uniform(-max_a_v, max_a_v)]
- actual_v = meas_v + random.normal(0, sqrt(var_u_v))
- pre_update_position = [current_loc[0] + cos(current_orientation)*actual_v*delta_t, current_loc[1] + sin(current_orientation)*actual_v*delta_t]
-
- orientation = current_orientation + meas_a_v*delta_t
- actual_loc = pre_update_position
- return [actual_v, meas_v, orientation, actual_loc]
-
- def generate_measurement_data(self, observer_loc, observee_loc, observer_orientation):
- delta_x = observer_loc[0] - observee_loc[0]
- delta_y = observer_loc[1] - observee_loc[1]
- dis = sqrt(delta_y*delta_y + delta_x*delta_x) + random.normal(0, sqrt(var_dis))
- bearing = atan2(delta_y, delta_x) + random.normal(0, sqrt(var_angle)) - observer_orientation
- return [dis, bearing]