From 66cc2c2188a6afd1e29a97ac01358fa29abb6698 Mon Sep 17 00:00:00 2001
From: "oceanpdoshi@g.ucla.edu" <oceanpdoshi@.ucla.edu>
Date: Mon, 1 Jul 2019 17:08:08 -0700
Subject: [PATCH] Velocity is now a parameter for circle and line
---
CoLo-AT/dataset_manager/data_generator.py | 50 ++++++++-----------
.../simulated_dataset_manager.py | 9 ++--
2 files changed, 26 insertions(+), 33 deletions(-)
diff --git a/CoLo-AT/dataset_manager/data_generator.py b/CoLo-AT/dataset_manager/data_generator.py
index 15105d1..1af1b6c 100644
--- a/CoLo-AT/dataset_manager/data_generator.py
+++ b/CoLo-AT/dataset_manager/data_generator.py
@@ -4,14 +4,13 @@ import matplotlib.pyplot as plt
import IPython
-# TODO - add collision/boundary checking for circle/random data
+# TODO - add collision checking for circle/random data
class DataGenerator():
- def __init__(self, boundary, landmarks, duration, robot_labels, starting_states, start_time, delta_t,
+ def __init__(self, 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,
robot_fov = 2*pi/3):
self.duration = duration
- self.boundary = boundary
self.robot_labels = robot_labels
self.num_robots = len(robot_labels)
self.start_time = start_time
@@ -49,11 +48,10 @@ class DataGenerator():
# Inputs: None
# Outputs: time_arr, groundtruth_data, self.odometry_data, measurement_data, self.dataset_data
- # Generates simulated data based of robots moving in a straight line until simulation ends/robots hit the boundary and stop moving
- def generate_straight_line_data(self, test):
-
- velocity_step = self.boundary/self.duration
- move_step = velocity_step * self.delta_t
+ # Generates simulated data based of robots moving in a straight line until simulation ends
+ def generate_straight_line_data(self, test, velocity=0.1, velocity_spread=0):
+
+ move_step = (velocity + np.random.normal(0, velocity_spread)) * self.delta_t
# Generate ground truth data
for i, label in enumerate(self.robot_labels, 0):
@@ -69,9 +67,8 @@ class DataGenerator():
next_x = curr_x
next_y = curr_y
- if (self.within_map([curr_x + move_step, curr_y])):
- next_x = curr_x + move_step
- next_y = curr_y
+ next_x = curr_x + move_step
+ next_y = curr_y
self.groundtruth_data[i].append({'time' : self.time_arr['groundtruth'][i][time_idx], 'x_pos': next_x, 'y_pos': next_y, 'orientation': next_orientation})
@@ -88,15 +85,11 @@ class DataGenerator():
# Inputs: None
# Outputs: time_arr, groundtruth_data, self.odometry_data, measurement_data, self.dataset_data
# Generates simulated of robots moving in a circle
- def generate_circular_data(self, test):
-
- # NOTE - this method of generating a circular path goes from polygons (low duration) to rounder as duration increases
- path_radius = self.boundary/(self.num_robots + 3)
- path_circumference = 2*pi*path_radius
+ def generate_circular_data(self, test, velocity=0.1, velocity_spread=0):
radian_step = 2*pi/self.duration
- velocity_step = path_circumference/self.duration # May want to randomize velocity w/normal distribution
- distance_step = velocity_step*self.delta_t
+ # if it's a "perfect" circle, then velocity spread will be 0
+ distance_step = (velocity + np.random.normal(0, velocity_spread))*self.delta_t
# Generate ground truth data
for i, label in enumerate(self.robot_labels, 0):
@@ -129,9 +122,9 @@ class DataGenerator():
return self.time_arr, self.groundtruth_data, self.odometry_data, self.measurement_data, self.dataset_data
- # Inputs: None
+ # Inputs: test=True (run self.verify_generated_data), veloicty, velocity_spread (normally distributed)
# Outputs: time_arr, groundtruth_data, self.odometry_data, measurement_data, self.dataset_data
- # Generates simulated of robots moving randomly
+ # Simulates random motion
def generate_random_data(self, test, velocity, velocity_spread):
# Generate ground truth data
@@ -166,8 +159,8 @@ class DataGenerator():
return self.time_arr, self.groundtruth_data, self.odometry_data, self.measurement_data, self.dataset_data
# Inputs: None
- # Outputs: time_arr, groundtruth_data, self.odometry_data, measurement_data, self.dataset_data
- # Generates simulated data for robots moving completely randomly (within realism)
+ # Outputs: None
+ # For given robot groundtruths, use velocity = distance/time to generate odometry data
def generate_odometry_data(self):
for i, label in enumerate(self.robot_labels, 0):
# Initial robot state
@@ -188,7 +181,9 @@ 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' : (t2-t1)})
-
+ # Inputs: None
+ # Outputs: None
+ # for given robot groundtruths, robot_fov, and landmark map, generate all potential measurements at each time step
def generate_measurement_data(self):
# Generate Measurement Data
for robot_idx, label in enumerate(self.robot_labels, 0):
@@ -364,9 +359,9 @@ class DataGenerator():
print('******Verification Complete******')
# Input: loc=[x,y]
- # Output: boolean if the point [x,y] is within the map
- def within_map(self, loc):
- return hypot(loc[0], loc[1]) < self.boundary
+ # Output: boolean if the point [x,y] is within the circular map of radius r centered @ origin
+ def within_map(self, loc, r):
+ return hypot(loc[0], loc[1]) < r
# Input: loc1=[x1,y1], loc2=[x2,y2]
# Output: Euclidean distance between two points
@@ -381,7 +376,6 @@ class DataGenerator():
distance = sqrt((x1-x2)**2 + (y1-y2)**2)
return distance
- # NOTE - add distance limitations to robot_vision?
# 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):
@@ -421,7 +415,7 @@ Odometry Format
Measurement Data Format
-subject ID = Landmark ID
+subject ID = Landmark ID/RobotID
measurement_range = distance between robot and landmark
bearing = angle betweeen robot and landmark, defined with respect to ROBOT'S FRAME OF REFERENCE
{
diff --git a/CoLo-AT/dataset_manager/simulated_dataset_manager.py b/CoLo-AT/dataset_manager/simulated_dataset_manager.py
index 5aa76d0..702e82d 100644
--- a/CoLo-AT/dataset_manager/simulated_dataset_manager.py
+++ b/CoLo-AT/dataset_manager/simulated_dataset_manager.py
@@ -26,12 +26,11 @@ def find_nearest_time_idx(l, value):
# scheduling
class SimulatedDataSetManager:
- def __init__(self, dataset_name, boundary, landmarks, duration, robot_labels,
+ def __init__(self, dataset_name, 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, delta_t=1.0):
self.name = dataset_name
- self.boundary = float(boundary)
self.duration = duration
self.robot_labels = robot_labels
self.num_robots = len(robot_labels)
@@ -64,17 +63,17 @@ class SimulatedDataSetManager:
self.measurement_data = [[] for robot in range(self.num_robots)]
self.dataset_data = []
- self.generator = DataGenerator(boundary, landmarks, duration, robot_labels, self.starting_states, self.start_time, self.delta_t,
+ self.generator = DataGenerator(landmarks, duration, robot_labels, self.starting_states, self.start_time, self.delta_t,
velocity_noise, angular_velocity_noise, measurement_range_noise, bearing_noise, communication_noise, robot_fov)
# Input: robot_path (line, circle, etc.)
# Output: Generates simulated data (gt, odo, meas)
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)
+ self.time_arr, self.groundtruth_data, self.odometry_data, self.measurement_data, self.dataset_data = self.generator.generate_straight_line_data(test, velocity, velocity_spread)
return self.start_time, self.starting_states, self.dataset_data, self.time_arr
elif(path=='circle'):
- self.time_arr, self.groundtruth_data, self.odometry_data, self.measurement_data, self.dataset_data = self.generator.generate_circular_data(test)
+ self.time_arr, self.groundtruth_data, self.odometry_data, self.measurement_data, self.dataset_data = self.generator.generate_circular_data(test, velocity, velocity_spread)
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, velocity, velocity_spread)
--
GitLab