From a26b2214614adcc774f6ea86f92a967af93a15ac Mon Sep 17 00:00:00 2001
From: "oceanpdoshi@g.ucla.edu" <oceanpdoshi@.ucla.edu>
Date: Fri, 19 Apr 2019 12:49:29 -0700
Subject: [PATCH] Refactored some more elements of the code to be
interchangable. Currently debugging communications of request/response
messages.
---
.../simulated_dataset_manager.py | 111 +++++++-----------
1 file changed, 42 insertions(+), 69 deletions(-)
diff --git a/CoLo-AT/dataset_manager/simulated_dataset_manager.py b/CoLo-AT/dataset_manager/simulated_dataset_manager.py
index b8ddf90..8656977 100644
--- a/CoLo-AT/dataset_manager/simulated_dataset_manager.py
+++ b/CoLo-AT/dataset_manager/simulated_dataset_manager.py
@@ -1,4 +1,4 @@
-from math import pi, sqrt
+from math import pi, sqrt, atan2
import numpy as np
import IPython
@@ -29,6 +29,10 @@ class SimulatedDataSetManager:
self.robot_labels = robot_labels
self.num_robots = len(robot_labels)
+ # Internal time "counter"
+ self.time_idx = 0
+ self.delta_t = 1
+
# angular width of robots's vision with respect to the x_axis (radians)
self.robot_vision = robot_vision
@@ -40,16 +44,17 @@ class SimulatedDataSetManager:
self.boundary = boundary
# All robots begin moving at t = 0s
- self.start_time = 0
+ self.start_time = 20.0
self.end_time = self.start_time + self.duration
self.start_moving_times = [0 for label in robot_labels]
# landmarks = {landmark ID: [x,y]}
+ # NOTE - landmark IDs must be bigger than 5.
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 : [0, i, i, 0] for i,label in enumerate(robot_labels)}
+ self.starting_states = {label : [self.start_time, i, i, 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)],
@@ -57,7 +62,7 @@ class SimulatedDataSetManager:
for data_type in self.time_arr:
for i in range(self.num_robots):
- self.time_arr[data_type][i] = np.arange(duration)
+ self.time_arr[data_type][i] = np.arange(self.start_time, self.end_time+1)
# Measurement Data Format
@@ -98,80 +103,57 @@ class SimulatedDataSetManager:
'''
self.groundtruth_data = [ [] for i in range(self.num_robots)]
-
- # TODO - convert to dynamic generation, just generate ground truth data
def simulate_dataset(self):
-
# Generate ground truth data
# NOTE - FOR NOW THIS WILL JUST MOVE ROBOTS IN A STRAIGHT LINE UNTIL THEY REACH THE BOUNDARY - THEN THEY WILL BE STATIONARY
for i, label in enumerate(self.robot_labels, 0):
# append starting state
self.groundtruth_data[i].append({'time' : self.starting_states[label][0], 'x_pos': self.starting_states[label][1], 'y_pos': self.starting_states[label][2], 'orientation' : self.starting_states[label][3]})
- for time in range(1, len(self.time_arr['groundtruth'][i])):
- curr_x = self.groundtruth_data[i][time-1]['x_pos']
- curr_y = self.groundtruth_data[i][time-1]['y_pos']
- curr_orientation = self.groundtruth_data[i][time-1]['orientation']
+ for j in range(1, len(self.time_arr['groundtruth'][i])):
+ curr_x = self.groundtruth_data[i][j-1]['x_pos']
+ curr_y = self.groundtruth_data[i][j-1]['y_pos']
+ curr_orientation = self.groundtruth_data[i][j-1]['orientation']
next_x = curr_x
next_y = curr_y
next_orientation = curr_orientation
- next_time = time + 1
if (self.within_map([curr_x + 1, curr_y + 1])):
next_x = curr_x + 1
next_y = curr_y
- self.groundtruth_data[i].append({'time' : next_time, 'x_pos': next_x, 'y_pos': next_y, 'orientation': next_orientation})
+ self.groundtruth_data[i].append({'time' : self.time_arr['groundtruth'][i][j], 'x_pos': next_x, 'y_pos': next_y, 'orientation': next_orientation})
# NOTE - modify incorporation of noise
# Generate Odometery Data
for i, label in enumerate(self.robot_labels, 0):
# Initial robot state
- self.odometry_data[i].append({'time': 0, 'velocity' : 0, 'angular velocity' : 0, 'orientation' : self.groundtruth_data[i][0]['orientation'],
+ self.odometry_data[i].append({'time': self.start_time, 'velocity' : 0, 'angular velocity' : 0, 'orientation' : self.groundtruth_data[i][0]['orientation'],
'delta_t' : 0})
- for time in range(1, len(self.time_arr['odometry'][i])):
- # NOTE - delta_t is currently hard coded to 1 as we only have ground truth data on each individual tick, this can be changed later on
- delta_t = 1
- loc1 = [self.groundtruth_data[i][time-1]['x_pos'], self.groundtruth_data[i][time-1]['y_pos']]
- loc2 = [self.groundtruth_data[i][time]['x_pos'], self.groundtruth_data[i][time]['y_pos']]
- velocity = self.calc_distance(loc1, loc2)/delta_t + np.random.normal(loc=0.0,scale=self.odometry_noise)
+ for j in range(1, len(self.time_arr['odometry'][i])):
+ # 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)
- theta_1 = self.groundtruth_data[i][time-1]['orientation']
- theta_2 = self.groundtruth_data[i][time]['orientation']
- angular_velocity = (theta_2-theta_1)/delta_t + np.random.normal(loc=0.0,scale=0.1)
+ 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)
- self.odometry_data[i].append({'time': time, 'velocity' : velocity, 'angular velocity' : angular_velocity,
- 'orientation' : self.groundtruth_data[i][time]['orientation'], 'delta_t' : delta_t})
+ 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})
- # Generate Measurment Data
+ # Generate Measurement Data
for i, label in enumerate(self.robot_labels, 0):
- for time in range(0, len(self.time_arr['measurement'][i])):
+ for j in range(0, len(self.time_arr['measurement'][i])):
for landmarkID, landmark_loc in self.landmark_map.items():
- robot_loc_x = self.groundtruth_data[i][time]['x_pos']
- robot_loc_y = self.groundtruth_data[i][time]['y_pos']
+ 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)):
-
- measurment_range = self.calc_distance(robot_loc, landmark_loc) + np.random.normal(loc=0.0,scale=self.measurement_noise)
- # NOTE - arctan returns between [-pi/2, pi/2]
- bearing = 0
- # Same point
- if (landmark_loc[1] == robot_loc[1] and landmark_loc[0] == robot_loc[0]):
- # TODO - robots are NOT supposed to collide with landmarks
- 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)
- # We only want bearings between [0, 2pi]
- if (bearing < 0):
- bearing += 2*pi
-
- self.measurement_data[i].append({'time':time, 'subject_ID': landmarkID, 'measurment_range':measurment_range, 'bearing':bearing})
+ 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)
+ 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}
return self.start_time, self.starting_states, self.dataset_data, self.time_arr
@@ -201,24 +183,26 @@ 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)
- current_time += 1
+ # current_time == self.time_arr[req_type][robot_idx][time_idx]
if (valid_respond):
- req.set_time(time_idx)
+ req.set_time(current_time)
req.set_message(message)
req.set_robot_idx(robot_idx)
req.set_type(req_type)
- print(req.get_type())
- return valid_respond, current_time, req
+ print(req.get_message())
+
+ return valid_respond, current_time+self.delta_t, req
# Alternates odometry and measurement updates
- # TODO - dynamic generation
def get_dataline(self, req, current_time):
message = req.get_message()
@@ -236,12 +220,12 @@ class SimulatedDataSetManager:
self.next_request = 'odometry'
else:
req_type = req.get_type()
- time_arr = self.get_time_arr(req_type)
- robot_idx = np.argmin(time_arr)
+ robot_idx = np.argmin(self.time_arr[req_type])
message['robot_index'] = robot_idx
- time_idx = current_time
+
+ 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 = False
else:
@@ -250,17 +234,6 @@ class SimulatedDataSetManager:
message['groundtruth'] = self.groundtruth_data[robot_idx][time_idx]
return valid_dataline, message, req_type, robot_idx, time_idx
- # Helper Functions
-
- def generate_groundtruth():
- return True
-
- def generate_odometry():
- return True
-
- def generate_measurement():
- return True
-
# Check if a given loc [x,y] is within map
def within_map(self, loc):
center_point = [0,0]
--
GitLab