diff --git a/src/pi/pi_main/pi_main/install_on_boot.py b/src/pi/pi_main/pi_main/install_on_boot.py
index e5f30eda..48808544 100644
--- a/src/pi/pi_main/pi_main/install_on_boot.py
+++ b/src/pi/pi_main/pi_main/install_on_boot.py
@@ -1,9 +1,10 @@
 import os
-import subprocess
 import sys
+import subprocess
+import pathlib
 
 from ament_index_python.packages import get_package_share_directory
-from robot_upstart import Job
+from robot_upstart.job import Job
 
 
 def main() -> None:
@@ -20,16 +21,25 @@ def main() -> None:
 
     os.symlink(launch_src, launch_dst)
 
-    udev_script = os.path.join(pi_main_share, 'udev_copy', 'udev_copy.py')
+    file_location = pathlib.Path(__file__).parent.resolve()
+    udev_script = os.path.join(file_location, 'udev_copy.py')
 
-    major_num = sys.version_info[0]
-    minor_num = sys.version_info[1]
-    udev_script = os.path.join(os.path.dirname(os.path.abspath(pi_main_share)), '../', 'lib',
-                               f'python{major_num}.{minor_num}',
-                               'site-packages', 'pi_main', 'udev_copy.py')
-    subprocess.call(['/usr/bin/sudo', 'python3', udev_script, pi_main_share])
+    cmd = ['/usr/bin/sudo', '/usr/bin/python3', udev_script, pi_main_share]
 
-    cwrubotix_job = Job(name='cwrubotix_pi', rmw='rmw_cyclonedds_cpp')
+    try:
+        p = subprocess.run(cmd, capture_output=True, check=True)
+    # Logs Error
+    except subprocess.CalledProcessError as e:
+        print(e.stderr)
+        sys.exit(1)
+
+    # Success Message
+    print(p.stdout.decode())
+
+    install_path = os.path.join(pi_main_share, "..", "..")
+    workspace_path = os.path.join(install_path, "setup.bash")
+    cwrubotix_job = Job(name='cwrubotix_pi', rmw='rmw_cyclonedds_cpp',
+                        workspace_setup=workspace_path)
     cwrubotix_job.symlink = True
     cwrubotix_job.uninstall()
     cwrubotix_job.add(package='pi_main', filename='launch/pi.launch.py')
diff --git a/src/pi/pi_main/pi_main/udev_copy.py b/src/pi/pi_main/pi_main/udev_copy.py
index 29c0ba17..539eaac4 100644
--- a/src/pi/pi_main/pi_main/udev_copy.py
+++ b/src/pi/pi_main/pi_main/udev_copy.py
@@ -9,3 +9,4 @@
     udev_dst_dir = os.path.join('/etc', 'udev', 'rules.d')
 
     shutil.copytree(udev_src_dir, udev_dst_dir, dirs_exist_ok=True)
+    print("Copying udev rules.")
diff --git a/src/pi/pi_main/setup.py b/src/pi/pi_main/setup.py
index 7bcaadac..1cbc9564 100644
--- a/src/pi/pi_main/setup.py
+++ b/src/pi/pi_main/setup.py
@@ -18,9 +18,7 @@
         (os.path.join('share', package_name, 'launch'),
          glob('launch/*launch.[pxy][yma]*')),
         (os.path.join('share', package_name, 'udev_rules'),
-         glob('udev_rules/*')),
-        (os.path.join('share', package_name, 'udev_copy'),
-         glob('pi_main/udev_copy.py'))
+         glob('udev_rules/*'))
     ],
     install_requires=['setuptools', 'flake8==4.0.1', 'mypy >= 1.7'],
     zip_safe=True,
diff --git a/src/pi/pi_main/test/test_install_on_boot.py b/src/pi/pi_main/test/test_install_on_boot.py
new file mode 100644
index 00000000..c37ffa54
--- /dev/null
+++ b/src/pi/pi_main/test/test_install_on_boot.py
@@ -0,0 +1,25 @@
+import os
+
+from pi_main.install_on_boot import main
+
+ROS_DISTRO = os.getenv("ROS_DISTRO")
+
+EXPECTED_SYSTEM_FILES = [f"/etc/ros/{ROS_DISTRO}/cwrubotix_pi.d/.installed_files",
+                         f"/etc/ros/{ROS_DISTRO}/cwrubotix_pi.d/pi.launch.py",
+                         "/usr/sbin/cwrubotix_pi-start",
+                         "/usr/sbin/cwrubotix_pi-stop"]
+
+
+def test_install_on_boot() -> None:
+    """Test that file copying and systemd are made."""
+    main()
+
+    # Test for rules files being copied correctly
+    # Could be split into a separate test but main() does both
+    actual_rules_files = set(os.listdir(os.path.join("/etc", "udev", "rules.d")))
+    expected_rules_files = set(["i2c.rules", "camera.rules", "pixhawk.rules"])
+    assert expected_rules_files.issubset(actual_rules_files)
+
+    # Checks for files created by robot_upstart
+    for file in EXPECTED_SYSTEM_FILES:
+        assert os.path.isfile(file)