diff --git a/extra/nt-bridge/.gitignore b/extra/nt-bridge/.gitignore
index 67e2f0b..4e10d56 100644
--- a/extra/nt-bridge/.gitignore
+++ b/extra/nt-bridge/.gitignore
@@ -1,2 +1,6 @@
 cmake-build/**
-cmake-install/**
\ No newline at end of file
+cmake-install/**
+build/**
+install/**
+log/**
+.vscode/**
\ No newline at end of file
diff --git a/extra/nt-bridge/CMakeLists.txt b/extra/nt-bridge/CMakeLists.txt
index 609854d..594541d 100644
--- a/extra/nt-bridge/CMakeLists.txt
+++ b/extra/nt-bridge/CMakeLists.txt
@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.12)
-project(nt-bridge)
+project(nt_bridge)
 
 if(NOT MSVC)
 	if(NOT CMAKE_BUILD_TYPE)
@@ -8,6 +8,8 @@ if(NOT MSVC)
 	endif()
 endif()
 
+set(CMAKE_CXX_STANDARD 20)
+
 message(STATUS "[NT BRIDGE]: Configuring protobuf...")
 execute_process(
 	COMMAND cmake
@@ -52,3 +54,29 @@ set(Protobuf_PROTOC_EXECUTABLE	"${PROJECT_SOURCE_DIR}/protobuf/cmake-install/bin
 list(APPEND CMAKE_LIBRARY_PATH	"${PROJECT_SOURCE_DIR}/protobuf/cmake-install/lib")
 add_subdirectory(allwpilib)
 set(BUILD_SHARED_LIBS ${__BUILD_SHARED_LIBS})
+
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+
+add_executable(nt_bridge_node "src/main.cpp")
+target_link_libraries(nt_bridge_node
+	wpiutil
+	wpinet
+	wpimath
+	ntcore
+)
+
+ament_target_dependencies(nt_bridge_node
+	"rclcpp"
+	"nav_msgs"
+	"sensor_msgs"
+	"geometry_msgs"
+)
+install(
+	TARGETS nt_bridge_node
+	DESTINATION lib/${PROJECT_NAME}/)
+ament_package()
diff --git a/extra/nt-bridge/package.xml b/extra/nt-bridge/package.xml
new file mode 100644
index 0000000..b26a507
--- /dev/null
+++ b/extra/nt-bridge/package.xml
@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>nt_bridge</name>
+  <version>0.1.0</version>
+  <description>Networktable to ROS bridge for UESim</description>
+  <maintainer email="samrichter888@gmail.com">sigma</maintainer>
+  <license>MIT</license>
+
+  <depend>rclcpp</depend>
+  <depend>sensor_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>nav_msgs</depend>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <!-- <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend> -->
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/extra/nt-bridge/src/main.cpp b/extra/nt-bridge/src/main.cpp
index e69de29..34a7379 100644
--- a/extra/nt-bridge/src/main.cpp
+++ b/extra/nt-bridge/src/main.cpp
@@ -0,0 +1,154 @@
+#include <functional>
+#include <memory>
+#include <span>
+
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <nav_msgs/msg/occupancy_grid.hpp>
+
+#include <networktables/NetworkTableInstance.h>
+#include <networktables/NetworkTable.h>
+#include <networktables/FloatArrayTopic.h>
+#include <networktables/RawTopic.h>
+
+
+class NTBridgeNode : public rclcpp::Node {
+public:
+	NTBridgeNode(
+		bool use_nt_client = false,
+		int nt_client_team = -1,
+		const char* nt_client_server = "localhost",
+		unsigned int nt_client_port = 0
+	) : Node("nt_bridge_node") {
+		nt::NetworkTableInstance inst = nt::NetworkTableInstance::GetDefault();
+
+		bool using_client = false;
+		if(use_nt_client) {
+			if(nt_client_team >= 0) {
+				inst.StartClient4("nt_bridge_node");
+				inst.SetServerTeam(
+					nt_client_team,
+					nt_client_port
+				);
+				using_client = true;
+				RCLCPP_INFO(this->get_logger(), "Started NT client using team number: %d", nt_client_team);
+			}
+			if(nt_client_server != nullptr) {
+				inst.StartClient4("nt_bridge_node");
+				inst.SetServer(
+					nt_client_server,
+					nt_client_port
+				);
+				using_client = true;
+				RCLCPP_INFO(this->get_logger(), "Started NT client using server: %s", nt_client_server);
+			}
+		}
+		if(!using_client) {
+			inst.StartServer();
+			RCLCPP_INFO(this->get_logger(), "Started NT server.");
+		}
+
+		this->nt_scan_sub = inst.GetRawTopic("uesim/scan").Subscribe( "PointXYZ_[]", {} );
+		this->nt_pose_sub = inst.GetFloatArrayTopic("uesim/pose").Subscribe( {} );
+
+		inst.AddListener(this->nt_scan_sub, nt::EventFlags::kValueAll, std::bind(&NTBridgeNode::nt_scan_cb, this, std::placeholders::_1));
+		inst.AddListener(this->nt_pose_sub, nt::EventFlags::kValueAll, std::bind(&NTBridgeNode::nt_pose_cb, this, std::placeholders::_1));
+
+		this->scan_pub = this->create_publisher<sensor_msgs::msg::PointCloud2>("/uesim/scan", 1);
+		this->pose_pub = this->create_publisher<geometry_msgs::msg::PoseStamped>("/uesim/pose", 1);
+	}
+	~NTBridgeNode() = default;
+
+protected:
+	void nt_scan_cb(const nt::Event& event) {
+		const nt::Value& val = event.GetValueEventData()->value;
+		if( val.IsRaw() ) {
+			std::span<const uint8_t> data = val.GetRaw();
+			const size_t
+				nelems = data.size() / 16,
+				nbytes = nelems * 16;
+
+			sensor_msgs::msg::PointCloud2 cloud;
+			cloud.data.resize(nbytes);
+			memcpy(cloud.data.data(), data.data(), nbytes);
+
+			cloud.width = nelems;
+			cloud.height = 1;
+			cloud.is_bigendian = false;
+			cloud.point_step = 16;
+			cloud.row_step = nbytes;
+			cloud.is_dense = true;
+			cloud.header.frame_id = "1";
+
+			cloud.fields.resize(3);
+			cloud.fields[0].name = "x";
+			cloud.fields[0].offset = 0;
+			cloud.fields[0].datatype = sensor_msgs::msg::PointField::FLOAT32;
+			cloud.fields[0].count = 1;
+			cloud.fields[1].name = "y";
+			cloud.fields[1].offset = 4;
+			cloud.fields[1].datatype = sensor_msgs::msg::PointField::FLOAT32;
+			cloud.fields[1].count = 1;
+			cloud.fields[2].name = "z";
+			cloud.fields[2].offset = 8;
+			cloud.fields[2].datatype = sensor_msgs::msg::PointField::FLOAT32;
+			cloud.fields[2].count = 1;
+
+			cloud.header.stamp.sec = static_cast<int32_t>( val.time() / 1000000L );
+			cloud.header.stamp.nanosec = static_cast<uint32_t>( val.time() % 1000000L ) * 1000U;
+
+			this->scan_pub->publish(cloud);
+
+			RCLCPP_INFO(this->get_logger(), "Published Scan!");
+		}
+	}
+	void nt_pose_cb(const nt::Event& event) {
+		const nt::Value& val = event.GetValueEventData()->value;
+		if( val.IsFloatArray() ) {
+			std::span<const float> data = val.GetFloatArray();
+
+			if(data.size() >= 7) {
+				geometry_msgs::msg::PoseStamped pose;
+				
+				pose.pose.position.x = static_cast<double>( data[0] );
+				pose.pose.position.y = static_cast<double>( data[1] );
+				pose.pose.position.z = static_cast<double>( data[2] );
+
+				pose.pose.orientation.w = static_cast<double>( data[3] );
+				pose.pose.orientation.x = static_cast<double>( data[4] );
+				pose.pose.orientation.y = static_cast<double>( data[5] );
+				pose.pose.orientation.z = static_cast<double>( data[6] );
+
+				pose.header.stamp.sec = static_cast<int32_t>( val.time() / 1000000L );
+				pose.header.stamp.nanosec = static_cast<uint32_t>( val.time() % 1000000L ) * 1000U;
+
+				this->pose_pub->publish(pose);
+
+				RCLCPP_INFO(this->get_logger(), "Published Pose!");
+			}
+		}
+	}
+
+
+protected:
+	nt::RawSubscriber nt_scan_sub;
+	nt::FloatArraySubscriber nt_pose_sub;
+
+	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr scan_pub;
+	rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pose_pub;
+
+	// rclcpp::Subscription<nav_msgs::msg::OccupancyGrid> grid_sub;
+
+};
+
+
+int main(int argc, char** argv) {
+
+	rclcpp::init(argc, argv);
+	rclcpp::spin(std::make_shared<NTBridgeNode>());
+	rclcpp::shutdown();
+
+	return 0;
+
+}
diff --git a/src/ldrp/perception.cpp b/src/ldrp/perception.cpp
index 113937b..017190a 100644
--- a/src/ldrp/perception.cpp
+++ b/src/ldrp/perception.cpp
@@ -66,17 +66,49 @@ namespace util {
 		return (static_cast<int64_t>(seconds) * 1000000L) + (static_cast<int64_t>(nanoseconds) / 1000L);
 	}
 
+
+	void _test(rclcpp::Node* node) {
+		pcl::PointCloud<pcl::PointXYZ> _c;
+		sensor_msgs::msg::PointCloud2 _c2;
+		pcl::toROSMsg(_c, _c2);
+		RCLCPP_INFO(node->get_logger(),
+			"\n-------------------------------- TESTING --------------------------------"
+			"\nPointXYZ \"frame_id\": %s"
+			"\nPointCloud2 total fields: %ld"
+			"\nPointCloud2 field #1: { name: %s, offset: %d, datatype: %d, count: %d }"
+			"\nPointCloud2 field #2: { name: %s, offset: %d, datatype: %d, count: %d }"
+			"\nPointCloud2 field #3: { name: %s, offset: %d, datatype: %d, count: %d }"
+			"\n-------------------------------------------------------------------------",
+			_c.header.frame_id.c_str(),
+			_c2.fields.size(),
+			_c2.fields[0].name.c_str(),
+			_c2.fields[0].offset,
+			_c2.fields[0].datatype,
+			_c2.fields[0].count,
+			_c2.fields[1].name.c_str(),
+			_c2.fields[1].offset,
+			_c2.fields[1].datatype,
+			_c2.fields[1].count,
+			_c2.fields[2].name.c_str(),
+			_c2.fields[2].offset,
+			_c2.fields[2].datatype,
+			_c2.fields[2].count
+		);
+	}
+
 };
 
 
 PerceptionNode::PerceptionNode() : rclcpp::Node("perception_node") {
 	RCLCPP_INFO(this->get_logger(), "Perception Node Initialization!");
 	// setup subs/pubs
-	this->scan_sub = this->create_subscription<sensor_msgs::msg::PointCloud2>("/cloud_all_fields_fullframe", 1,
+	this->scan_sub = this->create_subscription<sensor_msgs::msg::PointCloud2>("/uesim/scan", 1,
 		std::bind(&PerceptionNode::scan_cb, this, std::placeholders::_1));
-	this->pose_sub = this->create_subscription<geometry_msgs::msg::PoseStamped>("/dlio/odom_node/pose", 1,
+	this->pose_sub = this->create_subscription<geometry_msgs::msg::PoseStamped>("/uesim/pose", 1,
 		std::bind(&PerceptionNode::pose_cb, this, std::placeholders::_1));
-	this->grid_pub = this->create_publisher<nav_msgs::msg::OccupancyGrid>("/obstacle_grid", 1);
+	this->grid_pub = this->create_publisher<nav_msgs::msg::OccupancyGrid>("/ldrp/obstacle_grid", 1);
+
+	// util::_test(this);
 
 	//get parameters
 	util::declare_param(this, "map_resolution_cm", this->_config.map_resolution_cm, this->_config.map_resolution_cm);
@@ -132,57 +164,88 @@ PerceptionNode::~PerceptionNode() {}
 
 
 void PerceptionNode::scan_cb(const sensor_msgs::msg::PointCloud2::ConstSharedPtr& scan) {
-
+	// this->sampler_mutex.lock();
 	RCLCPP_INFO(this->get_logger(), "Scan callback called!");
 
 	pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud = std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
 	pcl::fromROSMsg(*scan, *point_cloud);
 
 	const int64_t ts = util::constructTimestampMicros<uint32_t>(scan->header.stamp.sec, scan->header.stamp.nanosec);
+	this->sampler_mutex.lock();
 	this->scan_sampler.insert( ts, point_cloud );
 	this->scan_sampler.updateMin(ts - util::floatSecondsToIntMicros(this->_config.scan_matching_history_range_s));
+	const size_t current_nsamples = this->scan_sampler.getSamples().size();
+	this->sampler_mutex.unlock();
 
 	RCLCPP_INFO(this->get_logger(),
-		"\nScan sample timestamp (ms): %ld"
-		"\nTotal scan samples: %ld",
+		"\n\tScan sample timestamp (ms): %ld"
+		"\n\tTotal scan samples: %ld",
 		ts,
-		this->scan_sampler.getSamples().size()
+		current_nsamples
 	);
+	// this->sampler_mutex.unlock();
 
 }
 
 
 void PerceptionNode::pose_cb(const geometry_msgs::msg::PoseStamped::ConstSharedPtr& pose) {
-	RCLCPP_DEBUG(this->get_logger(), "Transform callback called!");
+	// this->sampler_mutex.lock();
+	RCLCPP_INFO(this->get_logger(), "Transform callback called!");
 
 	const int64_t trfm_target_ts = util::constructTimestampMicros<uint32_t>(pose->header.stamp.sec, pose->header.stamp.nanosec);
+	this->sampler_mutex.lock();
 	const typename decltype(this->scan_sampler)::ElemT* sample = this->scan_sampler.sampleTimestamped(trfm_target_ts);
+	this->sampler_mutex.unlock();
 	
 	// if scan doesn't exist
-	if(!sample) return;
+	if(!sample) {
+		RCLCPP_INFO(this->get_logger(),
+			"Invalid sample recieved!"
+			"\n\tTransform timestamp (ms): %ld",
+			trfm_target_ts
+		);
+		// this->sampler_mutex.unlock();
+		return;
+	}
 
 	// transform age - scan age
 	const int64_t ts_diff = trfm_target_ts - sample->first;
 
 	// if scan came after the transform do not use it
-	if(ts_diff < 0) return;
+	// if(ts_diff < 0) {
+	// 	RCLCPP_INFO(this->get_logger(),
+	// 		"Invalid timestamp delta!"
+	// 		"\n\tTransform timestamp (ms): %ld"
+	// 		"\n\tSampled timestamp (ms): %ld"
+	// 		"\n\tTimestamp difference (ms): %ld",
+	// 		trfm_target_ts,
+	// 		sample->first,
+	// 		ts_diff
+	// 	);
+	// 	this->sampler_mutex.unlock();
+	// 	return;
+	// }
 
 	// valid case to use transform with scan
-	else if(ts_diff < this->_config.scan_matching_history_range_s) {
+	if(abs(ts_diff) < util::floatSecondsToIntMicros(this->_config.scan_matching_history_range_s)) {	// config for 'epsilon'
 
 		pcl::PointCloud<pcl::PointXYZ>::Ptr scan = sample->second;
+		this->sampler_mutex.lock();
 		this->scan_sampler.updateMin(sample->first + 1);	// don't use 'sample' after here since it is invalid
+		const size_t current_nsamples = this->scan_sampler.getSamples().size();
+		this->sampler_mutex.unlock();
 
 		RCLCPP_INFO(this->get_logger(),
-			"\nTransform timestamp (ms): %ld"
-			"\nSampled timestamp (ms): %ld"
-			"\nTimestamp difference (ms): %ld"
-			"\nTotal scan samples: %ld",
+			"\n\tTransform timestamp (ms): %ld"
+			"\n\tSampled timestamp (ms): %ld"
+			"\n\tTimestamp difference (ms): %ld"
+			"\n\tTotal scan samples: %ld",
 			trfm_target_ts,
 			sample->first,
 			ts_diff,
-			this->scan_sampler.getSamples().size()
+			current_nsamples
 		);
+		// this->sampler_mutex.unlock();
 
 		const Eigen::Quaternionf quat{
 			static_cast<float>(pose->pose.orientation.w),
@@ -204,6 +267,7 @@ void PerceptionNode::pose_cb(const geometry_msgs::msg::PoseStamped::ConstSharedP
 		this->process_and_export(*scan, *reinterpret_cast<const Eigen::Vector3f*>(&pos));
 
 	}
+	// this->sampler_mutex.unlock();
 
 }
 
@@ -344,4 +408,17 @@ void PerceptionNode::process_and_export(const pcl::PointCloud<pcl::PointXYZ>& cl
 
 	this->grid_pub->publish(out_grid);
 
+	RCLCPP_INFO(this->get_logger(),
+		"Grid Updated!"
+		"\n\tOrigin: (%f, %f)"
+		"\n\tDims: (%d, %d)"
+		"\n\tSize: (%f, %f)",
+		_origin.x(),
+		_origin.y(),
+		_grid_size.x(),
+		_grid_size.y(),
+		_grid_size.x() * this->accumulator.cellRes(),
+		_grid_size.y() * this->accumulator.cellRes()
+	);
+
 }
diff --git a/src/ldrp/perception.hpp b/src/ldrp/perception.hpp
index d995171..47e3c0c 100644
--- a/src/ldrp/perception.hpp
+++ b/src/ldrp/perception.hpp
@@ -29,6 +29,8 @@
 #include "./grid.hpp"
 #include "./timestamp_sampler.hpp"
 
+#include <mutex>
+
 #include <Eigen/Core>
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
@@ -62,12 +64,14 @@ class PerceptionNode : public rclcpp::Node {
 	TimestampSampler<pcl::PointCloud<pcl::PointXYZ>::Ptr, int64_t> scan_sampler{};
 	QuantizedRatioGrid<uint8_t, float> accumulator{};
 
+	std::mutex sampler_mutex{};
+
 	struct {
 
 		// bool
 		// 	scan_matching_skip_invalid = false;
 		double
-			scan_matching_history_range_s = 0.25,
+			scan_matching_history_range_s = 1.0,
 			map_resolution_cm		= 5.,
 			pmf_max_range_cm		= 250.,
 			max_z_thresh_cm			= 100.,
diff --git a/src/ldrp/timestamp_sampler.hpp b/src/ldrp/timestamp_sampler.hpp
index dc3d9c6..27b3a4e 100644
--- a/src/ldrp/timestamp_sampler.hpp
+++ b/src/ldrp/timestamp_sampler.hpp
@@ -35,25 +35,10 @@ class TimestampSampler {
 
 	/** Add a new sample to the buffer */
 	inline void insert(TimeT time, const Type& sample)
-		{ this->insert(time, sample); }
+		{ this->_insert(time, Type{ sample }); }
 	/** Add a new sample to the buffer */
-	void insert(TimeT time, Type&& sample) {
-		if(this->samples.size() <= 0 || time > this->samples.back().first) {
-			this->samples.emplace_back(time, std::forward<Type>(sample));
-		} else {
-			auto after = std::upper_bound(this->samples.begin(), this->samples.end(), time, &This_T::t_less__);
-			auto before = after - 1;
-			if(after == this->samples.begin()) {
-				this->samples.insert(after, std::pair{time, std::forward<Type>(sample)});
-			} else {
-				if(before == this->samples.begin() || before->first < time) {
-					this->samples.insert(after, std::pair{time, std::forward<Type>(sample)});
-				} else {
-					before->second = sample;
-				}
-			}
-		}
-	}
+	inline void insert(TimeT time, Type&& sample)
+		{ this->_insert(time, sample); }
 
 	/** Erase all the elements */
 	inline void clear() { this->samples.clear(); }
@@ -82,6 +67,24 @@ class TimestampSampler {
 
 
 protected:
+	void _insert(TimeT time, Type&& sample) {
+		if(this->samples.size() <= 0 || time > this->samples.back().first) {
+			this->samples.emplace_back(time, std::forward<Type>(sample));
+		} else {
+			auto after = std::upper_bound(this->samples.begin(), this->samples.end(), time, &This_T::t_less__);
+			auto before = after - 1;
+			if(after == this->samples.begin()) {
+				this->samples.insert(after, std::pair{time, std::forward<Type>(sample)});
+			} else {
+				if(before == this->samples.begin() || before->first < time) {
+					this->samples.insert(after, std::pair{time, std::forward<Type>(sample)});
+				} else {
+					before->second = sample;
+				}
+			}
+		}
+	}
+
 	void enforceBound() {
 		auto last = this->samples.begin();
 		for(; last < this->samples.end() && last->first < this->absolute_bound; last++);