From e7bed138dc3200d294a6225cc1586d9e545f9013 Mon Sep 17 00:00:00 2001
From: "oceanpdoshi@g.ucla.edu" <oceanpdoshi@.ucla.edu>
Date: Mon, 13 May 2019 15:14:22 -0700
Subject: [PATCH] Fixed within_vision function added relative observations
added testing for relative observations
---
CoLo-AT/dataset_manager/data_generator.py | 76 +++++++++++++------
.../simulated_dataset_manager.py | 5 +-
2 files changed, 54 insertions(+), 27 deletions(-)
diff --git a/CoLo-AT/dataset_manager/data_generator.py b/CoLo-AT/dataset_manager/data_generator.py
index 0dee247..182bcd5 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 738f22c..18ba1ab 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,
--
GitLab