Vision Detection System Improvements

[thomas0829]

This PR enhances the rover's vision detection capabilities with improvements to AR detection and integration of a new object detection system.

1. AR Detection Enhancements

Core Improvements:

• Replace deprecated estimatePoseSingleMarkers API with solvePnP for better accuracy
• Enable AR detection support in simulator mode with proper configuration
• Fix CMakeLists.txt by removing non-existent test executables
• Improve initialization and logging for better debugging

Runtime Control:

• Add keyboard toggle control with R key (default: OFF)
• Clear old landmark data when disabling detection
• Background thread monitors enable/disable state
• State change logging (STARTED/STOPPED)

2. Object Detection Integration (NEW ✨)

Model & Architecture:

• Integrate OWL-ViT (Vision Transformer) model with LibTorch 2.1.0+
• Support for custom object classes (configurable)
• CUDA GPU acceleration for real-time performance
• Model file: owlvit-cpp.pt (excluded from git, 586MB)

Features:

• NMS (Non-Maximum Suppression) with IoU threshold 0.5 for duplicate removal
• Configurable confidence threshold (default: 0.6)
• Background detection thread with proper synchronization
• Integration with camera system (Mast Camera)
• Keyboard toggle control with O key (default: OFF)
• Automatic CUDA JIT optimization disabling (fixes nvrtc compilation errors)

Testing & Visualization:

• Side-by-side comparison test showing enabled/disabled states
• Bounding box visualization with confidence scores
• Detection count display
• Smart model path detection (multiple fallback locations)

Code Structure:

src/object-detection/
├── ObjectDetector.h/cpp       # Core detection engine with NMS
├── DetectionResult.h/cpp      # Detection result data structure
├── read_objects.h/cpp         # Integration interface
├── ObjectDetectionTest.cpp    # Visualization test
├── CMakeLists.txt             # Build configuration
└── generate_tokens.py         # Token generation utility

3. Unified Keyboard Control Interface

Both detection systems can now be controlled via keyboard in real-time:

Key	Function	Default State
O	Toggle object detection	OFF
R	Toggle ArUco detection	OFF
Q	Quit program	-

Design Rationale:

• Both systems disabled by default to reduce computational load
• Operators can enable only what they need during missions
• Clear console feedback for state changes
• Logging at INFO level for visibility

4. Optional LibTorch Support

Flexibility:

• Object detection is now optional - project builds without LibTorch
• CMake gracefully falls back if LibTorch not found
• Clear warning messages guide users to install LibTorch if desired
• README includes complete LibTorch installation instructions

Installation:

# Optional: Install LibTorch for object detection
export LIBTORCH_PATH=~/libtorch
cmake ../src

Technical Details

Dependencies Added:

• LibTorch 2.1.0+ with CUDA support (optional)
• Environment variable: LIBTORCH_PATH for custom LibTorch location
• Alternative: OWLVIT_MODEL_PATH for custom model location

Modified Files:

• src/Globals.h/cpp - Added global enable flags
• src/Rover.cpp - Keyboard control loop with conditional compilation
• src/ar/read_landmarks.cpp - Enable/disable checking
• src/network/MissionControlTasks.cpp - ArUco detection guard
• src/CMakeLists.txt - Optional object detection module
• src/object-detection/ObjectDetector.cpp - CUDA JIT fix
• src/object-detection/read_objects.cpp - Smart model path detection
• README.md - LibTorch installation guide
• .gitignore - Ignore large model files

CUDA Compatibility:

• Automatic JIT optimization disabling prevents nvrtc errors
• No manual environment variable setup required
• Fusion strategy set to avoid __ldg undefined identifier issues

Testing:

# Build (object detection included if LibTorch found)
cd build
cmake ../src
make Rover -j4

# Run rover
./Rover -p arm

# Toggle object detection at runtime with 'O' key
# Toggle ArUco detection at runtime with 'R' key

Without LibTorch:

# Build succeeds, object detection disabled
cd build
cmake ../src  # Shows warning: "LibTorch not found. Object detection will be DISABLED."
make Rover -j4

# Run rover (ArUco detection still available)
./Rover -p arm

# Pressing 'O' shows: "Object detection not available (LibTorch not found)"

[imisaacwu]

Looks good, couple small things 👍

[imisaacwu]

Do we need to open the mast camera here? Could be wrong, but I believe the reason we don't initialize cameras on initialization is to wait until mission control is open and open cameras dynamically based on which cameras are displayed on the screen.

[imisaacwu]

Could y'all generalize this to have detection on any camera?

[imisaacwu]

Could you also generalize the marker type?

[imisaacwu]

Do we need to create a new DetectorParameters every time?

[imisaacwu]

Nice, I think this looks good for now, eventually we will want to use the data for positioning, etc.
For now, could we simplify these print statements to use LOG_F(INFO, ...) instead of printf (and remove the stdio import at the top)?

[Copilot]

Pull request overview

This PR enhances the rover's vision detection system with two major components: improvements to ArUco marker detection and integration of a new OWL-ViT based object detection system. The ArUco detection now uses the modern solvePnP API instead of deprecated methods, supports simulator mode, and includes keyboard-controlled enable/disable functionality. The object detection system adds support for detecting specific objects (orange hammer, rock pick, water bottle) using a Vision Transformer model with LibTorch, including depth-based distance estimation via RealSense cameras. Both systems can be toggled on/off via keyboard controls and are disabled by default to reduce computational load.

Changes:

• Replaced deprecated estimatePoseSingleMarkers with solvePnP for ArUco pose estimation
• Added optional LibTorch-based object detection with OWL-ViT model, including NMS, depth support, and task-based detection
• Implemented keyboard controls for runtime toggling of both detection systems with global enable flags

Reviewed changes

Copilot reviewed 40 out of 41 changed files in this pull request and generated 15 comments.

Show a summary per file

File	Description
test_object_detection.sh	Test script for object detection keyboard controls
src/world_interface/*.{h,cpp}	Added depth frame reading capability for RealSense cameras
src/object-detection/*	New object detection module with detector, model downloader, and test utilities
src/network/MissionControl*.{h,cpp}	Added object detection enable/disable protocol handlers and ArUco logging
src/camera/*.{h,cpp}	Added RealSenseCamera class and undistortion support
src/ar/*.{h,cpp}	Updated to use solvePnP and added enable/disable state checking
src/Rover.cpp	Added keyboard control loop for detection systems
src/Globals.{h,cpp}	Added global atomic flags for detection enable states
src/CMakeLists.txt	Added optional LibTorch support and RealSense configuration
camera-config/*.yml	Uncommented calibration parameters for AR detection
calibrate_and_test.sh	Testing commands for RealSense and camera calibration
README.md	Added LibTorch installation instructions

Comments suppressed due to low confidence (1)

src/network/MissionControlProtocol.cpp:443

• Functions are defined out of order compared to the header file. In the header, the order is: handleRequestObjectDetectionEnabled, setRequestedCmdVel, setRequestedTankCmdVel, then setObjectDetectionEnabled. Here they're implemented in a different order. While this doesn't affect functionality, maintaining the same order as the header improves code readability and maintenance.

void MissionControlProtocol::handleRequestObjectDetectionEnabled(const json& j) {
	bool enabled = j["enabled"];
	this->setObjectDetectionEnabled(enabled);
}

void MissionControlProtocol::setRequestedCmdVel(double dtheta, double dx) {
	_power_repeat_task.setCmdVel(dtheta, dx);
	robot::setCmdVel(dtheta, dx);
}

void MissionControlProtocol::setRequestedTankCmdVel(double left, double right) {
	_power_repeat_task.setTankCmdVel(left, right);
	robot::setTankCmdVel(left, right);
}

static bool validateJoint(const json& j) {
	return util::validateKey(j, "joint", val_t::string) &&
		   std::any_of(all_jointid_t.begin(), all_jointid_t.end(), [&](const auto& joint) {
			   return j["joint"].get<std::string>() == util::to_string(joint);
		   });
}

static bool validateJointPowerRequest(const json& j) {
	return validateJoint(j) && util::validateRange(j, "power", -1, 1);
}

void MissionControlProtocol::handleJointPowerRequest(const json& j) {
	// TODO: ignore this message if we are in autonomous mode.
	using robot::types::jointid_t;
	using robot::types::name_to_jointid;
	std::string joint = j["joint"];
	double power = j["power"];
	auto it = name_to_jointid.find(util::freezeStr(joint));
	if (it != name_to_jointid.end()) {
		jointid_t joint_id = it->second;
		setRequestedJointPower(joint_id, power);
	}
}

static bool validateJointPositionRequest(const json& j) {
	return validateJoint(j) && util::validateKey(j, "position", val_t::number_integer);
}

void MissionControlProtocol::handleJointPositionRequest([[maybe_unused]] const json& j) {
	// TODO: ignore this message if we are in autonomous mode.
	// std::string motor = j["joint"];
	// double position_deg = j["position"];
	// int32_t position_mdeg = std::round(position_deg * 1000);
	// TODO: actually implement joint position requests
	// setMotorPos(motor, position_mdeg);
}

static bool validateServoPositionRequest(const json& j) {
  return util::validateKey(j, "servo", val_t::string) && util::validateKey(j, "position", val_t::number_integer);
}

void MissionControlProtocol::handleServoPositionRequest(const json& j) {
  std::string servoName = j["servo"];
  int32_t position = j["position"];
  auto servo = name_to_servoid.find(util::freezeStr(servoName));
  if (servo != name_to_servoid.end()) {
	  robot::setServoPos(servo->second, position);
  }
}

static bool validateStepperTurnAngleRequest(const json& j) {
  return util::validateKey(j, "stepper", val_t::string) && util::validateKey(j, "angle", val_t::number_integer);
}

void MissionControlProtocol::handleStepperTurnAngleRequest(const json& j) {
  std::string stepperName = j["stepper"];
  int16_t angle = j["angle"];
  auto stepper = name_to_stepperid.find(util::freezeStr(stepperName));
  if (stepper != name_to_stepperid.end()) {
    robot::setRequestedStepperTurnAngle(stepper->second, angle);
  }
}

static bool validateWaypointNavRequest(const json& j) {
	bool lat_is_unsigned = util::validateKey(j, "latitude", val_t::number_unsigned);
	bool lon_is_unsigned = util::validateKey(j, "longitude", val_t::number_unsigned);
	return (lat_is_unsigned || util::validateKey(j, "latitude", val_t::number_float)) &&
		   (lon_is_unsigned || util::validateKey(j, "longitude", val_t::number_float)) &&
		   util::validateKey(j, "isApproximate", val_t::boolean) &&
		   util::validateKey(j, "isGate", val_t::boolean);
}

void MissionControlProtocol::handleWaypointNavRequest(const json& j) {
	float latitude = j["latitude"];
	float longitude = j["longitude"];
	bool isApproximate = j["isApproximate"];
	bool isGate = j["isGate"];
	if (Globals::AUTONOMOUS && !isApproximate && !isGate) {
		// gpsToMeters will not use altitude
		navtypes::gpscoords_t coords = {latitude, longitude, 0};
		auto target = robot::gpsToMeters(coords);
		if (target) {
			_autonomous_task.start(target.value());
		} else {
			LOG_F(WARNING, "No GPS converter initialized!");
		}
	}
}

static bool validateCameraStreamOpenRequest(const json& j) {
	return util::validateOneOf(j, "camera", Constants::CAMERA_SET);
}

void MissionControlProtocol::handleCameraStreamOpenRequest(const json& j) {
	CameraID cam = j["camera"];
	_camera_stream_task.openStream(cam, j["fps"]);
}

static bool validateCameraStreamCloseRequest(const json& j) {
	return util::validateOneOf(j, "camera", Constants::CAMERA_SET);
}

void MissionControlProtocol::handleCameraStreamCloseRequest(const json& j) {
	CameraID cam = j["camera"];
  	_camera_stream_task.closeStream(cam);
}

static bool validateCameraFrameRequest(const json& j) {
	return util::validateOneOf(j, "camera", Constants::CAMERA_SET);
}

void MissionControlProtocol::handleCameraFrameRequest(const json& j) {
	auto gps = gps::readGPSCoords();
	auto imu = robot::readIMU();
	CameraID cam = j["camera"];
	auto camDP = robot::readCamera(cam);

	Eigen::Quaterniond quat = imu.getData();
	double lon = 0, lat = 0, alt = 0;
	double w = 0, x = 0, y = 0, z = 0;
	if (gps.isValid()) {
		lon = gps.getData().lon;
		lat = gps.getData().lat;
		alt = gps.getData().alt;
    	w = quat.w();
    	x = quat.x();
    	y = quat.y();
    	z = quat.z();
	}

	if (camDP) {
		auto data = camDP.getData();
		cv::Mat frame = data.first;
		std::string b64_data = base64::encodeMat(frame, ".jpg");
		json msg = {{"type", CAMERA_FRAME_REP_TYPE}, {"camera", cam}, {"data", b64_data}, 
		{"orientW", w}, {"orientX", x}, {"orientY", y}, {"orientZ", z},
		{"lon", lon}, {"lat", lat}, {"alt", alt}};
		_server.sendJSON(Constants::MC_PROTOCOL_NAME, msg);
	}
}

void MissionControlProtocol::sendArmIKEnabledReport(bool enabled) {
	json msg = {{"type", ARM_IK_ENABLED_REP_TYPE}, {"enabled", enabled}};
	this->_server.sendJSON(Constants::MC_PROTOCOL_NAME, msg);
}

void MissionControlProtocol::sendObjectDetectionEnabledReport(bool enabled) {
	json msg = {{"type", OBJECT_DETECTION_ENABLED_REP_TYPE}, {"enabled", enabled}};
	this->_server.sendJSON(Constants::MC_PROTOCOL_NAME, msg);
}

void MissionControlProtocol::handleConnection() {
	// Turn off inverse kinematics on connection
	this->setArmIKEnabled(false);
	
	// Turn off object detection on connection
	this->setObjectDetectionEnabled(false);

	// TODO: send the actual mounted peripheral, as specified by the command-line parameter
	json j = {{"type", MOUNTED_PERIPHERAL_REP_TYPE}};

	if (Globals::mountedPeripheral == mountedperipheral_t::none) {
		j["peripheral"] = json(nullptr);
	} else {
		j["peripheral"] = util::to_string(Globals::mountedPeripheral);
	}

	this->_server.sendJSON(Constants::MC_PROTOCOL_NAME, j);

	if (!Globals::AUTONOMOUS) {
		// start power repeat thread (if not already running)
		_power_repeat_task.start();
	}
}

void MissionControlProtocol::handleHeartbeatTimedOut() {
	LOG_F(ERROR, "Heartbeat timed out! Emergency stopping.");
	this->stopAndShutdownPowerRepeat(true);
	robot::emergencyStop();
}

void MissionControlProtocol::stopAndShutdownPowerRepeat(bool sendDisableIK) {
	if (_power_repeat_task.isRunning()) {
		_power_repeat_task.stop();
		// explicitly set all joints to zero
		stopAllJoints();
		// explicitly stop chassis
		robot::setCmdVel(0, 0);
	}
	// Turn off inverse kinematics so that IK state will be in sync with mission control
	this->setArmIKEnabled(false, sendDisableIK);
}

MissionControlProtocol::MissionControlProtocol(SingleClientWSServer& server)
	: WebSocketProtocol(Constants::MC_PROTOCOL_NAME), _server(server),
	  _camera_stream_task(server), _telem_report_task(server), _arm_ik_task(server),
	  _autonomous_task() {
	// emergency stop and operation mode handlers need the class for context since they must
	// be able to access the methods to start and stop the power repeater thread
	this->addMessageHandler(
		EMERGENCY_STOP_REQ_TYPE,
		std::bind(&MissionControlProtocol::handleEmergencyStopRequest, this, _1),
		validateEmergencyStopRequest);
	this->addMessageHandler(
		OPERATION_MODE_REQ_TYPE,
		std::bind(&MissionControlProtocol::handleOperationModeRequest, this, _1),
		validateOperationModeRequest);
	// drive and joint power handlers need the class for context since they must modify
	// _last_joint_power and _last_cmd_vel (for the repeater thread)
	this->addMessageHandler(
		DRIVE_REQ_TYPE,
		std::bind(&MissionControlProtocol::handleDriveRequest, this, _1),
		validateDriveRequest);
	this->addMessageHandler(
		DRIVE_TANK_REQ_TYPE,
		std::bind(&MissionControlProtocol::handleTankDriveRequest, this, _1),
		validateTankDriveRequest);
	this->addMessageHandler(
		ARM_IK_ENABLED_TYPE,
		std::bind(&MissionControlProtocol::handleRequestArmIKEnabled, this, _1),
		validateArmIKEnable);
	this->addMessageHandler(
		OBJECT_DETECTION_ENABLED_TYPE,
		std::bind(&MissionControlProtocol::handleRequestObjectDetectionEnabled, this, _1),
		validateObjectDetectionEnable);
	this->addMessageHandler(
		JOINT_POWER_REQ_TYPE,
		std::bind(&MissionControlProtocol::handleJointPowerRequest, this, _1),
		validateJointPowerRequest);
	this->addMessageHandler(
		JOINT_POSITION_REQ_TYPE,
		std::bind(&MissionControlProtocol::handleJointPositionRequest, this, _1),
		validateJointPositionRequest);
	this->addMessageHandler(
		CAMERA_STREAM_OPEN_REQ_TYPE,
		std::bind(&MissionControlProtocol::handleCameraStreamOpenRequest, this, _1),
		validateCameraStreamOpenRequest);
	this->addMessageHandler(
		CAMERA_STREAM_CLOSE_REQ_TYPE,
		std::bind(&MissionControlProtocol::handleCameraStreamCloseRequest, this, _1),
		validateCameraStreamCloseRequest);
	this->addMessageHandler(
		CAMERA_FRAME_REQ_TYPE,
		std::bind(&MissionControlProtocol::handleCameraFrameRequest, this, _1),
		validateCameraFrameRequest);
	this->addMessageHandler(
		WAYPOINT_NAV_REQ_TYPE,
		std::bind(&MissionControlProtocol::handleWaypointNavRequest, this, _1),
		validateWaypointNavRequest);
	this->addMessageHandler(
    		SERVO_POSITION_REQ_TYPE,
    		std::bind(&MissionControlProtocol::handleServoPositionRequest, this, _1),
    		validateServoPositionRequest);
	this->addMessageHandler(
    		STEPPER_TURN_ANGLE_REQ_TYPE,
    		std::bind(&MissionControlProtocol::handleStepperTurnAngleRequest, this, _1),
    		validateStepperTurnAngleRequest);

	this->addConnectionHandler(std::bind(&MissionControlProtocol::handleConnection, this));

	this->addDisconnectionHandler(
		std::bind(&MissionControlProtocol::stopAndShutdownPowerRepeat, this, false));

	this->setHeartbeatTimedOutHandler(
		HEARTBEAT_TIMEOUT_PERIOD,
		std::bind(&MissionControlProtocol::handleHeartbeatTimedOut, this));

	_telem_report_task.start();
	_camera_stream_task.start();
}

MissionControlProtocol::~MissionControlProtocol() {
	this->stopAndShutdownPowerRepeat(true);
}

///// UTILITY FUNCTIONS //////
void MissionControlProtocol::setArmIKEnabled(bool enabled, bool sendReport) {
	Globals::armIKEnabled = enabled;
	if (enabled) {
		_arm_ik_task.start();
	} else {
		_arm_ik_task.stop();
	}

---

💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.

[Copilot]

This file modifies the camera calibration save functionality but the changes appear incomplete or unrelated to the PR's stated purpose. The PR description doesn't mention changes to camera calibration file format. These additional fields (format, framerate, dimensions) seem to duplicate information already stored elsewhere in the calibration file. Please verify this change is intentional and necessary.

[Copilot]

The keyboard input loop uses cin which is blocking and requires pressing Enter after each key. For a more responsive control interface, consider using non-blocking or immediate input methods (like ncurses or platform-specific APIs) so users can press a single key without needing to press Enter. This would improve the user experience significantly.

[Copilot]

The function signature in the header declares a bool return type, but the implementation in simulator_interface.cpp returns std::optional<std::pair<cv::Mat, float>> instead. This is a type mismatch that will cause compilation errors. The header and implementation signatures must match.

[Copilot]

The real_world_interface.cpp implementation returns std::optional but logs and returns immediately. This function should either match the header signature (bool return with out parameters) or the header should be updated to return std::optional. Currently there's a type mismatch between the header declaration and this implementation.

[Copilot]

The comment on line 557 says "Limit to max 3 results" but there's no clear explanation of why this limit exists or what happens to the other detections. If this is a performance optimization or a requirement, it should be documented more clearly. Consider adding this limit as a named constant with documentation explaining the reasoning.

[Copilot]

This hardcoded absolute path is specific to one user's machine and will fail in other development environments. The LibTorch path should be configurable via environment variable or CMake variable. Consider using: if(DEFINED ENV{LIBTORCH_PATH}) set(CMAKE_PREFIX_PATH "$ENV{LIBTORCH_PATH}") endif() at the start of this file, similar to what's done in the root CMakeLists.txt.

Suggested change

	# Set LibTorch path
	set(CMAKE_PREFIX_PATH "/home/thomas/libtorch")
	# Set LibTorch path from environment variable if provided
	if(DEFINED ENV{LIBTORCH_PATH})
	set(CMAKE_PREFIX_PATH "$ENV{LIBTORCH_PATH}")
	endif()

[Copilot]

The mutex is locked and then unlocked manually, which is error-prone if an exception occurs between lock() and unlock(). Use std::lock_guard or std::scoped_lock instead for exception-safe RAII-style locking. For example: { std::lock_guard<std::mutex> lock(detection_lock); current_detections = detections; fresh_data = true; }

[Copilot]

Manual mutex locking pattern that's not exception-safe. Multiple instances in this file use lock()/unlock() manually. All should use std::lock_guard for RAII-style locking to prevent deadlocks if exceptions are thrown.

[Copilot]

The camera configuration file has intrinsic and extrinsic parameters uncommented (previously they were commented out). This is a significant change that affects how the camera system operates. The PR description mentions enabling AR detection in simulator mode, but doesn't clearly state that camera parameters would be uncommented. Verify this change is intentional and won't break existing camera configurations or AR detection behavior.

[Copilot]

The same mutex locking issue exists here - using manual lock()/unlock() instead of RAII lock guards. This pattern appears multiple times in this file (lines 94-97, 164-166). All should use std::lock_guard for exception safety.