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=" ")