def resolver_crank_nicolson(self, u0_in, f_in, g1_in, g2_in, N_in):
delta_t = delta_x = 1 / N_in
lambda_calc = 1 / delta_x
a_diagonal, a_sub_diagonal = self.calc_decomp(
self.gerar_matriz_crank_nicolson(N_in, lambda_calc))
u = []
for xi in range(1, N_in):
u.append(u0_in(xi * delta_x))
contador_adicionar_novo_plot = 0
PlotDatas = []
for i in range(0, N_in):
t = delta_t * i
if contador_adicionar_novo_plot <= t:
u_sub = []
u_sub.append(g1_in(t))
for x in range(0, N_in - 1):
u_sub.append(u[x])
u_sub.append(g2_in(t))
PlotDatas.append(
PlotData(contador_adicionar_novo_plot, u_sub.copy()))
contador_adicionar_novo_plot = contador_adicionar_novo_plot + 0.1
u_aux = [None] * (N_in - 1)
u_aux[0] = u[0] + (lambda_calc / 2) * (
g1_in(t) - 2 * u[0] +
u[1]) + (delta_t / 2) * (f_in(t, delta_x, delta_x) + f_in(
t + delta_t, delta_x, delta_x)) + (lambda_calc /
2) * g1_in(t + delta_t)
u_aux[N_in - 2] = u[N_in - 2] + (lambda_calc / 2) * (
u[N_in - 3] - 2 * u[N_in - 2] + g2_in(t)) + (delta_t / 2) * (
f_in(t, (N_in - 1) * delta_x, delta_x) +
f_in(t + delta_t, (N_in - 1) * delta_x, delta_x)) + (
lambda_calc / 2) * (g2_in(t + delta_t))
for x in range(1, N_in - 2):
u_aux[x] = u[x] + (lambda_calc / 2) * (
u[x - 1] - 2 * u[x] + u[x + 1]) + (delta_t / 2) * (
f_in(t, (x + 1) * delta_x, delta_x) +
f_in(t + delta_t, (x + 1) * delta_x, delta_x))
u = u_aux.copy()
temp = self.substituicao_direta(a_sub_diagonal, u)
temp = self.resolver_diagonal(a_diagonal, temp)
u = self.substituicao_inversa(a_sub_diagonal, temp)
u_final = []
u_final.append(g1_in(1))
for x in range(0, N_in - 1):
u_final.append(u[x])
u_final.append(g2_in(1))
return PlotDatas, u_final
def __init__(self):
self.plot_data = PlotData()
self.cart = CartPoleEnv()
self.cart.reset()
self.predi_net = DQN()
self.updat_net = deepcopy(self.predi_net)
self.turn = 0
self.epidode = 0
self.epsilon = config.epsilon
self.eps_decay = 0.99
self.visu = False
self.visu_update = False #300
self.visu_window = 5
self.consecutive_wins = 0
self.best_consecutive_wins = 0
self.last_save = 0
self.memory = []
def __init__(self):
self.plot_data = PlotData()
self.env = CartPoleEnv()
self.main_net = DQN()
self.target_net = deepcopy(self.main_net)
self.epsilon = config.epsilon
self.eps_decay = 0.995
self.visu = False
self.visu_update = False#300
self.visu_window = 5
self.memory = Memory(memory_size = 30)
self.batch_size = 5
def get_plot_data(recording):
if recording not in prepared_plot_data:
if len(prepared_plot_data) > 2:
oldest_data = None
oldest_ts = math.inf
for key, data in prepared_plot_data.copy().items():
if data.last_access < oldest_ts:
oldest_ts = data.last_access
oldest_data = key
if oldest_data in prepared_plot_data:
del prepared_plot_data[oldest_data]
prepared_plot_data[recording] = PlotData(recording, os.path.join(RECORDINGS_FOLDER, recording))
return prepared_plot_data[recording]
def resolver_euler(self, u0_in, f_in, g1_in, g2_in, N_in):
delta_t = delta_x = 1 / N_in
lambda_calc = N_in
a_diagonal, a_sub_diagonal = self.calc_decomp(
self.gerar_matriz_euler(N_in, lambda_calc))
u = []
for xi in range(1, N_in):
u.append(u0_in(xi * delta_x))
contador_adicionar_novo_plot = 0
PlotDatas = []
for i in range(0, N_in):
t = delta_t * i
if contador_adicionar_novo_plot <= t:
u_aux = []
u_aux.append(g1_in(t))
for x in range(0, N_in - 1):
u_aux.append(u[x])
u_aux.append(g2_in(t))
PlotDatas.append(
PlotData(contador_adicionar_novo_plot, u_aux.copy()))
contador_adicionar_novo_plot = contador_adicionar_novo_plot + 0.1
u[0] = u[0] + lambda_calc * g1_in(t + delta_t)
u[N_in - 2] = u[N_in - 2] + lambda_calc * g2_in(t + delta_t)
for x in range(0, N_in - 1):
u[x] = u[x] + delta_t * f_in(t + delta_t,
(x + 1) * delta_x, delta_x)
temp = self.substituicao_direta(a_sub_diagonal, u)
temp = self.resolver_diagonal(a_diagonal, temp.copy())
u = self.substituicao_inversa(a_sub_diagonal, temp).copy()
u_final = []
u_final.append(g1_in(1))
for x in range(0, N_in - 1):
u_final.append(u[x])
u_final.append(g2_in(1))
return PlotDatas, u_final
def __init__(self):
rospy.init_node('performance_tracker', anonymous=True)
# check arguments
self.rovio_ns = rospy.get_param("~rovio_ns", "")
self.run_name = rospy.get_param("~run_name", "run")
run_duration = rospy.get_param("~run_duration", 107)
self.sampling_time = rospy.get_param(
"~sampling_time", 0.1) # The loop sleeps for this long
min_buffer_size = run_duration / self.sampling_time # Minimum columsn for arrays containing periodically fetched data
self.start_time = 0
#tf setup
self.tf_Buffer = tf.Buffer(rospy.Duration(10))
self.tf_listener = tf.TransformListener(self.tf_Buffer)
self.tf_broadcaster = tf.TransformBroadcaster()
self.tf_static_broadcaster = tf.StaticTransformBroadcaster()
self.world_frame = "odom"
self.drone_frame = "imu"
self.vicon_body_frame = "vicon/imu"
# All subscribers
# Subscribers listening to the asa ros wrapper
rospy.Subscriber('/odometry', Odometry, self.odometry_update)
rospy.Subscriber('/fox/vrpn_client/estimated_transform',
TransformStamped,
self.groundtruth_update,
queue_size=1)
self.rovio_odom_sub = rospy.Subscriber('/rovio/odometry',
Odometry,
self.trajestimate_update,
queue_size=1)
rospy.Subscriber('/odometry_alt',
Odometry,
self.alt_trajestimate_update,
queue_size=1)
self.use_alternative_facts = False
rospy.on_shutdown(self.save_data_to_disk)
self.stampedTrajEstimate = PlotData(min_buffer_size,
self.sampling_time)
self.stampedGroundtruth = PlotData(min_buffer_size, self.sampling_time)
self.correctionData = PlotData(
rospy.get_param("~plot_buffer_anchor_correction", 100))
self.poseUpdateTimeStamps = []
rospy.loginfo("Precision tracker intitialized with:")
rospy.loginfo("Duration of Rosbag reported: " + str(run_duration) +
"s")
rospy.loginfo("Buffer size selected: " + str(min_buffer_size))
def main(args):
"""Start analysis"""
if args.config:
cfg = create_dictionary(args.config)
ext = os.path.dirname(args.config)
else:
print("AliSys needs at least the --config parameter. Type AliSys --help to see all params")
sys.exit(0)
plot = PlotData(os.path.join(os.getcwd(),ext,cfg.get("plot_config_file", "plot_cfg.yml")))
results = {}
meas_files = read_meas_files(cfg)
it = 0
for ped, cal, run in meas_files:
it+=1
ped_data = NoiseAnalysis(ped, configs=cfg)
results["NoiseAnalysis"] = ped_data
cal_data = Calibration(cal, Noise_calc=ped_data, configs=cfg)
results["Calibration"] = cal_data
cfg.update({"calibration": cal_data,
"noise_analysis": ped_data})
if run:
run_data = MainAnalysis(run, configs=cfg)
results["MainAnalysis"] = run_data.results
# Start plotting all results
if it > 1: # Closing the old files
plt.close("all")
plot.start_plotting(cfg, results, group="from_file")
if cfg.get("Output_folder", "") and cfg.get("Output_name", "") and cfg.get("Save_output", False):
if cfg["Output_name"] == "generic":
fileName = os.path.basename(os.path.splitext(run)[0])
else:
fileName = cfg["Output_name"]
save_all_plots(fileName, cfg["Output_folder"], dpi=300)
if cfg.get("Pickle_output", False):
save_dict(run_data.outputdata,
cfg["Output_folder"],
cfg["Output_name"],
cfg["Pickle_output"])
if args.show_plots and it==1:
plot.show_plots()
plt.close("all")
def __init__(self):
super().__init__()
uic.loadUi('data_scope.ui', self)
# Graph window
self.scope = self.graph.addPlot()
self.scope.setDownsampling(mode='peak')
# Scroll window
self.scroll = self.graphScroll.addPlot()
self.scroll.setDownsampling(mode='peak')
self.scroll.showAxis('bottom', False)
self.plot_count = config["max_signal_count"]
signals = config["signals"]
self.InputClass = config["InputClass"]
self.plot_data = [
PlotData(self.scope, self.scroll, signals[0]),
PlotData(self.scope, self.scroll, signals[1]),
PlotData(self.scope, self.scroll, signals[2]),
PlotData(self.scope, self.scroll, signals[3]),
]
# cross hair & Stats label
self.vLine = pg.InfiniteLine(angle=90, movable=False)
self.hLine = pg.InfiniteLine(angle=0, movable=False)
#self.scope.addItem(self.vLine, ignoreBounds = True)
#self.scope.addItem(self.hLine, ignoreBounds = True)
def mouseMoved(pos):
if self.crosshairs_Check.isChecked() == False:
return
if self.scope.sceneBoundingRect().contains(pos):
mousePoint = self.scope.vb.mapSceneToView(pos)
point = mousePoint.x()
debug("point = {}".format(point))
index = self.plot_data[0].index(point)
if index is not None:
txt = [
"<span style='background-color black'>{:1.3f} Sec.".
format(self.plot_data[0].tstamp[index])
]
for channel in range(self.plot_count):
if self.shown_channels & (1 << channel):
txt.append(
self.plot_data[channel].crosshair_val_text(
index))
debug(" ".join(txt))
self.stats_Label.setText(" ".join(txt))
self.vLine.setPos(mousePoint.x())
self.hLine.setPos(mousePoint.y())
self.scope.scene().sigMouseMoved.connect(mouseMoved)
self.crosshairs_Check.stateChanged.connect(
lambda: self.onCrosshairs_StateChange(self.crosshairs_Check))
self.crosshairs_Check.setChecked(config["xhairs_checked"])
# "Linear region" - selection in scroll window that controls viewing of main window
self.region = pg.LinearRegionItem([0, 1])
self.region.setZValue(-10)
self.scroll.addItem(self.region)
def updatePlot():
minX, maxX = self.region.getRegion()
self.scope.setXRange(minX, maxX, padding=0)
debug("updatePlot()")
def updateRegion():
x1, x2 = self.scope.getViewBox().viewRange()[0]
self.region.setRegion([x1, x2])
debug("updateRegion([{}, {}])".format(x1, x2))
self.region.sigRegionChanged.connect(updatePlot)
self.scope.sigXRangeChanged.connect(updateRegion)
updatePlot()
# Capture button features
# the capture button
self.capture_Button.setCheckable(True)
self.capture_Button.toggled.connect(self.onCaptureToggle)
# The capture timer processes self.update, which captures data.
self.capture_timer = pg.QtCore.QTimer()
self.capture_timer.timeout.connect(self.update)
# Port selection combobox features
self.ports = []
self.ComportCombo.addItems([a[0] for a in self.ports])
self.ComportCombo.currentIndexChanged.connect(self.com_port_changed)
self.selected_com_port = self.ComportCombo.currentText()
# a timer periodically updates the available ports
self.comport_timer = QtCore.QTimer()
self.comport_timer.timeout.connect(self.update_comports)
self.comport_timer.start(2000) # update every 2 seconds
self.update_comport_list() # start thread that updates the comports
# Misc static information
self.shown_channels = 0 # bitmask of channels that actually have data
def __init__(self, logfile_name, set_dir, dirs):
PlotData.__init__(self, logfile_name, set_dir, dirs)
class CuteLearning():
def __init__(self):
self.plot_data = PlotData()
self.cart = CartPoleEnv()
self.cart.reset()
self.predi_net = DQN()
self.updat_net = deepcopy(self.predi_net)
self.turn = 0
self.epidode = 0
self.epsilon = config.epsilon
self.eps_decay = 0.99
self.visu = False
self.visu_update = False #300
self.visu_window = 5
self.consecutive_wins = 0
self.best_consecutive_wins = 0
self.last_save = 0
self.memory = []
def reward_optimisation(self, state, end):
reward = -25 if end else 1
if reward == 1:
# Angle reward modification
angle_r = 0.418 / 2
reward += (((abs(angle_r - abs(state[2])) / angle_r) * 2) - 1) * 2
# Position reward modification
pos_r = 0.418 / 2
reward += (((abs(pos_r - abs(state[0])) / pos_r) * 2) - 1) * 2
return reward
def learn(self):
self.episode = 0
n = 0
while self.episode < 10:
self.turn = 0
end = False
states = []
targets = []
while not end:
# 1. Init
state = self.cart.state
# 2. Choose action
q_values = self.predi_net.predict(state).tolist()
a = choose_action_net(q_values, self.epsilon)
# 3. Perform action
next_state, _, end, _ = self.cart.step(a)
# 4. Measure reward
reward = self.reward_optimisation(next_state, end)
q_values_next = self.predi_net.predict(next_state)
# 5. Calcul Q-Values
q_values[a] = reward + net_config.gamma * \
torch.max(q_values_next).item()
self.turn += 1
self.memory.append((state, a, next_state, reward, end))
# self.updat_net.update(state, q_values)
states.append(state)
targets.append(q_values)
if (self.turn % 20 and self.turn) or end:
self.updat_net.update(states, targets)
states = []
targets = []
if self.turn >= 500:
end = True
if self.visu:
self.cart.render()
self.episode += 1
self.replay(20)
if self.episode % net_config.n_update == 0 and self.episode:
print("Update")
self.predi_net.model.load_state_dict(
self.updat_net.model.state_dict())
self.end()
n += 1
self.save()
self.cart.close()
self.plot_data.clear()
def replay(self, size):
if size > len(self.memory):
size = len(self.memory)
data = random.sample(self.memory, size)
states = []
targets = []
for state, action, next_state, reward, done in data:
q_values = self.predi_net.predict(state)
if done:
q_values[action] = reward
else:
# The only difference between the simple replay is in this line
# It ensures that next q values are predicted with the target network.
q_values_next = self.predi_net.predict(next_state)
q_values[action] = reward + net_config.gamma * torch.max(
q_values_next).item()
states.append(state)
targets.append(q_values)
self.updat_net.update(state, q_values)
def end(self):
self.plot_data.new_data(self.turn)
if self.turn > 195:
self.consecutive_wins += 1
if self.best_consecutive_wins < self.consecutive_wins:
self.best_consecutive_wins = self.consecutive_wins
if self.consecutive_wins > 200:
self.save()
print(("WIN IN " + str(self.episode) + " EPISODES\n") * 100)
else:
self.consecutive_wins = 0
if self.last_save * 1.2 < self.best_consecutive_wins and 50 <= self.best_consecutive_wins:
self.save()
self.last_save = self.best_consecutive_wins
print("Episode: ", self.episode, "\tTurn:", self.turn, "\tEpsilon:",
self.epsilon, "\tWins: ", "{:3}".format(self.consecutive_wins),
"/", self.best_consecutive_wins)
self.turn = 0
self.cart.reset()
if self.episode % config.graph_update == 0 and self.episode != 0:
self.plot_data.graph()
if self.visu_update:
if self.episode % self.visu_update == 0:
self.visu = True
if self.episode % self.visu_update == self.visu_window:
self.visu = False
self.cart.close()
self.epsilon = max(self.epsilon * self.eps_decay, 0.01)
def save(self):
pass
def __init__(self):
rospy.init_node('performance_tracker', anonymous=True)
# check arguments
self.run_name = rospy.get_param("~run_name", "run")
run_duration = rospy.get_param("~run_duration", 107)
self.sampling_time = rospy.get_param(
"~sampling_time", 0.04) # The loop sleeps for this long
min_buffer_size = run_duration / self.sampling_time # Minimum columsn for arrays containing periodically fetched data
self.start_time = 0
self.groundtruth_frame = rospy.get_param("~groundtruth_frame", "fox")
self.static_frame = rospy.get_param("~static_frame", "map")
self.estimate_frame = rospy.get_param("~estimate_frame", "imu")
self.groundtruth_topic = rospy.get_param("~groundtruth_topic", "")
self.broadcast_groundtruth = rospy.get_param("~broadcast_groundtruth",
False)
self.trigger_topic = rospy.get_param("~trigger_topic",
"/rovio/odometry")
#tf setup
self.tf_Buffer = tf.Buffer(rospy.Duration(10))
self.tf_listener = tf.TransformListener(self.tf_Buffer)
self.tf_broadcaster = tf.TransformBroadcaster()
self.tf_static_broadcaster = tf.StaticTransformBroadcaster()
rospy.on_shutdown(self.save_data_to_disk)
self.stampedTrajEstimate = PlotData(min_buffer_size,
self.sampling_time)
self.stampedGroundtruth = PlotData(min_buffer_size, self.sampling_time)
self.anchors = []
if self.broadcast_groundtruth:
rospy.loginfo(self.groundtruth_topic)
rospy.Subscriber(self.groundtruth_topic,
TransformStamped,
self.groundtruth_received,
queue_size=1)
rospy.Subscriber(self.trigger_topic,
Odometry,
self.trigger_est,
queue_size=1)
rospy.Subscriber(self.trigger_topic,
Odometry,
self.trigger_gro,
queue_size=1)
rospy.Subscriber("asa_ros/found_anchor",
FoundAnchor,
self.found_anchor,
queue_size=2)
rospy.Subscriber("asa_ros_0/found_anchor",
FoundAnchor,
self.found_anchor,
queue_size=2)
rospy.Subscriber("asa_ros_1/found_anchor",
FoundAnchor,
self.found_anchor,
queue_size=2)
rospy.Subscriber("asa_ros_2/found_anchor",
FoundAnchor,
self.found_anchor,
queue_size=2)
rospy.Subscriber("asa_ros_3/found_anchor",
FoundAnchor,
self.found_anchor,
queue_size=2)
self.count = 0
rospy.loginfo("Tracking as groundtruth: " + self.groundtruth_frame)
rospy.loginfo("Tracking as estimate: " + self.estimate_frame)
rospy.loginfo("Using : " + self.static_frame + " as a base")
self.spin()
class PerformanceTracker:
def __init__(self):
rospy.init_node('performance_tracker', anonymous=True)
# check arguments
self.run_name = rospy.get_param("~run_name", "run")
run_duration = rospy.get_param("~run_duration", 107)
self.sampling_time = rospy.get_param(
"~sampling_time", 0.04) # The loop sleeps for this long
min_buffer_size = run_duration / self.sampling_time # Minimum columsn for arrays containing periodically fetched data
self.start_time = 0
self.groundtruth_frame = rospy.get_param("~groundtruth_frame", "fox")
self.static_frame = rospy.get_param("~static_frame", "map")
self.estimate_frame = rospy.get_param("~estimate_frame", "imu")
self.groundtruth_topic = rospy.get_param("~groundtruth_topic", "")
self.broadcast_groundtruth = rospy.get_param("~broadcast_groundtruth",
False)
self.trigger_topic = rospy.get_param("~trigger_topic",
"/rovio/odometry")
#tf setup
self.tf_Buffer = tf.Buffer(rospy.Duration(10))
self.tf_listener = tf.TransformListener(self.tf_Buffer)
self.tf_broadcaster = tf.TransformBroadcaster()
self.tf_static_broadcaster = tf.StaticTransformBroadcaster()
rospy.on_shutdown(self.save_data_to_disk)
self.stampedTrajEstimate = PlotData(min_buffer_size,
self.sampling_time)
self.stampedGroundtruth = PlotData(min_buffer_size, self.sampling_time)
self.anchors = []
if self.broadcast_groundtruth:
rospy.loginfo(self.groundtruth_topic)
rospy.Subscriber(self.groundtruth_topic,
TransformStamped,
self.groundtruth_received,
queue_size=1)
rospy.Subscriber(self.trigger_topic,
Odometry,
self.trigger_est,
queue_size=1)
rospy.Subscriber(self.trigger_topic,
Odometry,
self.trigger_gro,
queue_size=1)
rospy.Subscriber("asa_ros/found_anchor",
FoundAnchor,
self.found_anchor,
queue_size=2)
rospy.Subscriber("asa_ros_0/found_anchor",
FoundAnchor,
self.found_anchor,
queue_size=2)
rospy.Subscriber("asa_ros_1/found_anchor",
FoundAnchor,
self.found_anchor,
queue_size=2)
rospy.Subscriber("asa_ros_2/found_anchor",
FoundAnchor,
self.found_anchor,
queue_size=2)
rospy.Subscriber("asa_ros_3/found_anchor",
FoundAnchor,
self.found_anchor,
queue_size=2)
self.count = 0
rospy.loginfo("Tracking as groundtruth: " + self.groundtruth_frame)
rospy.loginfo("Tracking as estimate: " + self.estimate_frame)
rospy.loginfo("Using : " + self.static_frame + " as a base")
self.spin()
def spin(self):
rospy.spin()
def stampToSecs(self, stamp):
t = rospy.Time(stamp.secs, stamp.nsecs)
return t.to_sec()
def stampToTime(self, stamp):
t = rospy.Time(stamp.secs, stamp.nsecs)
return t
def trigger_est(self, odom):
time = self.stampToTime(odom.header.stamp)
self.track_frame(self.estimate_frame, self.stampedTrajEstimate, time)
def trigger_gro(self, odom):
time = self.stampToTime(odom.header.stamp)
self.track_frame(self.groundtruth_frame, self.stampedGroundtruth, time)
# Query the frames and store their locations
def track_frames(self, time):
self.track_frame(self.estimate_frame, self.stampedTrajEstimate, time)
self.track_frame(self.groundtruth_frame, self.stampedGroundtruth, time)
def track_frame(self, frame_id, data_storage, time):
try:
tf = self.tf_Buffer.lookup_transform(self.static_frame, frame_id,
time, rospy.Duration(0.01))
except Exception as e:
print("Exception while looking up tfs in precision tracker")
print(e)
return
self.submit_transform_stamped(tf, data_storage, time)
# Add the data point to the specified storage
def submit_transform_stamped(self, transform, data_storage, time):
time = time.to_sec()
pos = transform.transform.translation
quat = transform.transform.rotation
data_storage.AddStampedDataPoint(
time, [pos.x, pos.y, pos.z, quat.x, quat.y, quat.z, quat.w])
# publish groundtruth as tf
def groundtruth_received(self, transform):
transform.header.frame_id = self.static_frame
self.tf_broadcaster.sendTransform(transform)
return
# we can republish a camera frame if we want to
irc = TransformStamped()
irc.header.stamp = transform.header.stamp
irc.header.frame_id = "fox"
irc.child_frame_id = "camera1"
irc.transform.translation.x = 0.0371982445732
irc.transform.translation.y = -0.0397392343472
irc.transform.translation.z = 0.0265641652917
irc.transform.rotation.x = 0.00424533187421
irc.transform.rotation.y = -0.0024078414729
irc.transform.rotation.z = -0.711402164283
irc.transform.rotation.w = 0.702768197993
self.tf_broadcaster.sendTransform(irc)
def found_anchor(self, foundAnchorMsg):
self.anchors.append(rospy.get_time())
#############################
# Wrapping up this test run #
#############################
# Saves all collected matrices and plots to the disk
def save_data_to_disk(self):
# Update the plot a last time
path = "/home/eric/statistics/" + self.run_name
try:
os.makedirs(path)
except:
rospy.loginfo("Folder already existed: " + path)
np.savetxt(path + "/stamped_traj_estimate.txt",
self.stampedTrajEstimate.GetAsNumpy(),
delimiter=" ")
np.savetxt(path + "/stamped_groundtruth.txt",
self.stampedGroundtruth.GetAsNumpy(),
delimiter=" ")
np.savetxt(path + "/anchors.txt",
np.array(self.anchors),
delimiter=" ")
class PerformanceTracker:
def __init__(self):
rospy.init_node('performance_tracker', anonymous=True)
# check arguments
self.rovio_ns = rospy.get_param("~rovio_ns", "")
self.run_name = rospy.get_param("~run_name", "run")
run_duration = rospy.get_param("~run_duration", 107)
self.sampling_time = rospy.get_param(
"~sampling_time", 0.1) # The loop sleeps for this long
min_buffer_size = run_duration / self.sampling_time # Minimum columsn for arrays containing periodically fetched data
self.start_time = 0
#tf setup
self.tf_Buffer = tf.Buffer(rospy.Duration(10))
self.tf_listener = tf.TransformListener(self.tf_Buffer)
self.tf_broadcaster = tf.TransformBroadcaster()
self.tf_static_broadcaster = tf.StaticTransformBroadcaster()
self.world_frame = "odom"
self.drone_frame = "imu"
self.vicon_body_frame = "vicon/imu"
# All subscribers
# Subscribers listening to the asa ros wrapper
rospy.Subscriber('/odometry', Odometry, self.odometry_update)
rospy.Subscriber('/fox/vrpn_client/estimated_transform',
TransformStamped,
self.groundtruth_update,
queue_size=1)
self.rovio_odom_sub = rospy.Subscriber('/rovio/odometry',
Odometry,
self.trajestimate_update,
queue_size=1)
rospy.Subscriber('/odometry_alt',
Odometry,
self.alt_trajestimate_update,
queue_size=1)
self.use_alternative_facts = False
rospy.on_shutdown(self.save_data_to_disk)
self.stampedTrajEstimate = PlotData(min_buffer_size,
self.sampling_time)
self.stampedGroundtruth = PlotData(min_buffer_size, self.sampling_time)
self.correctionData = PlotData(
rospy.get_param("~plot_buffer_anchor_correction", 100))
self.poseUpdateTimeStamps = []
rospy.loginfo("Precision tracker intitialized with:")
rospy.loginfo("Duration of Rosbag reported: " + str(run_duration) +
"s")
rospy.loginfo("Buffer size selected: " + str(min_buffer_size))
def spin(self):
rospy.spin()
#########
# Utils #
#########
def stampToSecs(self, stamp):
t = rospy.Time(stamp.secs, stamp.nsecs)
return t.to_sec()
#############################
# Recording external events #
#############################
# Update from vicon
def groundtruth_update(self, stampedTransform):
time = self.stampToSecs(stampedTransform.header.stamp)
# add to tf
stampedTransform.header.frame_id = "map"
self.tf_broadcaster.sendTransform(stampedTransform)
pos = stampedTransform.transform.translation
quat = stampedTransform.transform.rotation
self.stampedGroundtruth.AddStampedDataPoint(
time, [pos.x, pos.y, pos.z, quat.x, quat.y, quat.z, quat.w])
# Trajectory update from rovio
def trajestimate_update(self, odometry):
if self.use_alternative_facts == False:
self.submit_odometry_point(odometry)
# Trajectory update from rovio
def alt_trajestimate_update(self, odometry):
# stop the normal subscriber since we are receiving alternative facts
# self.rovio_odom_sub.unregsiter() unregistering is not working... wtf.
# send our alternative facts to the normal tracking pipeline
self.use_alternative_facts = True
self.submit_odometry_point(odometry)
# time = self.stampToSecs(odometry.header.stamp)
# error = self.tf_Buffer.lookup_transform("fox", "imu_alt", rospy.Time(time), rospy.Duration(0.1)).transform.translation
# error2 = self.tf_Buffer.lookup_transform("fox", "imu", rospy.Time(time), rospy.Duration(0.1)).transform.translation
# magn = (error.x**2 + error.y**2 + error.z**2)**0.5
# magn2 = (error2.x**2 + error2.y**2 + error2.z**2)**0.5
# print("%.3f vs %.3f" % (magn, magn2))
def submit_odometry_point(self, odometry):
time = self.stampToSecs(odometry.header.stamp)
pos = odometry.pose.pose.position
quat = odometry.pose.pose.orientation
self.stampedTrajEstimate.AddStampedDataPoint(
time, [pos.x, pos.y, pos.z, quat.x, quat.y, quat.z, quat.w])
# Called whenever the drift compensator sends a correction signal
def odometry_update(self, poseUpdateOdom):
time = rospy.Time(poseUpdateOdom.header.stamp.secs,
poseUpdateOdom.header.stamp.nsecs)
msg_time = time.to_sec()
try:
trans = self.tf_Buffer.lookup_transform(self.world_frame, "imu",
time, rospy.Duration(1.0))
currentDroneVector = [
trans.transform.translation.x, trans.transform.translation.y,
trans.transform.translation.z
]
except:
currentDroneVector = np.array([0, 0, 0])
newPosUpdate = np.array([
poseUpdateOdom.pose.pose.position.x,
poseUpdateOdom.pose.pose.position.y,
poseUpdateOdom.pose.pose.position.z
])
delta = np.linalg.norm(newPosUpdate - currentDroneVector)
# Track the magnitude of the correction that had to be performed
self.correctionData.AddDataPoint([msg_time, delta])
# Track the timestmap of the pose update for the plots
self.poseUpdateTimeStamps.append(msg_time)
#############################
# Wrapping up this test run #
#############################
# Saves all collected matrices and plots to the disk
def save_data_to_disk(self):
# Update the plot a last time
path = "/home/eric/statistics/" + self.run_name
try:
os.makedirs(path)
except:
rospy.loginfo("Folder already existed: " + path)
np.savetxt(path + "/stamped_traj_estimate.txt",
self.stampedTrajEstimate.GetAsNumpy(),
delimiter=" ")
np.savetxt(path + "/stamped_groundtruth.txt",
self.stampedGroundtruth.GetAsNumpy(),
delimiter=" ")
np.savetxt(path + "/correctionData.txt",
self.correctionData.GetAsNumpy(),
delimiter=" ")
np.savetxt(path + "/poseUpdateTimeStamps.txt",
np.array(self.poseUpdateTimeStamps),
delimiter=" ")