diff --git a/src/perception/tag_detector/long_range_tag_detector.cpp b/src/perception/tag_detector/long_range_tag_detector.cpp
new file mode 100644
index 000000000..86ab376b4
--- /dev/null
+++ b/src/perception/tag_detector/long_range_tag_detector.cpp
@@ -0,0 +1,156 @@
+#include "tag_detector.hpp"
+
+namespace mrover {
+
+    void TagDetectorNodelet::onInit() {
+        mNh = getMTNodeHandle();
+        mPnh = getMTPrivateNodeHandle();
+        mDetectorParams = new cv::aruco::DetectorParameters();
+        auto defaultDetectorParams = cv::aruco::DetectorParameters::create();
+        int dictionaryNumber;
+
+        mPnh.param<bool>("publish_images", mPublishImages, true);
+        using DictEnumType = std::underlying_type_t<cv::aruco::PREDEFINED_DICTIONARY_NAME>;
+        mPnh.param<int>("dictionary", dictionaryNumber, static_cast<DictEnumType>(cv::aruco::DICT_4X4_50));
+        mPnh.param<int>("min_hit_count_before_publish", mMinHitCountBeforePublish, 5);
+        mPnh.param<int>("max_hit_count", mMaxHitCount, 5);
+        mPnh.param<int>("tag_increment_weight", mTagIncrementWeight, 2);
+        mPnh.param<int>("tag_decrement_weight", mTagDecrementWeight, 1);
+
+        mIt.emplace(mNh);
+        mImgPub = mIt->advertise("long_range_tag_detection", 1);
+        mDictionary = cv::aruco::getPredefinedDictionary(dictionaryNumber);
+
+        mPcSub = mNh.subscribe("zoom_cam/image", 1, &LongRangeTagDetectorNodelet::imageCallback, this);
+        mServiceEnableDetections = mNh.advertiseService("enable_detections", &TagDetectorNodelet::enableDetectionsCallback, this);
+
+        // Lambda handles passing class pointer (implicit first parameter) to configCallback
+        mCallbackType = [this](mrover::DetectorParamsConfig& config, uint32_t level) { configCallback(config, level); };
+        mConfigServer.setCallback(mCallbackType);
+
+        mPnh.param<double>("adaptiveThreshConstant",
+                           mDetectorParams->adaptiveThreshConstant, defaultDetectorParams->adaptiveThreshConstant);
+        mPnh.param<int>("adaptiveThreshWinSizeMax",
+                        mDetectorParams->adaptiveThreshWinSizeMax, defaultDetectorParams->adaptiveThreshWinSizeMax);
+        mPnh.param<int>("adaptiveThreshWinSizeMin",
+                        mDetectorParams->adaptiveThreshWinSizeMin, defaultDetectorParams->adaptiveThreshWinSizeMin);
+        mPnh.param<int>("adaptiveThreshWinSizeStep",
+                        mDetectorParams->adaptiveThreshWinSizeStep, defaultDetectorParams->adaptiveThreshWinSizeStep);
+        mPnh.param<int>("cornerRefinementMaxIterations",
+                        mDetectorParams->cornerRefinementMaxIterations,
+                        defaultDetectorParams->cornerRefinementMaxIterations);
+        mPnh.param<double>("cornerRefinementMinAccuracy",
+                           mDetectorParams->cornerRefinementMinAccuracy,
+                           defaultDetectorParams->cornerRefinementMinAccuracy);
+        mPnh.param<int>("cornerRefinementWinSize",
+                        mDetectorParams->cornerRefinementWinSize, defaultDetectorParams->cornerRefinementWinSize);
+
+        bool doCornerRefinement = true;
+        mPnh.param<bool>("doCornerRefinement", doCornerRefinement, true);
+        if (doCornerRefinement) {
+            bool cornerRefinementSubPix = true;
+            mPnh.param<bool>("cornerRefinementSubPix", cornerRefinementSubPix, true);
+            if (cornerRefinementSubPix) {
+                mDetectorParams->cornerRefinementMethod = cv::aruco::CORNER_REFINE_SUBPIX;
+            } else {
+                mDetectorParams->cornerRefinementMethod = cv::aruco::CORNER_REFINE_CONTOUR;
+            }
+        } else {
+            mDetectorParams->cornerRefinementMethod = cv::aruco::CORNER_REFINE_NONE;
+        }
+
+        mPnh.param<double>("errorCorrectionRate",
+                           mDetectorParams->errorCorrectionRate, defaultDetectorParams->errorCorrectionRate);
+        mPnh.param<double>("minCornerDistanceRate",
+                           mDetectorParams->minCornerDistanceRate, defaultDetectorParams->minCornerDistanceRate);
+        mPnh.param<int>("markerBorderBits",
+                        mDetectorParams->markerBorderBits, defaultDetectorParams->markerBorderBits);
+        mPnh.param<double>("maxErroneousBitsInBorderRate",
+                           mDetectorParams->maxErroneousBitsInBorderRate,
+                           defaultDetectorParams->maxErroneousBitsInBorderRate);
+        mPnh.param<int>("minDistanceToBorder",
+                        mDetectorParams->minDistanceToBorder, defaultDetectorParams->minDistanceToBorder);
+        mPnh.param<double>("minMarkerDistanceRate",
+                           mDetectorParams->minMarkerDistanceRate, defaultDetectorParams->minMarkerDistanceRate);
+        mPnh.param<double>("minMarkerPerimeterRate",
+                           mDetectorParams->minMarkerPerimeterRate, defaultDetectorParams->minMarkerPerimeterRate);
+        mPnh.param<double>("maxMarkerPerimeterRate",
+                           mDetectorParams->maxMarkerPerimeterRate, defaultDetectorParams->maxMarkerPerimeterRate);
+        mPnh.param<double>("minOtsuStdDev", mDetectorParams->minOtsuStdDev, defaultDetectorParams->minOtsuStdDev);
+        mPnh.param<double>("perspectiveRemoveIgnoredMarginPerCell",
+                           mDetectorParams->perspectiveRemoveIgnoredMarginPerCell,
+                           defaultDetectorParams->perspectiveRemoveIgnoredMarginPerCell);
+        mPnh.param<int>("perspectiveRemovePixelPerCell",
+                        mDetectorParams->perspectiveRemovePixelPerCell,
+                        defaultDetectorParams->perspectiveRemovePixelPerCell);
+        mPnh.param<double>("polygonalApproxAccuracyRate",
+                           mDetectorParams->polygonalApproxAccuracyRate,
+                           defaultDetectorParams->polygonalApproxAccuracyRate);
+
+        NODELET_INFO("Tag detection ready, use odom frame: %s, min hit count: %d, max hit count: %d, hit increment weight: %d, hit decrement weight: %d", mUseOdom ? "true" : "false", mMinHitCountBeforePublish, mMaxHitCount, mTagIncrementWeight, mTagDecrementWeight);
+    }
+
+    void TagDetectorNodelet::configCallback(mrover::DetectorParamsConfig& config, uint32_t level) {
+        // Don't load initial config, since it will overwrite the rosparam settings
+        if (level == std::numeric_limits<uint32_t>::max()) return;
+
+        mDetectorParams->adaptiveThreshConstant = config.adaptiveThreshConstant;
+        mDetectorParams->adaptiveThreshWinSizeMin = config.adaptiveThreshWinSizeMin;
+        mDetectorParams->adaptiveThreshWinSizeMax = config.adaptiveThreshWinSizeMax;
+        mDetectorParams->adaptiveThreshWinSizeStep = config.adaptiveThreshWinSizeStep;
+        mDetectorParams->cornerRefinementMaxIterations = config.cornerRefinementMaxIterations;
+        mDetectorParams->cornerRefinementMinAccuracy = config.cornerRefinementMinAccuracy;
+        mDetectorParams->cornerRefinementWinSize = config.cornerRefinementWinSize;
+        if (config.doCornerRefinement) {
+            if (config.cornerRefinementSubpix) {
+                mDetectorParams->cornerRefinementMethod = cv::aruco::CORNER_REFINE_SUBPIX;
+            } else {
+                mDetectorParams->cornerRefinementMethod = cv::aruco::CORNER_REFINE_CONTOUR;
+            }
+        } else {
+            mDetectorParams->cornerRefinementMethod = cv::aruco::CORNER_REFINE_NONE;
+        }
+        mDetectorParams->errorCorrectionRate = config.errorCorrectionRate;
+        mDetectorParams->minCornerDistanceRate = config.minCornerDistanceRate;
+        mDetectorParams->markerBorderBits = config.markerBorderBits;
+        mDetectorParams->maxErroneousBitsInBorderRate = config.maxErroneousBitsInBorderRate;
+        mDetectorParams->minDistanceToBorder = config.minDistanceToBorder;
+        mDetectorParams->minMarkerDistanceRate = config.minMarkerDistanceRate;
+        mDetectorParams->minMarkerPerimeterRate = config.minMarkerPerimeterRate;
+        mDetectorParams->maxMarkerPerimeterRate = config.maxMarkerPerimeterRate;
+        mDetectorParams->minOtsuStdDev = config.minOtsuStdDev;
+        mDetectorParams->perspectiveRemoveIgnoredMarginPerCell = config.perspectiveRemoveIgnoredMarginPerCell;
+        mDetectorParams->perspectiveRemovePixelPerCell = config.perspectiveRemovePixelPerCell;
+        mDetectorParams->polygonalApproxAccuracyRate = config.polygonalApproxAccuracyRate;
+    }
+
+    bool TagDetectorNodelet::enableDetectionsCallback(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) {
+        mEnableDetections = req.data;
+        if (mEnableDetections) {
+            res.message = "Enabled tag detections.";
+            NODELET_INFO("Enabled tag detections.");
+        } else {
+            res.message = "Disabled tag detections.";
+            NODELET_INFO("Disabled tag detections.");
+        }
+
+        res.success = true;
+        return true;
+    }
+
+} // namespace mrover
+
+int main(int argc, char** argv) {
+    ros::init(argc, argv, "tag_detector");
+
+    // Start the ZED Nodelet
+    nodelet::Loader nodelet;
+    nodelet.load(ros::this_node::getName(), "mrover/TagDetectorNodelet", ros::names::getRemappings(), {});
+
+    ros::spin();
+
+    return EXIT_SUCCESS;
+}
+
+#include <pluginlib/class_list_macros.h>
+PLUGINLIB_EXPORT_CLASS(mrover::TagDetectorNodelet, nodelet::Nodelet)
diff --git a/src/perception/tag_detector/long_range_tag_detector.hpp b/src/perception/tag_detector/long_range_tag_detector.hpp
new file mode 100644
index 000000000..bfeb2706e
--- /dev/null
+++ b/src/perception/tag_detector/long_range_tag_detector.hpp
@@ -0,0 +1,45 @@
+namespace mrover {
+
+    class LongRangeTagDetectorNodelet : public nodelet::Nodelet {
+        ros::NodeHandle mNh, mPnh;
+
+        std::optional<image_transport::ImageTransport> mIt;
+        image_transport::Publisher mImgPub;
+        std::unordered_map<int, image_transport::Publisher> mThreshPubs; // Map from threshold scale to publisher
+        ros::ServiceServer mServiceEnableDetections;
+
+        image_transport::Subscriber mImgSub;
+
+        bool mEnableDetections = true;
+        bool mUseOdom{};
+        std::string mOdomFrameId, mMapFrameId, mCameraFrameId;
+        bool mPublishImages{}; // If set, we publish the images with the tags drawn on top
+        int mMinHitCountBeforePublish{};
+        int mMaxHitCount{};
+        int mTagIncrementWeight{};
+        int mTagDecrementWeight{};
+
+        cv::Ptr<cv::aruco::DetectorParameters> mDetectorParams;
+        cv::Ptr<cv::aruco::Dictionary> mDictionary;
+
+        cv::Mat mImg;
+        cv::Mat mGrayImg;
+        sensor_msgs::Image mImgMsg;
+        sensor_msgs::Image mThreshMsg;
+        uint32_t mSeqNum{};
+        std::optional<size_t> mPrevDetectedCount; // Log spam prevention
+        std::vector<std::vector<cv::Point2f>> mImmediateCorners;
+        std::vector<int> mImmediateIds;
+        std::unordered_map<int, Tag> mTags;
+        dynamic_reconfigure::Server<mrover::DetectorParamsConfig> mConfigServer;
+        dynamic_reconfigure::Server<mrover::DetectorParamsConfig>::CallbackType mCallbackType;
+
+        LoopProfiler mProfiler{"Tag Detector"};
+
+        void onInit() override;
+
+        void publishThresholdedImage();
+
+        std::optional<SE3> getTagInCamFromPixel(sensor_msgs::PointCloud2ConstPtr const& cloudPtr, size_t u, size_t v);       
+    }
+}
\ No newline at end of file
diff --git a/src/perception/tag_detector/long_range_tag_detector.processing.cpp b/src/perception/tag_detector/long_range_tag_detector.processing.cpp
new file mode 100644
index 000000000..14b60ef2c
--- /dev/null
+++ b/src/perception/tag_detector/long_range_tag_detector.processing.cpp
@@ -0,0 +1,21 @@
+#include "long_range_tag_detector.hpp"
+
+#include "../point.hpp"
+
+namespace mrover {
+
+    /**
+     * Detects tags in an image, draws the detected markers onto the image, and publishes them to /long_range_tag_detection
+     *
+     * @param msg   image message
+     */
+    void TagDetectorNodelet::imageCallback(sensor_msgs::Image const& msg) {
+        // 1. Detect tags
+        // 2. Update the hit counts of the tags in the mTags map
+        // 3. We only want to publish the tags if the topic has subscribers
+        if (mPublishImages && mImgPub.getNumSubscribers()) {
+            // Draw the tags on the image using OpenCV
+        }
+    }
+
+} // namespace mrover
\ No newline at end of file
diff --git a/starter_project/autonomy/AutonomyStarterProject.cmake b/starter_project/autonomy/AutonomyStarterProject.cmake
index 4b84c75af..12e906eaa 100644
--- a/starter_project/autonomy/AutonomyStarterProject.cmake
+++ b/starter_project/autonomy/AutonomyStarterProject.cmake
@@ -8,6 +8,7 @@ add_message_files(
         DIRECTORY
         ${CMAKE_CURRENT_LIST_DIR}/msg
         FILES
+        StarterProjectTag.msg
         #add new messages here
 )
 
diff --git a/starter_project/autonomy/launch/starter_project.launch b/starter_project/autonomy/launch/starter_project.launch
index a0b8ce28c..d270dc0f8 100644
--- a/starter_project/autonomy/launch/starter_project.launch
+++ b/starter_project/autonomy/launch/starter_project.launch
@@ -37,13 +37,13 @@ starter_project/
     Navigation
     ===========
   -->
-    <!-- <node name="nav" pkg="mrover" type="navigation_starter_project.py" />-->
+    <node name="nav" pkg="mrover" type="navigation_starter_project.py" />
 
   <!--
     ============
     Localization
     ============
   -->
-  <!-- <node name="localization" pkg="mrover" type="localization.py" output="screen" /> -->
+  <node name="localization" pkg="mrover" type="localization.py" output="screen" />
 
 </launch>
diff --git a/starter_project/autonomy/msg/StarterProjectTag.msg b/starter_project/autonomy/msg/StarterProjectTag.msg
new file mode 100644
index 000000000..fb3b6f843
--- /dev/null
+++ b/starter_project/autonomy/msg/StarterProjectTag.msg
@@ -0,0 +1,4 @@
+int32 tagId
+float32 xTagCenterPixel
+float32 yTagCenterPixel
+float32 closenessMetric
\ No newline at end of file
diff --git a/starter_project/autonomy/src/localization.py b/starter_project/autonomy/src/localization.py
index b8ff5423a..7a7f00ecb 100755
--- a/starter_project/autonomy/src/localization.py
+++ b/starter_project/autonomy/src/localization.py
@@ -17,10 +17,17 @@
 class Localization:
     pose: SE3
 
+    gps_sub: rospy.Subscriber
+    imu_sub: rospy.Subscriber
+
+
     def __init__(self):
         # create subscribers for GPS and IMU data, linking them to our callback functions
         # TODO
 
+        self.gps_sub = rospy.Subscriber("/gps/fix", NavSatFix, self.gps_callback)
+        self.imu_sub = rospy.Subscriber("/imu/imu_only", Imu, self.imu_callback)
+
         # create a transform broadcaster for publishing to the TF tree
         self.tf_broadcaster = tf2_ros.TransformBroadcaster()
 
@@ -34,7 +41,17 @@ def gps_callback(self, msg: NavSatFix):
         convert it to cartesian coordinates, store that value in `self.pose`, then publish
         that pose to the TF tree.
         """
-        # TODO
+
+        latlong = np.array([msg.latitude, msg.longitude])
+        reference = np.array([42.293195, -83.7096706])
+
+        xyz = Localization.spherical_to_cartesian(latlong, reference)
+
+        self.pose = SE3.from_pos_quat(xyz.copy(), self.pose.rotation.quaternion.copy())
+
+        self.pose.publish_to_tf_tree(self.tf_broadcaster, "map", "base_link")
+
+
 
     def imu_callback(self, msg: Imu):
         """
@@ -42,7 +59,12 @@ def imu_callback(self, msg: Imu):
         on the /imu topic. It should read the orientation data from the given Imu message,
         store that value in `self.pose`, then publish that pose to the TF tree.
         """
-        # TODO
+
+        rot = np.array([msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w])
+
+        self.pose = SE3.from_pos_quat(self.pose.position.copy(), rot.copy())
+
+        self.pose.publish_to_tf_tree(self.tf_broadcaster, "map", "base_link")
 
     @staticmethod
     def spherical_to_cartesian(spherical_coord: np.ndarray, reference_coord: np.ndarray) -> np.ndarray:
@@ -56,8 +78,12 @@ def spherical_to_cartesian(spherical_coord: np.ndarray, reference_coord: np.ndar
                                 given as a numpy array [latitude, longitude]
         :returns: the approximated cartesian coordinates in meters, given as a numpy array [x, y, z]
         """
-        # TODO
+        R = 6371000 #CIRCUMF of earth
+
+        x = R*(np.radians(spherical_coord[1]) - np.radians(reference_coord[1])) * np.cos(np.radians(reference_coord[0]))
+        y = R*(np.radians(spherical_coord[0]) - np.radians(reference_coord[0]))
 
+        return np.array([x, y, 0]) # rotate by 90 degrees
 
 def main():
     # initialize the node
diff --git a/starter_project/autonomy/src/navigation/context.py b/starter_project/autonomy/src/navigation/context.py
index 4bbd06f61..afa9f6887 100644
--- a/starter_project/autonomy/src/navigation/context.py
+++ b/starter_project/autonomy/src/navigation/context.py
@@ -18,15 +18,21 @@ class Rover:
 
     def get_pose(self) -> Optional[SE3]:
         # TODO: return the pose of the rover (or None if we don't have one (catch exception))
-        pass
-
+        pose = None
+        try:
+            pose = SE3.from_tf_tree(self.ctx.tf_buffer, "map", "base_link")
+        except:
+            pose = None
+            print("No SE3 Pose")
+        finally:
+            return pose
+    
     def send_drive_command(self, twist: Twist):
         # TODO: send the Twist message to the rover
-        pass
+        self.ctx.vel_cmd_publisher.publish(twist)
 
     def send_drive_stop(self):
-        # TODO: tell the rover to stop
-        pass
+        self.send_drive_command(Twist())
 
 
 @dataclass
@@ -40,8 +46,7 @@ class Environment:
     fid_pos: Optional[StarterProjectTag]
 
     def receive_fid_data(self, message: StarterProjectTag):
-        # TODO: handle incoming FID data messages here
-        pass
+        self.fid_pos = message
 
     def get_fid_data(self) -> Optional[StarterProjectTag]:
         """
@@ -49,7 +54,10 @@ def get_fid_data(self) -> Optional[StarterProjectTag]:
         if it exists, otherwise returns None
         """
         # TODO: return either None or your position message
-        pass
+        if self.fid_pos is not None:
+            return self.fid_pos
+        
+        return None
 
 
 class Context:
diff --git a/starter_project/autonomy/src/navigation/drive_state.py b/starter_project/autonomy/src/navigation/drive_state.py
index bae1d07f1..3c4d3bfab 100644
--- a/starter_project/autonomy/src/navigation/drive_state.py
+++ b/starter_project/autonomy/src/navigation/drive_state.py
@@ -10,21 +10,29 @@ def __init__(self, context: Context):
         super().__init__(
             context,
             # TODO:
-            add_outcomes=["TODO: add outcomes here"],
+            add_outcomes=["driving_to_point", "reached_point"],
         )
 
     def evaluate(self, ud):
         target = np.array([5.5, 2.0, 0.0])
 
         # TODO: get the rover's pose, if it doesn't exist stay in DriveState (with outcome "driving_to_point")
+        pose = self.context.rover.get_pose()
+
+        if not pose:
+            return "driving_to_point"
 
         # TODO: get the drive command and completion status based on target and pose
         # (HINT: use get_drive_command(), with completion_thresh set to 0.7 and turn_in_place_thresh set to 0.2)
 
+        res = get_drive_command(target, pose, .7, .2)
+
         # TODO: if we are finished getting to the target, go to TagSeekState (with outcome "reached_point")
+        if res[1]:
+            return "reached_point"
 
         # TODO: send the drive command to the rover
+        self.context.rover.send_drive_command(res[0])
 
         # TODO: tell smach to stay in the DriveState by returning with outcome "driving_to_point"
-
-        pass
+        return "driving_to_point"
diff --git a/starter_project/autonomy/src/navigation/navigation_starter_project.py b/starter_project/autonomy/src/navigation/navigation_starter_project.py
index a3f00f0dd..e8f027455 100755
--- a/starter_project/autonomy/src/navigation/navigation_starter_project.py
+++ b/starter_project/autonomy/src/navigation/navigation_starter_project.py
@@ -30,7 +30,15 @@ def __init__(self, context: Context):
         self.sis.start()
         with self.state_machine:
             # TODO: add DriveState and its transitions here
-
+            self.state_machine.add("DriveState",
+                                   DriveState(self.context),
+                                   transitions={"driving_to_point": "DriveState", "reached_point": "TagSeekState"}) 
+            
+            self.state_machine.add("TagSeekState",
+                                   TagSeekState(self.context),
+                                   transitions={"failure": "DoneState",
+                                                "success": "DoneState",
+                                                "working": "TagSeekState"})
             # DoneState and its transitions
             self.state_machine.add(
                 "DoneState",
@@ -53,6 +61,7 @@ def stop(self):
 
 def main():
     # TODO: init a node called "navigation"
+    rospy.init_node("navigation")
 
     # context and navigation objects
     context = Context()
diff --git a/starter_project/autonomy/src/navigation/tag_seek.py b/starter_project/autonomy/src/navigation/tag_seek.py
index 577731516..c8c35ef4b 100644
--- a/starter_project/autonomy/src/navigation/tag_seek.py
+++ b/starter_project/autonomy/src/navigation/tag_seek.py
@@ -9,22 +9,36 @@ def __init__(self, context: Context):
         super().__init__(
             context,
             # TODO: add outcomes
-            add_outcomes=["TODO: add outcomes here"],
+            add_outcomes=["failure", "success", "working"],
         )
 
     def evaluate(self, ud):
         DISTANCE_TOLERANCE = 0.99
         ANUGLAR_TOLERANCE = 0.3
         # TODO: get the tag's location and properties (HINT: use get_fid_data() from context.env)
+        tag_loc = self.context.env.get_fid_data()
 
         # TODO: if we don't have a tag: go to the DoneState (with outcome "failure")
+        if tag_loc is None:
+            return "failure"
 
         # TODO: if we are within angular and distance tolerances: go to DoneState (with outcome "success")
+        in_dist_tol = (1 - tag_loc.closenessMetric) > DISTANCE_TOLERANCE
+        in_ang_tol = (tag_loc.x / 10.0) < ANUGLAR_TOLERANCE
+
+        if in_dist_tol and in_ang_tol:
+            return "success"
 
         # TODO: figure out the Twist command to be applied to move the rover closer to the tag
+        twist_command = Twist()
+        if not in_dist_tol:
+            twist_command.linear.x = tag_loc.closenessMetric
+        if not in_ang_tol:
+            twist_command.angular.z = -1 * tag_loc.x
 
         # TODO: send Twist command to rover
+        self.context.rover.send_drive_command(twist_command)
 
         # TODO: stay in the TagSeekState (with outcome "working")
 
-        pass
+        return "working"
diff --git a/starter_project/autonomy/src/perception.cpp b/starter_project/autonomy/src/perception.cpp
index 2185a5fe2..7e0cad39e 100644
--- a/starter_project/autonomy/src/perception.cpp
+++ b/starter_project/autonomy/src/perception.cpp
@@ -26,10 +26,13 @@ namespace mrover {
         // Create a publisher for our tag topic
         // See: http://wiki.ros.org/ROS/Tutorials/WritingPublisherSubscriber%28c%2B%2B%29
         // TODO: uncomment me!
-        // mTagPublisher = mNodeHandle.advertise<StarterProjectTag>("tag", 1);
+        mTagPublisher = mNodeHandle.advertise<StarterProjectTag>("tag", 1);
 
         mTagDetectorParams = cv::aruco::DetectorParameters::create();
         mTagDictionary = cv::aruco::getPredefinedDictionary(0);
+
+        Perception::image_height = 0;
+        Perception::image_width = 0;
     }
 
     void Perception::imageCallback(sensor_msgs::ImageConstPtr const& image) {
@@ -39,6 +42,10 @@ namespace mrover {
         cv::Mat cvImage{static_cast<int>(image->height), static_cast<int>(image->width),
                         CV_8UC3, const_cast<uint8_t*>(image->data.data())};
         // Detect tags in the image pixels
+
+        Perception::image_width = image->width;
+        Perception::image_height = image->height;
+
         findTagsInImage(cvImage, mTags);
         // Select the tag that is closest to the middle of the screen
         StarterProjectTag tag = selectTag(mTags);
@@ -54,15 +61,60 @@ namespace mrover {
         tags.clear(); // Clear old tags in output vector
 
         // TODO: implement me!
+        // cv::aruco::ArucoDetector detector = cv::aruco::ArucoDetector(mTagDictionary, mTagDetectorParams);
+
+        cv::aruco::detectMarkers(image, mTagDictionary, mTagCorners, mTagIds, mTagDetectorParams);
+
+        std::vector<float> closeness_metrics;
+        std::vector<std::pair<float, float>> centers;
+
+        for (std::vector<cv::Point2f> &corner_set : mTagCorners) {
+            closeness_metrics.push_back(getClosenessMetricFromTagCorners(image, corner_set));
+            centers.push_back(getCenterFromTagCorners(corner_set));
+        }
+
+        for (size_t i = 0; i < closeness_metrics.size(); i++) {
+            StarterProjectTag tag;
+
+            tag.tagId = mTagIds[i];
+            tag.xTagCenterPixel = centers[i].first;
+            tag.yTagCenterPixel = centers[i].second;
+            tag.closenessMetric = closeness_metrics[i];
+
+            tags.push_back(tag);
+        }
+
+
     }
 
-    StarterProjectTag Perception::selectTag(std::vector<StarterProjectTag> const& tags) { // NOLINT(*-convert-member-functions-to-static)
-        // TODO: implement me!
-        return {};
+    // double Perception::get_center_offset_magnitude_sq(float x, float y) const {
+    //     double x_offset = x - (double(Perception::image_width) / 2.0);
+    //     double y_offset = y - (double(Perception::image_height) / 2.0);
+
+    //     return x_offset*x_offset + y_offset*y_offset;
+    // }
+
+    StarterProjectTag Perception::selectTag(std::vector<StarterProjectTag> const& tags) const { // NOLINT(*-convert-member-functions-to-static)
+        if (tags.empty())
+            return {};
+
+        StarterProjectTag best = tags[0];
+        double best_dist = best.xTagCenterPixel*best.xTagCenterPixel + best.yTagCenterPixel*best.yTagCenterPixel;
+
+        for (int i = 1; i < tags.size(); i++) {
+            if ((tags[i].xTagCenterPixel*tags[i].xTagCenterPixel + tags[i].yTagCenterPixel*tags[i].yTagCenterPixel) < best_dist) {
+                best_dist = tags[i].xTagCenterPixel*tags[i].xTagCenterPixel + tags[i].yTagCenterPixel*tags[i].yTagCenterPixel;
+                best = tags[i];
+            }
+        }
+
+        return best;
     }
 
     void Perception::publishTag(StarterProjectTag const& tag) {
         // TODO: implement me!
+        Perception::mTagPublisher.publish(tag);
+        
     }
 
     float Perception::getClosenessMetricFromTagCorners(cv::Mat const& image, std::vector<cv::Point2f> const& tagCorners) { // NOLINT(*-convert-member-functions-to-static)
@@ -71,12 +123,28 @@ namespace mrover {
         // hint: try not overthink, this metric does not have to be perfectly accurate, just correlated to distance away from a tag
 
         // TODO: implement me!
-        return {};
+        //return percent of screen taken off by corners
+        //assume the tag is a square?
+        int total_area = image.rows * image.cols;
+        
+        float corner_area = abs((tagCorners[1].x - tagCorners[0].x) * (tagCorners[0].y - tagCorners[3].y));
+
+        return 1 - float((1.0*corner_area) / (1.0*total_area));
     }
 
-    std::pair<float, float> Perception::getCenterFromTagCorners(std::vector<cv::Point2f> const& tagCorners) { // NOLINT(*-convert-member-functions-to-static)
+    std::pair<float, float> Perception::getCenterFromTagCorners(std::vector<cv::Point2f> const& tagCorners) const { // NOLINT(*-convert-member-functions-to-static)
         // TODO: implement me!
-        return {};
+        float center_x = float(tagCorners[0].x + tagCorners[1].x) / float(2.0);
+        float center_y = float(tagCorners[0].y + tagCorners[3].y) / float(2.0);
+
+        center_x = float(Perception::image_width) - center_x;
+        center_y = float(Perception::image_height) - center_y;
+
+        std::pair<float, float> ret;
+        ret.first = center_x;
+        ret.second = center_y;
+
+        return ret;
     }
 
 } // namespace mrover
diff --git a/starter_project/autonomy/src/perception.hpp b/starter_project/autonomy/src/perception.hpp
index d690e2486..94067a462 100644
--- a/starter_project/autonomy/src/perception.hpp
+++ b/starter_project/autonomy/src/perception.hpp
@@ -7,6 +7,8 @@
 #include <unordered_map>
 #include <vector>
 
+#include <cmath>
+
 // OpenCV Headers, cv namespace
 #include <opencv2/aruco.hpp>
 #include <opencv2/core/mat.hpp>
@@ -18,6 +20,8 @@
 #include <sensor_msgs/Image.h>
 #include <sensor_msgs/PointCloud2.h>
 
+#include <ros/console.h>
+
 #if __has_include(<mrover/StarterProjectTag.h>)
 #include <mrover/StarterProjectTag.h>
 #else
@@ -45,6 +49,9 @@ namespace mrover {
         std::vector<StarterProjectTag> mTags;
         ros::Publisher mTagPublisher;
 
+        size_t image_width;
+        size_t image_height;
+
     public:
         Perception();
 
@@ -56,6 +63,11 @@ namespace mrover {
          */
         void imageCallback(sensor_msgs::ImageConstPtr const& image);
 
+        /**
+        *   Dumbass helper I wrote to do center offset
+        */
+        // [[nodiscard]] double get_center_offset_magnitude_sq(float x, float y) const;
+
         /**
          *  Given an image, detect ArUco tags, and fill a vector full of output messages.
          *
@@ -86,7 +98,7 @@ namespace mrover {
          * @param tagCorners    4-tuple of tag pixel coordinates representing the corners
          * @return              2-tuple (x,y) approximate center in pixel space
          */
-        [[nodiscard]] std::pair<float, float> getCenterFromTagCorners(std::vector<cv::Point2f> const& tagCorners);
+        [[nodiscard]] std::pair<float, float> getCenterFromTagCorners(std::vector<cv::Point2f> const& tagCorners) const;
 
         /**
          *  Select the tag closest to the center of the camera
@@ -94,7 +106,7 @@ namespace mrover {
          * @param tags          Vector of tags
          * @return              Center tag
          */
-        [[nodiscard]] StarterProjectTag selectTag(std::vector<StarterProjectTag> const& tags);
+        [[nodiscard]] StarterProjectTag selectTag(std::vector<StarterProjectTag> const& tags) const;
     };
 
 } // namespace mrover