diff --git a/autonav_ws/src/autonav_hardware/CMakeLists.txt b/autonav_ws/src/autonav_hardware/CMakeLists.txt
new file mode 100644
index 0000000..7762dc0
--- /dev/null
+++ b/autonav_ws/src/autonav_hardware/CMakeLists.txt
@@ -0,0 +1,66 @@
+cmake_minimum_required(VERSION 3.8)
+
+project(autonav_hardware)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclpy REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+# msgs and srvs 
+#rosidl_generate_interfaces(${PROJECT_NAME}
+  # add message types here
+  #"path_to_message/message.msg"
+#)
+
+# C++
+
+# Inlcude Cpp "include" directory
+include_directories(include)
+
+# Create Cpp executables
+#add_executable(executable_name path_to_executable/executable.cpp)
+#ament_target_dependencies(executable_name rclcpp other_dependencies)
+
+# Install Cpp executables
+install(TARGETS
+  # install executables by name
+  # executable_name
+  DESTINATION lib/${PROJECT_NAME}
+ )
+ 
+# Python
+
+ # Use only if not using rosidl_generate_interfaces
+ # Install Python modules
+ #ament_python_install_package(${PROJECT_NAME})
+ 
+ # Install Python programs
+ install(PROGRAMS
+   # add programs in format:
+   src/can_node.py
+   DESTINATION lib/${PROJECT_NAME}
+)
+
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()
\ No newline at end of file
diff --git a/autonav_ws/src/autonav_hardware/LICENSE b/autonav_ws/src/autonav_hardware/LICENSE
new file mode 100644
index 0000000..30e8e2e
--- /dev/null
+++ b/autonav_ws/src/autonav_hardware/LICENSE
@@ -0,0 +1,17 @@
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+THE SOFTWARE.
diff --git a/autonav_ws/src/autonav_hardware/package.xml b/autonav_ws/src/autonav_hardware/package.xml
new file mode 100644
index 0000000..0ac7cf0
--- /dev/null
+++ b/autonav_ws/src/autonav_hardware/package.xml
@@ -0,0 +1,18 @@
+<?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>autonav_hardware</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="antoniochappell@protonmail.com">tony</maintainer>
+  <license>MIT</license>
+
+  <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/autonav_ws/src/autonav_hardware/src/can_node.py b/autonav_ws/src/autonav_hardware/src/can_node.py
new file mode 100644
index 0000000..d91d4d9
--- /dev/null
+++ b/autonav_ws/src/autonav_hardware/src/can_node.py
@@ -0,0 +1,296 @@
+#!/usr/bin/env python3
+
+import rclpy
+from autonav_shared.node import Node
+from autonav_msgs.msg import MotorInput, MotorFeedback, SafetyLights, Conbus
+from autonav_msgs.msg import LinearPIDStatistics, AngularPIDStatistics, MotorStatisticsFrontMotors, MotorStatisticsBackMotors
+from autonav_shared.types import LogLevel, DeviceState, SystemState
+import can
+import threading
+import struct
+from ctypes import Structure, c_bool, c_uint8
+
+arbitration_ids = {
+    "EStop": 0,
+    "MobilityStop": 1,
+    "MobilityStart": 9,
+    "MotorsCommand": 10,
+    "SafetyLightsCommand": 13,
+    "OdometryFeedback": 14,
+    "ObjectDetection": 20,
+    "LinearPIDStatistics": 50,
+    "AngularPIDStatistics": 51,
+    "MotorStatisticsFrontMotors": 52,
+    "MotorStatisticsBackMotors": 53,
+    "ConbusLowerBound": 1000,
+    "ConbusUpperBound": 1400
+}
+
+class SafetyLightsPacket(Structure):
+    _fields_ = [
+        ("autonomous", c_bool, 1),
+        ("mode", c_uint8, 7),
+        ("brightness", c_uint8, 8),
+        ("red", c_uint8, 8),
+        ("green", c_uint8, 8),
+        ("blue", c_uint8, 8),
+        ("blink_period", c_uint8, 8)
+    ]
+
+class CanConfig:
+    def __init__(self):
+        self.odom_feedback_scaler = 10000
+        self.linear_pid_scaling_factor = 1000
+        self.angular_pid_scaling_factor = 1000
+
+
+class CanNode(Node):
+    def __init__(self):
+        super().__init__("CAN_node")
+        self.write_config(CanConfig())
+
+        # can
+        self.can = None
+
+        # safety lights
+        self.safetyLightsSubscriber = self.create_subscription(
+            SafetyLights,
+            "/autonav/safety_lights",
+            self.on_safety_lights_received,
+            20
+        )
+
+        # motor messages
+        self.motorInputSubscriber = self.create_subscription(
+            MotorInput,
+            "/autonav/motor_input",
+            self.on_motor_input_received,
+            20
+        )
+
+        self.motorFeedbackPublisher = self.create_publisher(
+            MotorFeedback,
+            "/autonav/motor_feedback", 
+            20
+        )
+
+        # conbus
+        self.conbusSubscriber = self.create_subscription(
+            Conbus,
+            "/autonav/conbus/instruction", 
+            self.on_conbus_received, 
+            20
+        )
+
+        self.conbusPublisher = self.create_publisher(
+            Conbus, 
+            "/autonav/conbus/data", 
+            20
+        )
+
+        # motor statistics and PID tuning
+        self.linearPIDStatisticsPublisher = self.create_publisher(
+            LinearPIDStatistics,
+            "/autonav/linear_pid_statistics",
+            20
+        )
+        self.angularPIDStatisticsPublisher = self.create_publisher(
+            AngularPIDStatistics,
+            "/autonav/angular_pid_statistics",
+            20
+        )
+        self.motorStatisticsFrontMotorsPublisher = self.create_publisher(
+            MotorStatisticsFrontMotors,
+            "/autonav/motor_statistics_front_motors",
+            20
+        )
+        self.motorStatisticsBackMotorsPublisher = self.create_publisher(
+            MotorStatisticsBackMotors,
+            "/autonav/motor_statistics_back_motors",
+            20
+        )
+
+
+    def init(self):
+        # can threading
+        self.canTimer = self.create_timer(0.5, self.can_worker)
+        self.canReadThread = threading.Thread(target=self.can_thread_worker)
+        self.canReadThread.daemon = True
+
+
+    def can_worker(self):
+        try:
+            with open("/dev/ttyACM0", "r") as f:
+                pass
+
+            if self.can is not None:
+                return
+
+            self.can = can.ThreadSafeBus(
+                bustype="slcan", channel="/dev/ttyACM0", bitrate=100000)
+            self.set_device_state(DeviceState.OPERATING)
+        except:
+            if self.can is not None:
+                self.can = None
+
+            if self.get_device_state() != DeviceState.WARMING:
+                self.set_device_state(DeviceState.WARMING)
+
+
+    def can_thread_worker(self):
+        while rclpy.ok():
+            if self.get_device_state() != DeviceState.READY and self.get_device_state() != DeviceState.OPERATING:
+                continue
+            if self.can is not None:
+                try:
+                    msg = self.can.recv(timeout=0.01)
+                    if msg is not None:
+                        self.onCanMessageReceived(msg)
+                except can.CanError:
+                    pass
+    
+
+    def onCanMessageReceived(self, msg):
+        arbitration_id = msg.arbitration_id
+        if arbitration_id == arbitration_ids["Estop"]:
+            self.set_mobility(False)
+
+        if arbitration_id == arbitration_ids["MobilityStart"]:
+            self.set_mobility(True)
+        
+        if arbitration_id == arbitration_ids["MobilityStop"]:
+            self.set_mobility(False)
+
+        if arbitration_id == arbitration_ids["OdometryFeedback"]:
+            self.publish_odom_feedback(msg)
+
+        if arbitration_id == arbitration_ids["LinearPIDStatistics"]:
+            self.publish_linear_pid_statistics(msg)
+    
+        if arbitration_id == arbitration_ids["AngularPIDStatistics"]:
+            self.publish_angular_pid_statistics(msg)
+
+        if arbitration_id == arbitration_ids["MotorStatisticsFrontMotors"]:
+            self.publish_motor_statistics_front_motors(msg)
+        
+        if arbitration_id == arbitration_ids["MotorStatisticsBackMotors"]:
+            self.publish_motor_statistics_back_motors(msg)
+
+        if arbitration_id >= arbitration_ids["ConbusLowerBound"] and arbitration_id < arbitration_ids["ConbusUpperBound"]:
+            self.publish_conbus(msg)
+
+
+    def publish_odom_feedback(self, msg):
+        delta_x, delta_y, delta_theta = struct.unpack('hhh', msg.data)
+        motor_feedback_msg = MotorFeedback()
+        motor_feedback_msg.delta_x = delta_x / self.config.get("odom_feedback_scaler")
+        motor_feedback_msg.delta_y = delta_y / self.config.get("odom_feedback_scaler")
+        motor_feedback_msg.delta_theta = delta_theta / self.config.get("odom_feedback_scaler")
+
+        self.motorFeedbackPublisher.publish(motor_feedback_msg)
+
+
+    def publish_linear_pid_statistics(self, msg):
+        forward_velocity, forward_velocity_setpoint, sideways_velocity, sideways_velocity_setpoint = struct.unpack("hhhh", msg.data)
+        linear_pid_statistics_msg = LinearPIDStatistics()
+        linear_pid_statistics_msg.forward_velocity = forward_velocity
+        linear_pid_statistics_msg.forward_velocity_setpoint - forward_velocity_setpoint
+        linear_pid_statistics_msg.sideways_velocity = sideways_velocity
+        linear_pid_statistics_msg.sideways_velocity_setpoint = sideways_velocity_setpoint
+
+        self.linearPIDStatisticsPublisher.publish(linear_pid_statistics_msg)
+
+
+    def publish_angular_pid_statistics(self, msg):
+        angular_velocity, angular_velocity_setpoint = struct.unpack("hh", msg.data)
+        angular_pid_statistics_msg = AngularPIDStatistics()
+        angular_pid_statistics_msg.angular_velocity = angular_velocity
+        angular_pid_statistics_msg.angular_velocity_setpoint = angular_velocity_setpoint
+
+        self.angularPIDStatisticsPublisher.publish(angular_pid_statistics_msg)
+
+
+    def publish_motor_statistics_front_motors(self, msg):
+        left_drive_motor_output, left_steer_motor_output, right_drive_motor_output, right_steer_motor_output= struct.unpack("hh", msg.data)
+        motor_statistics_front_motors = MotorStatisticsFrontMotors()
+        motor_statistics_front_motors.left_drive_motor_output = left_drive_motor_output
+        motor_statistics_front_motors.left_steer_motor_output = left_steer_motor_output
+        motor_statistics_front_motors.right_drive_motor_output = right_drive_motor_output
+        motor_statistics_front_motors.right_steer_motor_output = right_steer_motor_output
+
+        self.motorStatisticsFrontMotorsPublisher.publish(motor_statistics_front_motors)
+
+
+    def publish_motor_statistics_back_motors(self, msg):
+        left_drive_motor_output, left_steer_motor_output, right_drive_motor_output, right_steer_motor_output= struct.unpack("hh", msg.data)
+        motor_statistics_back_motors = MotorStatisticsFrontMotors()
+        motor_statistics_back_motors.left_drive_motor_output = left_drive_motor_output
+        motor_statistics_back_motors.left_steer_motor_output = left_steer_motor_output
+        motor_statistics_back_motors.right_drive_motor_output = right_drive_motor_output
+        motor_statistics_back_motors.right_steer_motor_output = right_steer_motor_output
+
+        self.motorStatisticsFrontMotorsPublisher.publish(motor_statistics_back_motors)
+
+
+    def publish_conbus(self, msg):
+        conbus = Conbus()
+        conbus.id = msg.arbitration_id
+        conbus.data = msg.data
+        self.conbusPublisher.publish(conbus)
+
+
+    # subscriber callbacks
+    def on_safety_lights_received(self, msg:SafetyLights):
+        safety_lights_packet = SafetyLightsPacket()
+        safety_lights_packet.autonomous = msg.autonomous
+        safety_lights_packet.mode = msg.mode
+        safety_lights_packet.red = msg.red
+        safety_lights_packet.green = msg.green
+        safety_lights_packet.blue = msg.blue
+        safety_lights_packet.blink_period = msg.blink_period
+
+        data = bytes(safety_lights_packet)
+        can_msg = can.Message(arbitration_id=arbitration_ids["SafetyLightsCommand"], data=data)
+
+        try:
+            self.can.send(can_msg)
+        except can.CanError:
+            pass
+
+
+    def on_motor_input_received(self, msg:MotorInput):
+        if self.get_device_state() != DeviceState.OPERATING:
+            return
+        data = struct.pack("hhh", int(msg.forward_velocity * 10000), int(msg.sideways_velocity * 10000), int(msg.angular_velocity * 10000))
+        can_msg = can.message(arbitration_id = arbitration_ids["MotorsCommand"], data = data)
+        try:
+            self.can.send(can_msg)
+        except can.CanError:
+            pass
+
+
+    def on_conbus_received(self, msg:Conbus):
+        if self.get_device_state() != DeviceState.OPERATING:
+            return
+        try:
+            data = bytes(msg.data)
+            arbitration_id = msg.id
+
+            can_msg = can.message(arbitration_id = arbitration_id, data = data)
+            
+            try:
+                self.can.send(can_msg)
+            except can.CanError:
+                pass
+        except:
+            pass
+    
+def main():
+    rclpy.init()
+    can_node = CanNode()
+    rclpy.spin(can_node)
+    rclpy.shutdown()
+
+if __name__ == "__main__":
+    main()
+
diff --git a/autonav_ws/src/autonav_launch/launch/test.xml b/autonav_ws/src/autonav_launch/launch/test.xml
index a7b130c..6f03007 100644
--- a/autonav_ws/src/autonav_launch/launch/test.xml
+++ b/autonav_ws/src/autonav_launch/launch/test.xml
@@ -1,4 +1,5 @@
 <launch>
     <node pkg="autonav_example_py" exec="example.py" output="screen" emulate_tty="true" />
     <node pkg="autonav_example_cpp" exec="autonav_example_cpp" output="screen" emulate_tty="true" />
+    <node pkg="ros_tcp_endpoint" exec="default_server_endpoint" output="screen" emulate_tty="true" />
 </launch>
\ No newline at end of file
diff --git a/autonav_ws/src/autonav_msgs/CMakeLists.txt b/autonav_ws/src/autonav_msgs/CMakeLists.txt
index 99c6eed..6cf4df7 100644
--- a/autonav_ws/src/autonav_msgs/CMakeLists.txt
+++ b/autonav_ws/src/autonav_msgs/CMakeLists.txt
@@ -12,15 +12,19 @@ find_package(builtin_interfaces REQUIRED)
 
 # generate messages
 set(msg_files
+    "msg/AngularPIDStatistics.msg"
     "msg/AudibleFeedback.msg"
     "msg/Conbus.msg"
     "msg/ControllerInput.msg"
     "msg/DeviceState.msg"
     "msg/GPSFeedback.msg"
     "msg/IMUData.msg"
+    "msg/LinearPIDStatistics.msg"
     "msg/Log.msg"
     "msg/MotorFeedback.msg"
     "msg/MotorInput.msg"
+    "msg/MotorStatisticsBackMotors.msg"
+    "msg/MotorStatisticsFrontMotors.msg"
     "msg/NUCStatistics.msg"
     "msg/Position.msg"
     "msg/SafetyLights.msg"
diff --git a/autonav_ws/src/autonav_msgs/msg/AngularPIDStatistics.msg b/autonav_ws/src/autonav_msgs/msg/AngularPIDStatistics.msg
new file mode 100644
index 0000000..828957a
--- /dev/null
+++ b/autonav_ws/src/autonav_msgs/msg/AngularPIDStatistics.msg
@@ -0,0 +1,2 @@
+uint8 angular_velocity
+uint8 angular_velocity_setpoint
\ No newline at end of file
diff --git a/autonav_ws/src/autonav_msgs/msg/LinearPIDStatistics.msg b/autonav_ws/src/autonav_msgs/msg/LinearPIDStatistics.msg
new file mode 100644
index 0000000..194aa33
--- /dev/null
+++ b/autonav_ws/src/autonav_msgs/msg/LinearPIDStatistics.msg
@@ -0,0 +1,4 @@
+uint8 forward_velocity
+uint8 forward_velocity_setpoint
+uint8 sideways_velocity
+uint8 sideways_velocity_setpoint
\ No newline at end of file
diff --git a/autonav_ws/src/autonav_msgs/msg/MotorStatisticsBackMotors.msg b/autonav_ws/src/autonav_msgs/msg/MotorStatisticsBackMotors.msg
new file mode 100644
index 0000000..994b01a
--- /dev/null
+++ b/autonav_ws/src/autonav_msgs/msg/MotorStatisticsBackMotors.msg
@@ -0,0 +1,4 @@
+uint8 left_drive_motor_output
+uint8 left_steer_motor_output
+uint8 right_drive_motor_output
+uint8 right_steer_motor_output
\ No newline at end of file
diff --git a/autonav_ws/src/autonav_msgs/msg/MotorStatisticsFrontMotors.msg b/autonav_ws/src/autonav_msgs/msg/MotorStatisticsFrontMotors.msg
new file mode 100644
index 0000000..994b01a
--- /dev/null
+++ b/autonav_ws/src/autonav_msgs/msg/MotorStatisticsFrontMotors.msg
@@ -0,0 +1,4 @@
+uint8 left_drive_motor_output
+uint8 left_steer_motor_output
+uint8 right_drive_motor_output
+uint8 right_steer_motor_output
\ No newline at end of file
diff --git a/autonav_ws/src/autonav_msgs/msg/SafetyLights.msg b/autonav_ws/src/autonav_msgs/msg/SafetyLights.msg
index b4090b3..98d29a7 100644
--- a/autonav_ws/src/autonav_msgs/msg/SafetyLights.msg
+++ b/autonav_ws/src/autonav_msgs/msg/SafetyLights.msg
@@ -1,4 +1,7 @@
 bool autonomous
+uint8 mode
+uint8 brightness
 uint8 red
 uint8 green
-uint8 blue
\ No newline at end of file
+uint8 blue
+uint8 blink_period
\ No newline at end of file
diff --git a/autonav_ws/src/ros_tcp_endpoint_ros2/LICENSE b/autonav_ws/src/ros_tcp_endpoint_ros2/LICENSE
new file mode 100644
index 0000000..7507bc9
--- /dev/null
+++ b/autonav_ws/src/ros_tcp_endpoint_ros2/LICENSE
@@ -0,0 +1,190 @@
+                                 Apache License
+                           Version 2.0, January 2004
+                        http://www.apache.org/licenses/
+
+   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+   1. Definitions.
+
+      "License" shall mean the terms and conditions for use, reproduction,
+      and distribution as defined by Sections 1 through 9 of this document.
+
+      "Licensor" shall mean the copyright owner or entity authorized by
+      the copyright owner that is granting the License.
+
+      "Legal Entity" shall mean the union of the acting entity and all
+      other entities that control, are controlled by, or are under common
+      control with that entity. For the purposes of this definition,
+      "control" means (i) the power, direct or indirect, to cause the
+      direction or management of such entity, whether by contract or
+      otherwise, or (ii) ownership of fifty percent (50%) or more of the
+      outstanding shares, or (iii) beneficial ownership of such entity.
+
+      "You" (or "Your") shall mean an individual or Legal Entity
+      exercising permissions granted by this License.
+
+      "Source" form shall mean the preferred form for making modifications,
+      including but not limited to software source code, documentation
+      source, and configuration files.
+
+      "Object" form shall mean any form resulting from mechanical
+      transformation or translation of a Source form, including but
+      not limited to compiled object code, generated documentation,
+      and conversions to other media types.
+
+      "Work" shall mean the work of authorship, whether in Source or
+      Object form, made available under the License, as indicated by a
+      copyright notice that is included in or attached to the work
+      (an example is provided in the Appendix below).
+
+      "Derivative Works" shall mean any work, whether in Source or Object
+      form, that is based on (or derived from) the Work and for which the
+      editorial revisions, annotations, elaborations, or other modifications
+      represent, as a whole, an original work of authorship. For the purposes
+      of this License, Derivative Works shall not include works that remain
+      separable from, or merely link (or bind by name) to the interfaces of,
+      the Work and Derivative Works thereof.
+
+      "Contribution" shall mean any work of authorship, including
+      the original version of the Work and any modifications or additions
+      to that Work or Derivative Works thereof, that is intentionally
+      submitted to Licensor for inclusion in the Work by the copyright owner
+      or by an individual or Legal Entity authorized to submit on behalf of
+      the copyright owner. For the purposes of this definition, "submitted"
+      means any form of electronic, verbal, or written communication sent
+      to the Licensor or its representatives, including but not limited to
+      communication on electronic mailing lists, source code control systems,
+      and issue tracking systems that are managed by, or on behalf of, the
+      Licensor for the purpose of discussing and improving the Work, but
+      excluding communication that is conspicuously marked or otherwise
+      designated in writing by the copyright owner as "Not a Contribution."
+
+      "Contributor" shall mean Licensor and any individual or Legal Entity
+      on behalf of whom a Contribution has been received by Licensor and
+      subsequently incorporated within the Work.
+
+   2. Grant of Copyright License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      copyright license to reproduce, prepare Derivative Works of,
+      publicly display, publicly perform, sublicense, and distribute the
+      Work and such Derivative Works in Source or Object form.
+
+   3. Grant of Patent License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      (except as stated in this section) patent license to make, have made,
+      use, offer to sell, sell, import, and otherwise transfer the Work,
+      where such license applies only to those patent claims licensable
+      by such Contributor that are necessarily infringed by their
+      Contribution(s) alone or by combination of their Contribution(s)
+      with the Work to which such Contribution(s) was submitted. If You
+      institute patent litigation against any entity (including a
+      cross-claim or counterclaim in a lawsuit) alleging that the Work
+      or a Contribution incorporated within the Work constitutes direct
+      or contributory patent infringement, then any patent licenses
+      granted to You under this License for that Work shall terminate
+      as of the date such litigation is filed.
+
+   4. Redistribution. You may reproduce and distribute copies of the
+      Work or Derivative Works thereof in any medium, with or without
+      modifications, and in Source or Object form, provided that You
+      meet the following conditions:
+
+      (a) You must give any other recipients of the Work or
+          Derivative Works a copy of this License; and
+
+      (b) You must cause any modified files to carry prominent notices
+          stating that You changed the files; and
+
+      (c) You must retain, in the Source form of any Derivative Works
+          that You distribute, all copyright, patent, trademark, and
+          attribution notices from the Source form of the Work,
+          excluding those notices that do not pertain to any part of
+          the Derivative Works; and
+
+      (d) If the Work includes a "NOTICE" text file as part of its
+          distribution, then any Derivative Works that You distribute must
+          include a readable copy of the attribution notices contained
+          within such NOTICE file, excluding those notices that do not
+          pertain to any part of the Derivative Works, in at least one
+          of the following places: within a NOTICE text file distributed
+          as part of the Derivative Works; within the Source form or
+          documentation, if provided along with the Derivative Works; or,
+          within a display generated by the Derivative Works, if and
+          wherever such third-party notices normally appear. The contents
+          of the NOTICE file are for informational purposes only and
+          do not modify the License. You may add Your own attribution
+          notices within Derivative Works that You distribute, alongside
+          or as an addendum to the NOTICE text from the Work, provided
+          that such additional attribution notices cannot be construed
+          as modifying the License.
+
+      You may add Your own copyright statement to Your modifications and
+      may provide additional or different license terms and conditions
+      for use, reproduction, or distribution of Your modifications, or
+      for any such Derivative Works as a whole, provided Your use,
+      reproduction, and distribution of the Work otherwise complies with
+      the conditions stated in this License.
+
+   5. Submission of Contributions. Unless You explicitly state otherwise,
+      any Contribution intentionally submitted for inclusion in the Work
+      by You to the Licensor shall be under the terms and conditions of
+      this License, without any additional terms or conditions.
+      Notwithstanding the above, nothing herein shall supersede or modify
+      the terms of any separate license agreement you may have executed
+      with Licensor regarding such Contributions.
+
+   6. Trademarks. This License does not grant permission to use the trade
+      names, trademarks, service marks, or product names of the Licensor,
+      except as required for reasonable and customary use in describing the
+      origin of the Work and reproducing the content of the NOTICE file.
+
+   7. Disclaimer of Warranty. Unless required by applicable law or
+      agreed to in writing, Licensor provides the Work (and each
+      Contributor provides its Contributions) on an "AS IS" BASIS,
+      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+      implied, including, without limitation, any warranties or conditions
+      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+      PARTICULAR PURPOSE. You are solely responsible for determining the
+      appropriateness of using or redistributing the Work and assume any
+      risks associated with Your exercise of permissions under this License.
+
+   8. Limitation of Liability. In no event and under no legal theory,
+      whether in tort (including negligence), contract, or otherwise,
+      unless required by applicable law (such as deliberate and grossly
+      negligent acts) or agreed to in writing, shall any Contributor be
+      liable to You for damages, including any direct, indirect, special,
+      incidental, or consequential damages of any character arising as a
+      result of this License or out of the use or inability to use the
+      Work (including but not limited to damages for loss of goodwill,
+      work stoppage, computer failure or malfunction, or any and all
+      other commercial damages or losses), even if such Contributor
+      has been advised of the possibility of such damages.
+
+   9. Accepting Warranty or Additional Liability. While redistributing
+      the Work or Derivative Works thereof, You may choose to offer,
+      and charge a fee for, acceptance of support, warranty, indemnity,
+      or other liability obligations and/or rights consistent with this
+      License. However, in accepting such obligations, You may act only
+      on Your own behalf and on Your sole responsibility, not on behalf
+      of any other Contributor, and only if You agree to indemnify,
+      defend, and hold each Contributor harmless for any liability
+      incurred by, or claims asserted against, such Contributor by reason
+      of your accepting any such warranty or additional liability.
+
+   END OF TERMS AND CONDITIONS
+
+   Copyright 2020 Unity Technologies
+
+   Licensed under the Apache License, Version 2.0 (the "License");
+   you may not use this file except in compliance with the License.
+   You may obtain a copy of the License at
+
+       http://www.apache.org/licenses/LICENSE-2.0
+
+   Unless required by applicable law or agreed to in writing, software
+   distributed under the License is distributed on an "AS IS" BASIS,
+   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+   See the License for the specific language governing permissions and
+   limitations under the License.
diff --git a/autonav_ws/src/ros_tcp_endpoint_ros2/README.md b/autonav_ws/src/ros_tcp_endpoint_ros2/README.md
new file mode 100644
index 0000000..40f60cb
--- /dev/null
+++ b/autonav_ws/src/ros_tcp_endpoint_ros2/README.md
@@ -0,0 +1,26 @@
+# ROS TCP Endpoint
+
+[![License](https://img.shields.io/badge/License-Apache%202.0-blue.svg)](https://opensource.org/licenses/Apache-2.0)
+
+## Introduction
+
+[ROS](https://www.ros.org/) package used to create an endpoint to accept ROS messages sent from a Unity scene using the [ROS TCP Connector](https://github.com/Unity-Technologies/ROS-TCP-Connector) scripts.
+
+Instructions and examples on how to use this ROS package can be found on the [Unity Robotics Hub](https://github.com/Unity-Technologies/Unity-Robotics-Hub/blob/master/tutorials/ros_unity_integration/README.md) repository.
+
+## Community and Feedback
+
+The Unity Robotics projects are open-source and we encourage and welcome contributions.
+If you wish to contribute, be sure to review our [contribution guidelines](CONTRIBUTING.md)
+and [code of conduct](CODE_OF_CONDUCT.md).
+
+## Support
+For questions or discussions about Unity Robotics package installations or how to best set up and integrate your robotics projects, please create a new thread on the [Unity Robotics forum](https://forum.unity.com/forums/robotics.623/) and make sure to include as much detail as possible.
+
+For feature requests, bugs, or other issues, please file a [GitHub issue](https://github.com/Unity-Technologies/ROS-TCP-Endpoint/issues) using the provided templates and the Robotics team will investigate as soon as possible.
+
+For any other questions or feedback, connect directly with the
+Robotics team at [unity-robotics@unity3d.com](mailto:unity-robotics@unity3d.com).
+
+## License
+[Apache License 2.0](LICENSE)
\ No newline at end of file
diff --git a/autonav_ws/src/ros_tcp_endpoint_ros2/launch/endpoint.py b/autonav_ws/src/ros_tcp_endpoint_ros2/launch/endpoint.py
new file mode 100644
index 0000000..2e1bf8c
--- /dev/null
+++ b/autonav_ws/src/ros_tcp_endpoint_ros2/launch/endpoint.py
@@ -0,0 +1,15 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    return LaunchDescription(
+        [
+            Node(
+                package="ros_tcp_endpoint",
+                executable="default_server_endpoint",
+                emulate_tty=True,
+                parameters=[{"ROS_IP": "0.0.0.0"}, {"ROS_TCP_PORT": 10000}],
+            )
+        ]
+    )
diff --git a/autonav_ws/src/ros_tcp_endpoint_ros2/package.xml b/autonav_ws/src/ros_tcp_endpoint_ros2/package.xml
new file mode 100644
index 0000000..7919782
--- /dev/null
+++ b/autonav_ws/src/ros_tcp_endpoint_ros2/package.xml
@@ -0,0 +1,20 @@
+<?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>ros_tcp_endpoint</name>
+  <version>0.7.0</version>
+  <description>ROS TCP Endpoint Unity Integration (ROS2 version)
+  Acts as the bridge between Unity messages sent via TCP socket and ROS messages.
+  </description>
+  <maintainer email="unity-robotics@unity3d.com">Unity Robotics</maintainer>
+  <license>Apache 2.0</license>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>
diff --git a/autonav_ws/src/ros_tcp_endpoint_ros2/resource/ros_tcp_endpoint b/autonav_ws/src/ros_tcp_endpoint_ros2/resource/ros_tcp_endpoint
new file mode 100644
index 0000000..e69de29
diff --git a/autonav_ws/src/ros_tcp_endpoint_ros2/ros_tcp_endpoint/__init__.py b/autonav_ws/src/ros_tcp_endpoint_ros2/ros_tcp_endpoint/__init__.py
new file mode 100644
index 0000000..381ca8e
--- /dev/null
+++ b/autonav_ws/src/ros_tcp_endpoint_ros2/ros_tcp_endpoint/__init__.py
@@ -0,0 +1,15 @@
+#  Copyright 2020 Unity Technologies
+#
+#  Licensed under the Apache License, Version 2.0 (the "License");
+#  you may not use this file except in compliance with the License.
+#  You may obtain a copy of the License at
+#
+#      http://www.apache.org/licenses/LICENSE-2.0
+#
+#  Unless required by applicable law or agreed to in writing, software
+#  distributed under the License is distributed on an "AS IS" BASIS,
+#  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+#  See the License for the specific language governing permissions and
+#  limitations under the License.
+
+from .server import TcpServer
diff --git a/autonav_ws/src/ros_tcp_endpoint_ros2/ros_tcp_endpoint/client.py b/autonav_ws/src/ros_tcp_endpoint_ros2/ros_tcp_endpoint/client.py
new file mode 100644
index 0000000..1783fb8
--- /dev/null
+++ b/autonav_ws/src/ros_tcp_endpoint_ros2/ros_tcp_endpoint/client.py
@@ -0,0 +1,228 @@
+#  Copyright 2020 Unity Technologies
+#
+#  Licensed under the Apache License, Version 2.0 (the "License");
+#  you may not use this file except in compliance with the License.
+#  You may obtain a copy of the License at
+#
+#      http://www.apache.org/licenses/LICENSE-2.0
+#
+#  Unless required by applicable law or agreed to in writing, software
+#  distributed under the License is distributed on an "AS IS" BASIS,
+#  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+#  See the License for the specific language governing permissions and
+#  limitations under the License.
+
+import rclpy
+import struct
+
+import threading
+import json
+
+from rclpy.serialization import deserialize_message
+from rclpy.serialization import serialize_message
+
+from .exceptions import TopicOrServiceNameDoesNotExistError
+
+
+class ClientThread(threading.Thread):
+    """
+    Thread class to read all data from a connection and pass along the data to the
+    desired source.
+    """
+
+    def __init__(self, conn, tcp_server, incoming_ip, incoming_port):
+        """
+        Set class variables
+        Args:
+            conn:
+            tcp_server: server object
+            incoming_ip: connected from this IP address
+            incoming_port: connected from this port
+        """
+        self.conn = conn
+        self.tcp_server = tcp_server
+        self.incoming_ip = incoming_ip
+        self.incoming_port = incoming_port
+        threading.Thread.__init__(self)
+
+    @staticmethod
+    def recvall(conn, size, flags=0):
+        """
+        Receive exactly bufsize bytes from the socket.
+        """
+        buffer = bytearray(size)
+        view = memoryview(buffer)
+        pos = 0
+        while pos < size:
+            read = conn.recv_into(view[pos:], size - pos, flags)
+            if not read:
+                raise IOError("No more data available")
+            pos += read
+        return bytes(buffer)
+
+    @staticmethod
+    def read_int32(conn):
+        """
+        Reads four bytes from socket connection and unpacks them to an int
+
+        Returns: int
+
+        """
+        raw_bytes = ClientThread.recvall(conn, 4)
+        num = struct.unpack("<I", raw_bytes)[0]
+        return num
+
+    def read_string(self):
+        """
+        Reads int32 from socket connection to determine how many bytes to
+        read to get the string that follows. Read that number of bytes and
+        decode to utf-8 string.
+
+        Returns: string
+
+        """
+        str_len = ClientThread.read_int32(self.conn)
+
+        str_bytes = ClientThread.recvall(self.conn, str_len)
+        decoded_str = str_bytes.decode("utf-8")
+
+        return decoded_str
+
+    def read_message(self, conn):
+        """
+        Decode destination and full message size from socket connection.
+        Grab bytes in chunks until full message has been read.
+        """
+        data = b""
+
+        destination = self.read_string()
+        full_message_size = ClientThread.read_int32(conn)
+
+        data = ClientThread.recvall(conn, full_message_size)
+
+        if full_message_size > 0 and not data:
+            self.logerr("No data for a message size of {}, breaking!".format(full_message_size))
+            return
+
+        destination = destination.rstrip("\x00")
+        return destination, data
+
+    @staticmethod
+    def serialize_message(destination, message):
+        """
+        Serialize a destination and message class.
+
+        Args:
+            destination: name of destination
+            message:     message class to serialize
+
+        Returns:
+            serialized destination and message as a list of bytes
+        """
+        dest_bytes = destination.encode("utf-8")
+        length = len(dest_bytes)
+        dest_info = struct.pack("<I%ss" % length, length, dest_bytes)
+
+        serial_response = serialize_message(message)
+
+        msg_length = struct.pack("<I", len(serial_response))
+        serialized_message = dest_info + msg_length + serial_response
+
+        return serialized_message
+
+    @staticmethod
+    def serialize_command(command, params):
+        cmd_bytes = command.encode("utf-8")
+        cmd_length = len(cmd_bytes)
+        cmd_info = struct.pack("<I%ss" % cmd_length, cmd_length, cmd_bytes)
+
+        json_bytes = json.dumps(params.__dict__).encode("utf-8")
+        json_length = len(json_bytes)
+        json_info = struct.pack("<I%ss" % json_length, json_length, json_bytes)
+
+        return cmd_info + json_info
+
+    def send_ros_service_request(self, srv_id, destination, data):
+        if destination not in self.tcp_server.ros_services_table.keys():
+            error_msg = "Service destination '{}' is not registered! Known services are: {} ".format(
+                destination, self.tcp_server.ros_services_table.keys()
+            )
+            self.tcp_server.send_unity_error(error_msg)
+            self.tcp_server.logerr(error_msg)
+            # TODO: send a response to Unity anyway?
+            return
+        else:
+            ros_communicator = self.tcp_server.ros_services_table[destination]
+            service_thread = threading.Thread(
+                target=self.service_call_thread, args=(srv_id, destination, data, ros_communicator)
+            )
+            service_thread.daemon = True
+            service_thread.start()
+
+    def service_call_thread(self, srv_id, destination, data, ros_communicator):
+        response = ros_communicator.send(data)
+
+        if not response:
+            error_msg = "No response data from service '{}'!".format(destination)
+            self.tcp_server.send_unity_error(error_msg)
+            self.tcp_server.logerr(error_msg)
+            # TODO: send a response to Unity anyway?
+            return
+
+        self.tcp_server.unity_tcp_sender.send_ros_service_response(srv_id, destination, response)
+
+    def run(self):
+        """
+        Receive a message from Unity and determine where to send it based on the publishers table
+         and topic string. Then send the read message.
+
+        If there is a response after sending the serialized data, assume it is a
+        ROS service response.
+
+        Message format is expected to arrive as
+            int: length of destination bytes
+            str: destination. Publisher topic, Subscriber topic, Service name, etc
+            int: size of full message
+            msg: the ROS msg type as bytes
+
+        """
+        self.tcp_server.loginfo("Connection from {}".format(self.incoming_ip))
+        halt_event = threading.Event()
+        self.tcp_server.unity_tcp_sender.start_sender(self.conn, halt_event)
+        try:
+            while not halt_event.is_set():
+                destination, data = self.read_message(self.conn)
+
+                # Process this message that was sent from Unity
+                if self.tcp_server.pending_srv_id is not None:
+                    # if we've been told that the next message will be a service request/response, process it as such
+                    if self.tcp_server.pending_srv_is_request:
+                        self.send_ros_service_request(
+                            self.tcp_server.pending_srv_id, destination, data
+                        )
+                    else:
+                        self.tcp_server.send_unity_service_response(
+                            self.tcp_server.pending_srv_id, data
+                        )
+                    self.tcp_server.pending_srv_id = None
+                elif destination == "":
+                    # ignore this keepalive message, listen for more
+                    pass
+                elif destination.startswith("__"):
+                    # handle a system command, such as registering new topics
+                    self.tcp_server.handle_syscommand(destination, data)
+                elif destination in self.tcp_server.publishers_table:
+                    ros_communicator = self.tcp_server.publishers_table[destination]
+                    ros_communicator.send(data)
+                else:
+                    error_msg = "Not registered to publish topic '{}'! Valid publish topics are: {} ".format(
+                        destination, self.tcp_server.publishers_table.keys()
+                    )
+                    self.tcp_server.send_unity_error(error_msg)
+                    self.tcp_server.logerr(error_msg)
+        except IOError as e:
+            self.tcp_server.logerr("Exception: {}".format(e))
+        finally:
+            halt_event.set()
+            self.conn.close()
+            self.tcp_server.loginfo("Disconnected from {}".format(self.incoming_ip))
diff --git a/autonav_ws/src/ros_tcp_endpoint_ros2/ros_tcp_endpoint/communication.py b/autonav_ws/src/ros_tcp_endpoint_ros2/ros_tcp_endpoint/communication.py
new file mode 100644
index 0000000..6aafbfe
--- /dev/null
+++ b/autonav_ws/src/ros_tcp_endpoint_ros2/ros_tcp_endpoint/communication.py
@@ -0,0 +1,42 @@
+#  Copyright 2020 Unity Technologies
+#
+#  Licensed under the Apache License, Version 2.0 (the "License");
+#  you may not use this file except in compliance with the License.
+#  You may obtain a copy of the License at
+#
+#      http://www.apache.org/licenses/LICENSE-2.0
+#
+#  Unless required by applicable law or agreed to in writing, software
+#  distributed under the License is distributed on an "AS IS" BASIS,
+#  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+#  See the License for the specific language governing permissions and
+#  limitations under the License.
+
+import rclpy
+from rclpy.node import Node
+
+
+class RosSender(Node):
+    """
+        Base class for ROS communication where data is sent to the ROS network.
+    """
+
+    def __init__(self, node_name):
+        super().__init__(node_name)
+        pass
+
+    def send(self, *args):
+        raise NotImplementedError
+
+
+class RosReceiver(Node):
+    """
+        Base class for ROS communication where data is being sent outside of the ROS network.
+    """
+
+    def __init__(self, node_name):
+        super().__init__(node_name)
+        pass
+
+    def send(self, *args):
+        raise NotImplementedError
diff --git a/autonav_ws/src/ros_tcp_endpoint_ros2/ros_tcp_endpoint/default_server_endpoint.py b/autonav_ws/src/ros_tcp_endpoint_ros2/ros_tcp_endpoint/default_server_endpoint.py
new file mode 100644
index 0000000..3f09552
--- /dev/null
+++ b/autonav_ws/src/ros_tcp_endpoint_ros2/ros_tcp_endpoint/default_server_endpoint.py
@@ -0,0 +1,21 @@
+#!/usr/bin/env python
+
+import rclpy
+
+from ros_tcp_endpoint import TcpServer
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    tcp_server = TcpServer("UnityEndpoint")
+
+    tcp_server.start()
+
+    tcp_server.setup_executor()
+
+    tcp_server.destroy_nodes()
+    rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()
diff --git a/autonav_ws/src/ros_tcp_endpoint_ros2/ros_tcp_endpoint/exceptions.py b/autonav_ws/src/ros_tcp_endpoint_ros2/ros_tcp_endpoint/exceptions.py
new file mode 100644
index 0000000..a0a808d
--- /dev/null
+++ b/autonav_ws/src/ros_tcp_endpoint_ros2/ros_tcp_endpoint/exceptions.py
@@ -0,0 +1,23 @@
+#  Copyright 2020 Unity Technologies
+#
+#  Licensed under the Apache License, Version 2.0 (the "License");
+#  you may not use this file except in compliance with the License.
+#  You may obtain a copy of the License at
+#
+#      http://www.apache.org/licenses/LICENSE-2.0
+#
+#  Unless required by applicable law or agreed to in writing, software
+#  distributed under the License is distributed on an "AS IS" BASIS,
+#  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+#  See the License for the specific language governing permissions and
+#  limitations under the License.
+
+
+class RosTcpEndpointError(Exception):
+    """Base class for this package's custom exceptions"""
+    pass
+
+
+class TopicOrServiceNameDoesNotExistError(RosTcpEndpointError):
+    """The topic or service name passed does not exist in the source destination dictionary."""
+    pass
diff --git a/autonav_ws/src/ros_tcp_endpoint_ros2/ros_tcp_endpoint/publisher.py b/autonav_ws/src/ros_tcp_endpoint_ros2/ros_tcp_endpoint/publisher.py
new file mode 100644
index 0000000..4a66976
--- /dev/null
+++ b/autonav_ws/src/ros_tcp_endpoint_ros2/ros_tcp_endpoint/publisher.py
@@ -0,0 +1,68 @@
+#  Copyright 2020 Unity Technologies
+#
+#  Licensed under the Apache License, Version 2.0 (the "License");
+#  you may not use this file except in compliance with the License.
+#  You may obtain a copy of the License at
+#
+#      http://www.apache.org/licenses/LICENSE-2.0
+#
+#  Unless required by applicable law or agreed to in writing, software
+#  distributed under the License is distributed on an "AS IS" BASIS,
+#  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+#  See the License for the specific language governing permissions and
+#  limitations under the License.
+
+import rclpy
+import re
+
+from rclpy.serialization import deserialize_message
+
+from .communication import RosSender
+
+
+class RosPublisher(RosSender):
+    """
+    Class to publish messages to a ROS topic
+    """
+
+    # TODO: surface latch functionality
+    def __init__(self, topic, message_class, queue_size=10, latch=False):
+        """
+
+        Args:
+            topic:         Topic name to publish messages to
+            message_class: The message class in catkin workspace
+            queue_size:    Max number of entries to maintain in an outgoing queue
+        """
+        strippedTopic = re.sub("[^A-Za-z0-9_]+", "", topic)
+        node_name = f"{strippedTopic}_RosPublisher"
+        RosSender.__init__(self, node_name)
+        self.msg = message_class()
+        self.pub = self.create_publisher(message_class, topic, queue_size)
+
+    def send(self, data):
+        """
+        Takes in serialized message data from source outside of the ROS network,
+        deserializes it into it's message class, and publishes the message to ROS topic.
+
+        Args:
+            data: The already serialized message_class data coming from outside of ROS
+
+        Returns:
+            None: Explicitly return None so behaviour can be
+        """
+        # message_type = type(self.msg)
+        # message = deserialize_message(data, message_type)
+
+        self.pub.publish(data)
+
+        return None
+
+    def unregister(self):
+        """
+
+        Returns:
+
+        """
+        self.destroy_publisher(self.pub)
+        self.destroy_node()
diff --git a/autonav_ws/src/ros_tcp_endpoint_ros2/ros_tcp_endpoint/server.py b/autonav_ws/src/ros_tcp_endpoint_ros2/ros_tcp_endpoint/server.py
new file mode 100644
index 0000000..5954994
--- /dev/null
+++ b/autonav_ws/src/ros_tcp_endpoint_ros2/ros_tcp_endpoint/server.py
@@ -0,0 +1,342 @@
+#  Copyright 2020 Unity Technologies
+#
+#  Licensed under the Apache License, Version 2.0 (the "License");
+#  you may not use this file except in compliance with the License.
+#  You may obtain a copy of the License at
+#
+#      http://www.apache.org/licenses/LICENSE-2.0
+#
+#  Unless required by applicable law or agreed to in writing, software
+#  distributed under the License is distributed on an "AS IS" BASIS,
+#  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+#  See the License for the specific language governing permissions and
+#  limitations under the License.
+
+import rclpy
+import socket
+import json
+import sys
+import threading
+import importlib
+
+from rclpy.node import Node
+from rclpy.parameter import Parameter
+from rclpy.executors import MultiThreadedExecutor
+from rclpy.serialization import deserialize_message
+
+from .tcp_sender import UnityTcpSender
+from .client import ClientThread
+from .subscriber import RosSubscriber
+from .publisher import RosPublisher
+from .service import RosService
+from .unity_service import UnityService
+
+
+class TcpServer(Node):
+    """
+    Initializes ROS node and TCP server.
+    """
+
+    def __init__(self, node_name, buffer_size=1024, connections=10, tcp_ip=None, tcp_port=None):
+        """
+        Initializes ROS node and class variables.
+
+        Args:
+            node_name:               ROS node name for executing code
+            buffer_size:             The read buffer size used when reading from a socket
+            connections:             Max number of queued connections. See Python Socket documentation
+        """
+        super().__init__(node_name)
+
+        self.declare_parameter("ROS_IP", "0.0.0.0")
+        self.declare_parameter("ROS_TCP_PORT", 10000)
+
+        if tcp_ip:
+            self.loginfo("Using ROS_IP override from constructor: {}".format(tcp_ip))
+            self.tcp_ip = tcp_ip
+        else:
+            self.tcp_ip = self.get_parameter("ROS_IP").get_parameter_value().string_value
+
+        if tcp_port:
+            self.loginfo("Using ROS_TCP_PORT override from constructor: {}".format(tcp_port))
+            self.tcp_port = tcp_port
+        else:
+            self.tcp_port = self.get_parameter("ROS_TCP_PORT").get_parameter_value().integer_value
+
+        self.unity_tcp_sender = UnityTcpSender(self)
+
+        self.node_name = node_name
+        self.publishers_table = {}
+        self.subscribers_table = {}
+        self.ros_services_table = {}
+        self.unity_services_table = {}
+        self.buffer_size = buffer_size
+        self.connections = connections
+        self.syscommands = SysCommands(self)
+        self.pending_srv_id = None
+        self.pending_srv_is_request = False
+
+    def start(self, publishers=None, subscribers=None):
+        if publishers is not None:
+            self.publishers_table = publishers
+        if subscribers is not None:
+            self.subscribers_table = subscribers
+        server_thread = threading.Thread(target=self.listen_loop)
+        # Exit the server thread when the main thread terminates
+        server_thread.daemon = True
+        server_thread.start()
+
+    def listen_loop(self):
+        """
+            Creates and binds sockets using TCP variables then listens for incoming connections.
+            For each new connection a client thread will be created to handle communication.
+        """
+        self.loginfo("Starting server on {}:{}".format(self.tcp_ip, self.tcp_port))
+        tcp_server = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+        tcp_server.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
+        tcp_server.bind((self.tcp_ip, self.tcp_port))
+
+        while True:
+            tcp_server.listen(self.connections)
+
+            try:
+                (conn, (ip, port)) = tcp_server.accept()
+                ClientThread(conn, self, ip, port).start()
+            except socket.timeout as err:
+                self.logerr("ros_tcp_endpoint.TcpServer: socket timeout")
+
+    def send_unity_error(self, error):
+        self.unity_tcp_sender.send_unity_error(error)
+
+    def send_unity_message(self, topic, message):
+        self.unity_tcp_sender.send_unity_message(topic, message)
+
+    def send_unity_service(self, topic, service_class, request):
+        return self.unity_tcp_sender.send_unity_service_request(topic, service_class, request)
+
+    def send_unity_service_response(self, srv_id, data):
+        self.unity_tcp_sender.send_unity_service_response(srv_id, data)
+
+    def handle_syscommand(self, topic, data):
+        function = getattr(self.syscommands, topic[2:])
+        if function is None:
+            self.send_unity_error("Don't understand SysCommand.'{}'".format(topic))
+        else:
+            message_json = data.decode("utf-8")[:-1]
+            params = json.loads(message_json)
+            function(**params)
+
+    def loginfo(self, text):
+        self.get_logger().info(text)
+
+    def logwarn(self, text):
+        self.get_logger().warning(text)
+
+    def logerr(self, text):
+        self.get_logger().error(text)
+
+    def setup_executor(self):
+        """
+            Since rclpy.spin() is a blocking call the server needed a way
+            to spin all of the relevant nodes at the same time.
+
+            MultiThreadedExecutor allows us to set the number of threads
+            needed as well as the nodes that need to be spun.
+        """
+        num_threads = (
+            len(self.publishers_table.keys())
+            + len(self.subscribers_table.keys())
+            + len(self.ros_services_table.keys())
+            + len(self.unity_services_table.keys())
+            + 1
+        )
+        executor = MultiThreadedExecutor(num_threads)
+
+        executor.add_node(self)
+
+        for ros_node in self.publishers_table.values():
+            executor.add_node(ros_node)
+        for ros_node in self.subscribers_table.values():
+            executor.add_node(ros_node)
+        for ros_node in self.ros_services_table.values():
+            executor.add_node(ros_node)
+        for ros_node in self.unity_services_table.values():
+            executor.add_node(ros_node)
+
+        self.executor = executor
+        executor.spin()
+
+    def unregister_node(self, old_node):
+        if old_node is not None:
+            old_node.unregister()
+            if self.executor is not None:
+                self.executor.remove_node(old_node)
+
+    def destroy_nodes(self):
+        """
+            Clean up all of the nodes
+        """
+        for ros_node in self.publishers_table.values():
+            ros_node.destroy_node()
+        for ros_node in self.subscribers_table.values():
+            ros_node.destroy_node()
+        for ros_node in self.ros_services_table.values():
+            ros_node.destroy_node()
+        for ros_node in self.unity_services_table.values():
+            ros_node.destroy_node()
+
+        self.destroy_node()
+
+
+class SysCommands:
+    def __init__(self, tcp_server):
+        self.tcp_server = tcp_server
+
+    def subscribe(self, topic, message_name):
+        if topic == "":
+            self.tcp_server.send_unity_error(
+                "Can't subscribe to a blank topic name! SysCommand.subscribe({}, {})".format(
+                    topic, message_name
+                )
+            )
+            return
+
+        message_class = self.resolve_message_name(message_name)
+        if message_class is None:
+            self.tcp_server.send_unity_error(
+                "SysCommand.subscribe - Unknown message class '{}'".format(message_name)
+            )
+            return
+
+        old_node = self.tcp_server.subscribers_table.get(topic)
+        if old_node is not None:
+            self.tcp_server.unregister_node(old_node)
+
+        new_subscriber = RosSubscriber(topic, message_class, self.tcp_server)
+        self.tcp_server.subscribers_table[topic] = new_subscriber
+        if self.tcp_server.executor is not None:
+            self.tcp_server.executor.add_node(new_subscriber)
+
+        self.tcp_server.loginfo("RegisterSubscriber({}, {}) OK".format(topic, message_class))
+
+    def publish(self, topic, message_name, queue_size=10, latch=False):
+        if topic == "":
+            self.tcp_server.send_unity_error(
+                "Can't publish to a blank topic name! SysCommand.publish({}, {})".format(
+                    topic, message_name
+                )
+            )
+            return
+
+        message_class = self.resolve_message_name(message_name)
+        if message_class is None:
+            self.tcp_server.send_unity_error(
+                "SysCommand.publish - Unknown message class '{}'".format(message_name)
+            )
+            return
+
+        old_node = self.tcp_server.publishers_table.get(topic)
+        if old_node is not None:
+            self.tcp_server.unregister_node(old_node)
+
+        new_publisher = RosPublisher(topic, message_class, queue_size=queue_size, latch=latch)
+
+        self.tcp_server.publishers_table[topic] = new_publisher
+        if self.tcp_server.executor is not None:
+            self.tcp_server.executor.add_node(new_publisher)
+
+        self.tcp_server.loginfo("RegisterPublisher({}, {}) OK".format(topic, message_class))
+
+    def ros_service(self, topic, message_name):
+        if topic == "":
+            self.tcp_server.send_unity_error(
+                "RegisterRosService({}, {}) - Can't register a blank topic name!".format(
+                    topic, message_name
+                )
+            )
+            return
+        message_class = self.resolve_message_name(message_name, "srv")
+        if message_class is None:
+            self.tcp_server.send_unity_error(
+                "RegisterRosService({}, {}) - Unknown service class '{}'".format(
+                    topic, message_name, message_name
+                )
+            )
+            return
+
+        old_node = self.tcp_server.ros_services_table.get(topic)
+        if old_node is not None:
+            self.tcp_server.unregister_node(old_node)
+
+        new_service = RosService(topic, message_class)
+
+        self.tcp_server.ros_services_table[topic] = new_service
+        if self.tcp_server.executor is not None:
+            self.tcp_server.executor.add_node(new_service)
+
+        self.tcp_server.loginfo("RegisterRosService({}, {}) OK".format(topic, message_class))
+
+    def unity_service(self, topic, message_name):
+        if topic == "":
+            self.tcp_server.send_unity_error(
+                "RegisterUnityService({}, {}) - Can't register a blank topic name!".format(
+                    topic, message_name
+                )
+            )
+            return
+
+        message_class = self.resolve_message_name(message_name, "srv")
+        if message_class is None:
+            self.tcp_server.send_unity_error(
+                "RegisterUnityService({}, {}) - Unknown service class '{}'".format(
+                    topic, message_name, message_name
+                )
+            )
+            return
+
+        old_node = self.tcp_server.unity_services_table.get(topic)
+        if old_node is not None:
+            self.tcp_server.unregister_node(old_node)
+
+        new_service = UnityService(str(topic), message_class, self.tcp_server)
+
+        self.tcp_server.unity_services_table[topic] = new_service
+        if self.tcp_server.executor is not None:
+            self.tcp_server.executor.add_node(new_service)
+
+        self.tcp_server.loginfo("RegisterUnityService({}, {}) OK".format(topic, message_class))
+
+    def response(self, srv_id):  # the next message is a service response
+        self.tcp_server.pending_srv_id = srv_id
+        self.tcp_server.pending_srv_is_request = False
+
+    def request(self, srv_id):  # the next message is a service request
+        self.tcp_server.pending_srv_id = srv_id
+        self.tcp_server.pending_srv_is_request = True
+
+    def topic_list(self):
+        self.tcp_server.unity_tcp_sender.send_topic_list()
+
+    def resolve_message_name(self, name, extension="msg"):
+        try:
+            names = name.split("/")
+            module_name = names[0]
+            class_name = names[1]
+            importlib.import_module(module_name + "." + extension)
+            module = sys.modules[module_name]
+            if module is None:
+                self.tcp_server.logerr("Failed to resolve module {}".format(module_name))
+            module = getattr(module, extension)
+            if module is None:
+                self.tcp_server.logerr(
+                    "Failed to resolve module {}.{}".format(module_name, extension)
+                )
+            module = getattr(module, class_name)
+            if module is None:
+                self.tcp_server.logerr(
+                    "Failed to resolve module {}.{}.{}".format(module_name, extension, class_name)
+                )
+            return module
+        except (IndexError, KeyError, AttributeError, ImportError) as e:
+            self.tcp_server.logerr("Failed to resolve message name: {}".format(e))
+            return None
diff --git a/autonav_ws/src/ros_tcp_endpoint_ros2/ros_tcp_endpoint/service.py b/autonav_ws/src/ros_tcp_endpoint_ros2/ros_tcp_endpoint/service.py
new file mode 100644
index 0000000..eca9b49
--- /dev/null
+++ b/autonav_ws/src/ros_tcp_endpoint_ros2/ros_tcp_endpoint/service.py
@@ -0,0 +1,84 @@
+#  Copyright 2020 Unity Technologies
+#
+#  Licensed under the Apache License, Version 2.0 (the "License");
+#  you may not use this file except in compliance with the License.
+#  You may obtain a copy of the License at
+#
+#      http://www.apache.org/licenses/LICENSE-2.0
+#
+#  Unless required by applicable law or agreed to in writing, software
+#  distributed under the License is distributed on an "AS IS" BASIS,
+#  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+#  See the License for the specific language governing permissions and
+#  limitations under the License.
+
+import rclpy
+import re
+
+from rclpy.serialization import deserialize_message
+
+from .communication import RosSender
+
+
+class RosService(RosSender):
+    """
+    Class to send messages to a ROS service.
+    """
+
+    def __init__(self, service, service_class):
+        """
+        Args:
+            service:        The service name in ROS
+            service_class:  The service class in catkin workspace
+        """
+        strippedService = re.sub("[^A-Za-z0-9_]+", "", service)
+        node_name = f"{strippedService}_RosService"
+        RosSender.__init__(self, node_name)
+
+        self.service_topic = service
+        self.cli = self.create_client(service_class, service)
+        self.req = service_class.Request()
+
+    def send(self, data):
+        """
+        Takes in serialized message data from source outside of the ROS network,
+        deserializes it into it's class, calls the service with the message, and returns
+        the service's response.
+
+        Args:
+            data: The already serialized message_class data coming from outside of ROS
+
+        Returns:
+            service response
+        """
+        message_type = type(self.req)
+        message = deserialize_message(data, message_type)
+
+        if not self.cli.service_is_ready():
+            self.get_logger().error(
+                "Ignoring service call to {} - service is not ready.".format(self.service_topic)
+            )
+            return None
+
+        self.future = self.cli.call_async(message)
+
+        while rclpy.ok():
+            if self.future.done():
+                try:
+                    response = self.future.result()
+                    return response
+                except Exception as e:
+                    self.get_logger().info(f"Service call failed {e}")
+
+                break
+
+        return None
+
+    def unregister(self):
+        """
+
+        Returns:
+
+        """
+        self.destroy_client(self.cli)
+        self.destroy_node()
diff --git a/autonav_ws/src/ros_tcp_endpoint_ros2/ros_tcp_endpoint/subscriber.py b/autonav_ws/src/ros_tcp_endpoint_ros2/ros_tcp_endpoint/subscriber.py
new file mode 100644
index 0000000..c5b2eee
--- /dev/null
+++ b/autonav_ws/src/ros_tcp_endpoint_ros2/ros_tcp_endpoint/subscriber.py
@@ -0,0 +1,74 @@
+#  Copyright 2020 Unity Technologies
+#
+#  Licensed under the Apache License, Version 2.0 (the "License");
+#  you may not use this file except in compliance with the License.
+#  You may obtain a copy of the License at
+#
+#      http://www.apache.org/licenses/LICENSE-2.0
+#
+#  Unless required by applicable law or agreed to in writing, software
+#  distributed under the License is distributed on an "AS IS" BASIS,
+#  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+#  See the License for the specific language governing permissions and
+#  limitations under the License.
+
+import rclpy
+import socket
+import re
+
+from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy
+from rclpy.qos import QoSProfile
+
+from .communication import RosReceiver
+
+
+class RosSubscriber(RosReceiver):
+    """
+    Class to send messages outside of ROS network
+    """
+
+    def __init__(self, topic, message_class, tcp_server, queue_size=10):
+        """
+
+        Args:
+            topic:         Topic name to publish messages to
+            message_class: The message class in catkin workspace
+            queue_size:    Max number of entries to maintain in an outgoing queue
+        """
+        strippedTopic = re.sub("[^A-Za-z0-9_]+", "", topic)
+        self.node_name = f"{strippedTopic}_RosSubscriber"
+        RosReceiver.__init__(self, self.node_name)
+        self.topic = topic
+        self.msg = message_class
+        self.tcp_server = tcp_server
+        self.queue_size = queue_size
+
+        qos_profile = QoSProfile(depth=queue_size)
+
+        # Start Subscriber listener function
+        self.subscription = self.create_subscription(
+            self.msg, self.topic, self.send, qos_profile  # queue_size
+        )
+        self.subscription
+
+    def send(self, data):
+        """
+        Connect to TCP endpoint on client and pass along message
+        Args:
+            data: message data to send outside of ROS network
+
+        Returns:
+            self.msg: The deserialize message
+
+        """
+        self.tcp_server.send_unity_message(self.topic, data)
+        return self.msg
+
+    def unregister(self):
+        """
+
+        Returns:
+
+        """
+        self.destroy_subscription(self.subscription)
+        self.destroy_node()
diff --git a/autonav_ws/src/ros_tcp_endpoint_ros2/ros_tcp_endpoint/tcp_sender.py b/autonav_ws/src/ros_tcp_endpoint_ros2/ros_tcp_endpoint/tcp_sender.py
new file mode 100644
index 0000000..6efbbcc
--- /dev/null
+++ b/autonav_ws/src/ros_tcp_endpoint_ros2/ros_tcp_endpoint/tcp_sender.py
@@ -0,0 +1,238 @@
+#  Copyright 2020 Unity Technologies
+#
+#  Licensed under the Apache License, Version 2.0 (the "License");
+#  you may not use this file except in compliance with the License.
+#  You may obtain a copy of the License at
+#
+#      http://www.apache.org/licenses/LICENSE-2.0
+#
+#  Unless required by applicable law or agreed to in writing, software
+#  distributed under the License is distributed on an "AS IS" BASIS,
+#  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+#  See the License for the specific language governing permissions and
+#  limitations under the License.
+
+import rclpy
+import socket
+import time
+import threading
+import json
+
+from rclpy.node import Node
+from rclpy.serialization import deserialize_message
+from rclpy.serialization import serialize_message
+
+from .client import ClientThread
+from .thread_pauser import ThreadPauser
+
+# queue module was renamed between python 2 and 3
+try:
+    from queue import Queue
+    from queue import Empty
+except:
+    from Queue import Queue
+    from Queue import Empty
+
+
+class UnityTcpSender:
+    """
+    Sends messages to Unity.
+    """
+
+    def __init__(self, tcp_server):
+        # super().__init__(f'UnityTcpSender')
+
+        self.sender_id = 1
+        self.time_between_halt_checks = 5
+        self.tcp_server = tcp_server
+
+        # Each sender thread has its own queue: this is always the queue for the currently active thread.
+        self.queue = None
+        self.queue_lock = threading.Lock()
+
+        # variables needed for matching up unity service requests with responses
+        self.next_srv_id = 1001
+        self.srv_lock = threading.Lock()
+        self.services_waiting = {}
+
+    def send_unity_info(self, text):
+        if self.queue is not None:
+            command = SysCommand_Log()
+            command.text = text
+            serialized_bytes = ClientThread.serialize_command("__log", command)
+            self.queue.put(serialized_bytes)
+
+    def send_unity_warning(self, text):
+        if self.queue is not None:
+            command = SysCommand_Log()
+            command.text = text
+            serialized_bytes = ClientThread.serialize_command("__warn", command)
+            self.queue.put(serialized_bytes)
+
+    def send_unity_error(self, text):
+        if self.queue is not None:
+            command = SysCommand_Log()
+            command.text = text
+            serialized_bytes = ClientThread.serialize_command("__error", command)
+            self.queue.put(serialized_bytes)
+
+    def send_ros_service_response(self, srv_id, destination, response):
+        if self.queue is not None:
+            command = SysCommand_Service()
+            command.srv_id = srv_id
+            serialized_header = ClientThread.serialize_command("__response", command)
+            serialized_message = ClientThread.serialize_message(destination, response)
+            self.queue.put(b"".join([serialized_header, serialized_message]))
+
+    def send_unity_message(self, topic, message):
+        if self.queue is not None:
+            serialized_message = ClientThread.serialize_message(topic, message)
+            self.queue.put(serialized_message)
+
+    def send_unity_service_request(self, topic, service_class, request):
+        if self.queue is None:
+            return None
+
+        thread_pauser = ThreadPauser()
+        with self.srv_lock:
+            srv_id = self.next_srv_id
+            self.next_srv_id += 1
+            self.services_waiting[srv_id] = thread_pauser
+
+        command = SysCommand_Service()
+        command.srv_id = srv_id
+        serialized_header = ClientThread.serialize_command("__request", command)
+        serialized_message = ClientThread.serialize_message(topic, request)
+        self.queue.put(b"".join([serialized_header, serialized_message]))
+
+        # rospy starts a new thread for each service request,
+        # so it won't break anything if we sleep now while waiting for the response
+        thread_pauser.sleep_until_resumed()
+
+        response = deserialize_message(thread_pauser.result, service_class.Response())
+        return response
+
+    def send_unity_service_response(self, srv_id, data):
+        thread_pauser = None
+        with self.srv_lock:
+            thread_pauser = self.services_waiting[srv_id]
+            del self.services_waiting[srv_id]
+
+        thread_pauser.resume_with_result(data)
+
+    def get_registered_topic(self, topic):
+        if topic in self.tcp_server.publishers_table:
+            return self.tcp_server.publishers_table[topic]
+        elif topic in self.tcp_server.subscribers_table:
+            return self.tcp_server.subscribers_table[topic]
+        elif topic in self.tcp_server.ros_services_table:
+            return self.tcp_server.ros_services_table[topic]
+        elif topic in self.tcp_server.unity_services_table:
+            return self.tcp_server.unity_services_table[topic]
+        else:
+            return None
+
+    def send_topic_list(self):
+        if self.queue is not None:
+            topic_list = SysCommand_TopicsResponse()
+            topics_and_types = self.tcp_server.get_topic_names_and_types()
+            topic_list.topics = [item[0] for item in topics_and_types]
+            for i in topics_and_types:
+                node = self.get_registered_topic(i[0])
+                if len(i[1]) > 1:
+                    if node is not None:
+                        self.tcp_server.get_logger().warning(
+                            "Only one message type per topic is supported, but found multiple types for topic {}; maintaining {} as the subscribed type.".format(
+                                i[0], self.parse_message_name(node.msg)
+                            )
+                        )
+                topic_list.types = [
+                    item[1][0].replace("/msg/", "/")
+                    if (len(item[1]) <= 1)
+                    else self.parse_message_name(node.msg)
+                    for item in topics_and_types
+                ]
+            serialized_bytes = ClientThread.serialize_command("__topic_list", topic_list)
+            self.queue.put(serialized_bytes)
+
+    def start_sender(self, conn, halt_event):
+        sender_thread = threading.Thread(
+            target=self.sender_loop, args=(conn, self.sender_id, halt_event)
+        )
+        self.sender_id += 1
+
+        # Exit the server thread when the main thread terminates
+        sender_thread.daemon = True
+        sender_thread.start()
+
+    def sender_loop(self, conn, tid, halt_event):
+        s = None
+        local_queue = Queue()
+
+        # send a handshake message to confirm the connection and version number
+        handshake_metadata = SysCommand_Handshake_Metadata()
+        handshake = SysCommand_Handshake(handshake_metadata)
+        local_queue.put(ClientThread.serialize_command("__handshake", handshake))
+
+        with self.queue_lock:
+            self.queue = local_queue
+
+        try:
+            while not halt_event.is_set():
+                try:
+                    item = local_queue.get(timeout=self.time_between_halt_checks)
+                except Empty:
+                    # I'd like to just wait on the queue, but we also need to check occasionally for the connection being closed
+                    # (otherwise the thread never terminates.)
+                    continue
+
+                # print("Sender {} sending an item".format(tid))
+
+                try:
+                    conn.sendall(item)
+                except Exception as e:
+                    self.tcp_server.logerr("Exception {}".format(e))
+                    break
+        finally:
+            halt_event.set()
+            with self.queue_lock:
+                if self.queue is local_queue:
+                    self.queue = None
+
+    def parse_message_name(self, name):
+        try:
+            # Example input string: <class 'std_msgs.msg._string.Metaclass_String'>
+            names = (str(type(name))).split(".")
+            module_name = names[0][8:]
+            class_name = names[-1].split("_")[-1][:-2]
+            return "{}/{}".format(module_name, class_name)
+        except (IndexError, AttributeError, ImportError) as e:
+            self.tcp_server.logerr("Failed to resolve message name: {}".format(e))
+            return None
+
+
+class SysCommand_Log:
+    def __init__(self):
+        text = ""
+
+
+class SysCommand_Service:
+    def __init__(self):
+        srv_id = 0
+
+
+class SysCommand_TopicsResponse:
+    def __init__(self):
+        topics = []
+        types = []
+
+
+class SysCommand_Handshake:
+    def __init__(self, metadata):
+        self.version = "v0.7.0"
+        self.metadata = json.dumps(metadata.__dict__)
+
+
+class SysCommand_Handshake_Metadata:
+    def __init__(self):
+        self.protocol = "ROS2"
diff --git a/autonav_ws/src/ros_tcp_endpoint_ros2/ros_tcp_endpoint/thread_pauser.py b/autonav_ws/src/ros_tcp_endpoint_ros2/ros_tcp_endpoint/thread_pauser.py
new file mode 100644
index 0000000..a2ad543
--- /dev/null
+++ b/autonav_ws/src/ros_tcp_endpoint_ros2/ros_tcp_endpoint/thread_pauser.py
@@ -0,0 +1,15 @@
+import threading
+
+class ThreadPauser:
+    def __init__(self):
+        self.condition = threading.Condition()
+        self.result = None
+	
+    def sleep_until_resumed(self):
+        with self.condition:
+            self.condition.wait()
+
+    def resume_with_result(self, result):
+        self.result = result
+        with self.condition:
+            self.condition.notify()
diff --git a/autonav_ws/src/ros_tcp_endpoint_ros2/ros_tcp_endpoint/unity_service.py b/autonav_ws/src/ros_tcp_endpoint_ros2/ros_tcp_endpoint/unity_service.py
new file mode 100644
index 0000000..c23b2fa
--- /dev/null
+++ b/autonav_ws/src/ros_tcp_endpoint_ros2/ros_tcp_endpoint/unity_service.py
@@ -0,0 +1,65 @@
+#  Copyright 2020 Unity Technologies
+#
+#  Licensed under the Apache License, Version 2.0 (the "License");
+#  you may not use this file except in compliance with the License.
+#  You may obtain a copy of the License at
+#
+#      http://www.apache.org/licenses/LICENSE-2.0
+#
+#  Unless required by applicable law or agreed to in writing, software
+#  distributed under the License is distributed on an "AS IS" BASIS,
+#  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+#  See the License for the specific language governing permissions and
+#  limitations under the License.
+
+import rclpy
+import socket
+import re
+
+from .communication import RosReceiver
+from .client import ClientThread
+
+
+class UnityService(RosReceiver):
+    """
+    Class to register a ROS service that's implemented in Unity.
+    """
+
+    def __init__(self, topic, service_class, tcp_server, queue_size=10):
+        """
+
+        Args:
+            topic:         Topic name to publish messages to
+            service_class: The message class in catkin workspace
+            queue_size:    Max number of entries to maintain in an outgoing queue
+        """
+        strippedTopic = re.sub("[^A-Za-z0-9_]+", "", topic)
+        node_name = f"{strippedTopic}_service"
+        RosReceiver.__init__(self, node_name)
+
+        self.topic = topic
+        self.node_name = node_name
+        self.service_class = service_class
+        self.tcp_server = tcp_server
+        self.queue_size = queue_size
+
+        self.service = self.create_service(self.service_class, self.topic, self.send)
+
+    def send(self, request, response):
+        """
+        Connect to TCP endpoint on client, pass along message and get reply
+        Args:
+            data: message data to send outside of ROS network
+
+        Returns:
+            The response message
+        """
+        return self.tcp_server.send_unity_service(self.topic, self.service_class, request)
+
+    def unregister(self):
+        """
+
+        Returns:
+
+        """
+        self.destroy_node()
diff --git a/autonav_ws/src/ros_tcp_endpoint_ros2/setup.cfg b/autonav_ws/src/ros_tcp_endpoint_ros2/setup.cfg
new file mode 100644
index 0000000..1525691
--- /dev/null
+++ b/autonav_ws/src/ros_tcp_endpoint_ros2/setup.cfg
@@ -0,0 +1,4 @@
+[develop]
+script-dir=$base/lib/ros_tcp_endpoint
+[install]
+install-scripts=$base/lib/ros_tcp_endpoint
diff --git a/autonav_ws/src/ros_tcp_endpoint_ros2/setup.py b/autonav_ws/src/ros_tcp_endpoint_ros2/setup.py
new file mode 100644
index 0000000..0ec66d4
--- /dev/null
+++ b/autonav_ws/src/ros_tcp_endpoint_ros2/setup.py
@@ -0,0 +1,29 @@
+import os
+
+from setuptools import setup
+
+package_name = "ros_tcp_endpoint"
+share_dir = os.path.join("share", package_name)
+
+setup(
+    name=package_name,
+    version="0.0.1",
+    packages=[package_name],
+    data_files=[
+        ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
+        (share_dir, ["package.xml"]),
+        (os.path.join(share_dir, "launch"), ["launch/endpoint.py"]),
+    ],
+    install_requires=["setuptools"],
+    zip_safe=True,
+    maintainer="Unity Robotics",
+    maintainer_email="unity-robotics@unity3d.com",
+    description="ROS TCP Endpoint Unity Integration (ROS2 version)",
+    license="Apache 2.0",
+    tests_require=["pytest"],
+    entry_points={
+        "console_scripts": [
+            "default_server_endpoint = ros_tcp_endpoint.default_server_endpoint:main"
+        ]
+    },
+)