umrover · MyCabbages4 · Jan 15, 2026 · Feb 5, 2026 · Feb 24, 2026 · Mar 12, 2026
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -289,7 +289,7 @@ if(ZED_FOUND AND CUDA_FOUND)
     # Object Detector
     mrover_add_component(object_detector perception/object_detector/*.c* perception/object_detector/pch.hpp StereoObjectDetector ImageObjectDetector)
     mrover_link_component(object_detector opencv_core opencv_dnn opencv_imgproc lie tbb tensorrt opencv_imgcodecs opencv_highgui loop_profiler parameter_utils cuda_compiler_flags)
-    mrover_ament_component(object_detector rclcpp sensor_msgs rclcpp_components CUDA tf2 tf2_ros)
+    mrover_ament_component(object_detector rclcpp std_srvs sensor_msgs rclcpp_components CUDA tf2 tf2_ros)
 
     mrover_executable_from_component(stereo_object_detector perception/executables/stereo_object_detector.cpp object_detector_component)
     mrover_executable_from_component(image_object_detector perception/executables/image_object_detector.cpp object_detector_component)
@@ -299,15 +299,15 @@ endif()
 
 # Tag Detector
 mrover_add_component(tag_detector perception/tag_detector/*.cpp perception/tag_detector/pch.hpp StereoTagDetector ImageTagDetector)
-mrover_ament_component(tag_detector rclcpp rclcpp_components tf2 tf2_ros)
+mrover_ament_component(tag_detector rclcpp std_srvs rclcpp_components tf2 tf2_ros)
 mrover_link_component(tag_detector lie opencv_core opencv_aruco opencv_imgproc loop_profiler parameter_utils)
 
 mrover_executable_from_component(stereo_tag_detector perception/executables/stereo_tag_detector.cpp tag_detector_component)
 mrover_executable_from_component(image_tag_detector perception/executables/image_tag_detector.cpp tag_detector_component)
 
 # Cost Map
 mrover_add_component(cost_map perception/cost_map/*.cpp perception/cost_map/pch.hpp CostMapNode)
-mrover_ament_component(cost_map rclcpp rclcpp_components tf2 tf2_ros)
+mrover_ament_component(cost_map rclcpp std_srvs rclcpp_components tf2 tf2_ros)
 mrover_link_component(cost_map lie parameter_utils)
 
 mrover_executable_from_component(cost_map perception/executables/cost_map.cpp cost_map_component)

diff --git a/perception/cost_map/cost_map.cpp b/perception/cost_map/cost_map.cpp
@@ -35,6 +35,10 @@ namespace mrover {
             dilateCostMapCallback(req, res);
         });
 
+        mToggleCostMap = create_service<std_srvs::srv::SetBool>("toggle_cost_map", [this](std_srvs::srv::SetBool::Request::ConstSharedPtr req, std_srvs::srv::SetBool::Response::SharedPtr res) {
+            toggleCostMapCallback(req, res);
+        });
+
         mPCDebugPub = create_publisher<sensor_msgs::msg::PointCloud2>("cost_map/debug_pc", 1);
 
         // Fill in grid resolution

diff --git a/perception/cost_map/cost_map.hpp b/perception/cost_map/cost_map.hpp
@@ -46,6 +46,8 @@ namespace mrover {
 
         rclcpp::Service<mrover::srv::DilateCostMap>::SharedPtr mCostServer;
 
+        rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr mToggleCostMap;
+
         struct BinEntry {
             R3f pointInCamera;
             R3f pointInMap;
@@ -65,6 +67,8 @@ namespace mrover {
         // To Test (CLI): ros2 service call /dilate_cost_map mrover/srv/DilateCostMap "{dilation_amount: [enter here]}"
         auto dilateCostMapCallback(mrover::srv::DilateCostMap::Request::ConstSharedPtr& req, mrover::srv::DilateCostMap::Response::SharedPtr& res) -> void;
 
+        auto toggleCostMapCallback(std_srvs::srv::SetBool::Request::ConstSharedPtr& req, std_srvs::srv::SetBool::Response::SharedPtr& res) -> void;
+
         void uploadPC();
 
         // Bin vector coordinate

diff --git a/perception/cost_map/cost_map.processing.cpp b/perception/cost_map/cost_map.processing.cpp
@@ -331,6 +331,21 @@ namespace mrover {
         RCLCPP_INFO_STREAM(get_logger(), "Done changing dilation");
     }
 
+    auto CostMapNode::toggleCostMapCallback(std_srvs::srv::SetBool::Request::ConstSharedPtr& req, std_srvs::srv::SetBool::Response::SharedPtr& res) -> void {
+        if (!req->data && mPcSub) {
+            mPcSub.reset();
+        }
+
+        else if (req->data && !mPcSub) {
+            mPcSub = create_subscription<sensor_msgs::msg::PointCloud2>("/zed/left/points", 1, [this](sensor_msgs::msg::PointCloud2::ConstSharedPtr const& msg) {
+                pointCloudCallback(msg);
+            });
+        }
+
+        res->success = true;
+        res->message = "";
+    }
+
     auto CostMapNode::indexToCoordinate(int const index) const -> CostMapNode::Coordinate {
         return {index / mWidth, index % mWidth};
     }

diff --git a/perception/cost_map/pch.hpp b/perception/cost_map/pch.hpp
@@ -43,3 +43,4 @@
 #include <mrover/srv/dilate_cost_map.hpp>
 #include <mrover/srv/move_cost_map.hpp>
 #include <rclcpp/service.hpp>
+#include <std_srvs/srv/set_bool.hpp>
diff --git a/perception/object_detector/pch.hpp b/perception/object_detector/pch.hpp
@@ -31,6 +31,7 @@
 // Ros Client Library
 #include <ament_index_cpp/get_package_prefix.hpp>
 #include <rclcpp/rclcpp.hpp>
+#include <std_srvs/srv/set_bool.hpp>
 #include <tf2_ros/transform_broadcaster.h>
 #include <tf2_ros/transform_listener.h>
 

diff --git a/perception/tag_detector/pch.hpp b/perception/tag_detector/pch.hpp
@@ -21,10 +21,12 @@
 
 #include <rclcpp/executors.hpp>
 #include <rclcpp/node.hpp>
+#include <rclcpp/service.hpp>
 #include <sensor_msgs/image_encodings.hpp>
 #include <sensor_msgs/msg/image.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
 #include <sensor_msgs/point_cloud2_iterator.hpp>
+#include <std_srvs/srv/set_bool.hpp>
 #include <tf2_ros/transform_broadcaster.h>
 #include <tf2_ros/transform_listener.h>
 

diff --git a/perception/tag_detector/tag_detector.cpp b/perception/tag_detector/tag_detector.cpp
@@ -64,6 +64,10 @@ namespace mrover {
         mPointCloudSub = create_subscription<sensor_msgs::msg::PointCloud2>("/zed/left/points", 1, [this](sensor_msgs::msg::PointCloud2::ConstSharedPtr const& msg) {
             pointCloudCallback(msg);
         });
+
+        mToggleStereoService = create_service<std_srvs::srv::SetBool>("/toggle_stereo_tag", [this](std_srvs::srv::SetBool::Request::ConstSharedPtr req, std_srvs::srv::SetBool::Response::SharedPtr res) {
+            toggleStereoCallback(req, res);
+        });
     }
 
     ImageTagDetector::ImageTagDetector(rclcpp::NodeOptions const& options) : TagDetectorBase{"image_tag_detector", options}, mCameraHorizontalFOV{} {
@@ -79,6 +83,10 @@ namespace mrover {
         mImageSub = create_subscription<sensor_msgs::msg::Image>("/long_range_cam/image", 1, [this](sensor_msgs::msg::Image::ConstSharedPtr const& msg) {
             imageCallback(msg);
         });
+
+        mToggleImageService = create_service<std_srvs::srv::SetBool>("/toggle_image_tag", [this](std_srvs::srv::SetBool::Request::ConstSharedPtr req, std_srvs::srv::SetBool::Response::SharedPtr res) {
+            toggleImageCallback(req, res);
+        });
     }
 } // namespace mrover
 

diff --git a/perception/tag_detector/tag_detector.hpp b/perception/tag_detector/tag_detector.hpp
@@ -60,6 +60,9 @@ namespace mrover {
                                        std::size_t u, std::size_t v,
                                        std::size_t width, std::size_t height) const -> std::optional<SE3d>;
 
+        rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr mToggleStereoService;
+        auto toggleStereoCallback(std_srvs::srv::SetBool::Request::ConstSharedPtr& req, std_srvs::srv::SetBool::Response::SharedPtr& res) -> void;
+
     public:
         explicit StereoTagDetector(rclcpp::NodeOptions const& options = rclcpp::NodeOptions());
     };
@@ -76,6 +79,9 @@ namespace mrover {
 
         auto imageCallback(sensor_msgs::msg::Image::ConstSharedPtr const& msg) -> void;
 
+        rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr mToggleImageService;
+        auto toggleImageCallback(std_srvs::srv::SetBool::Request::ConstSharedPtr& req, std_srvs::srv::SetBool::Response::SharedPtr& res) -> void;
+
     public:
         explicit ImageTagDetector(rclcpp::NodeOptions const& options = rclcpp::NodeOptions());
     };

diff --git a/perception/tag_detector/tag_detector.processing.cpp b/perception/tag_detector/tag_detector.processing.cpp
@@ -201,6 +201,21 @@ namespace mrover {
         return std::nullopt;
     }
 
+    auto StereoTagDetector::toggleStereoCallback(std_srvs::srv::SetBool::Request::ConstSharedPtr& req, std_srvs::srv::SetBool::Response::SharedPtr& res) -> void {
+        if (!req->data && mPointCloudSub) {
+            mPointCloudSub.reset();
+        }
+
+        else if (req->data && !mPointCloudSub) {
+            mPointCloudSub = create_subscription<sensor_msgs::msg::PointCloud2>("/zed/left/points", 1, [this](sensor_msgs::msg::PointCloud2::ConstSharedPtr const& msg) {
+                pointCloudCallback(msg);
+            });
+        }
+
+        res->success = true;
+        res->message = "";
+    }
+
     auto ImageTagDetector::getTagBearing(cv::InputArray image, std::span<cv::Point2f const> tagCorners) const -> float {
         // Takes the average of the corners
         cv::Point2f center = std::reduce(tagCorners.begin(), tagCorners.end()) / static_cast<float>(tagCorners.size());
@@ -210,4 +225,19 @@ namespace mrover {
         return bearingDegrees * std::numbers::pi_v<float> / 180.0f;
     }
 
+    auto ImageTagDetector::toggleImageCallback(std_srvs::srv::SetBool::Request::ConstSharedPtr& req, std_srvs::srv::SetBool::Response::SharedPtr& res) -> void {
+        if (!req->data && mImageSub) {
+            mImageSub.reset();
+        }
+
+        else if (req->data && !mImageSub) {
+            mImageSub = create_subscription<sensor_msgs::msg::Image>("/long_range_cam/image", 1, [this](sensor_msgs::msg::Image::ConstSharedPtr const& msg) {
+                imageCallback(msg);
+            });
+        }
+
+        res->success = true;
+        res->message = "";
+    }
+
 } // namespace mrover
diff --git a/srv/ToggleCostMap.srv b/srv/ToggleCostMap.srv
diff --git a/srv/ToggleObjectDetector.srv b/srv/ToggleObjectDetector.srv