Skip to content
Draft
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
47 changes: 35 additions & 12 deletions application/backend/app/routers/robot.py
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@
from sqlalchemy.orm import Session
from sqlalchemy import desc
from datetime import datetime
import time
from app.database import get_db
from app.models.robot import RobotStatus
from app.models.scan import ScanRecord
Expand Down Expand Up @@ -68,7 +69,7 @@ class MissionConfig(BaseModel):
sampling_precision_m: float = 5.0
area_width: Optional[float] = None
area_height: Optional[float] = None
row_spacing: float = 0.8
row_spacing: float = 0.5
waypoint_spacing: float = 0.5
origin_x: float = 0.0
origin_y: float = 0.0
Expand All @@ -78,7 +79,7 @@ class MissionConfig(BaseModel):

def _calc_total_waypoints(area_width: float, area_height: float,
row_spacing: float, waypoint_spacing: float,
wall_margin: float = 0.20) -> int:
wall_margin: float = 0.30) -> int:
"""Match the coverage_planner.py waypoint generation logic (centered grid)."""
import math
ew = area_width - 2 * wall_margin # centered: same effective width
Expand Down Expand Up @@ -151,18 +152,17 @@ async def start_coverage_mission(config: MissionConfig = None, db: Session = Dep
)
cmd = ['bash', '-c', ros_cmd]

# Start the mission as a background process (may fail on non-ROS hosts; continue for capture)
try:
mission_process = subprocess.Popen(
cmd,
env=clean_env,
preexec_fn=os.setsid if hasattr(os, "setsid") else None,
)
except Exception:
mission_process = None # noqa: PLW0602
mission_process = subprocess.Popen(
cmd,
env=clean_env,
preexec_fn=os.setsid if hasattr(os, "setsid") else None,
)
time.sleep(1.0)
if mission_process.poll() is not None:
raise RuntimeError(f"roslaunch exited immediately with return code {mission_process.returncode}")

# Start waypoint capture loop (one sample per '/coverage/capture_ready'=true message).
start_capture_loop(scan_id, total_points=total_points)
start_capture_loop(scan_id, total_points=total_points, require_ros=True)
progress = get_capture_progress()

return {
Expand All @@ -177,6 +177,17 @@ async def start_coverage_mission(config: MissionConfig = None, db: Session = Dep
}

except Exception as e:
if mission_process is not None and mission_process.poll() is None:
try:
pgid = os.getpgid(mission_process.pid)
if pgid != os.getpgrp():
os.killpg(pgid, signal.SIGTERM)
else:
mission_process.terminate()
mission_process.wait(timeout=5)
except Exception:
pass
mission_process = None # noqa: PLW0602
raise HTTPException(
status_code=status.HTTP_500_INTERNAL_SERVER_ERROR,
detail=f"Failed to start mission: {str(e)}"
Expand Down Expand Up @@ -248,6 +259,18 @@ async def get_mission_status():
"message": "No mission has been started",
}

if active_scan_id and progress.get("status") == "completed":
return {
"status": "completed",
"running": False,
"pid": mission_process.pid if mission_process and mission_process.poll() is None else None,
"scan_id": active_scan_id,
"captured_points": progress["captured_points"],
"total_points": progress["total_points"],
"progress_percent": progress["progress_percent"],
"message": "Mission completed",
}

if active_scan_id and progress.get("status") == "running":
return {
"status": "running",
Expand Down
71 changes: 65 additions & 6 deletions application/backend/app/services/ros_sensor_bridge.py
Original file line number Diff line number Diff line change
Expand Up @@ -16,8 +16,10 @@
import io
import os
import logging
import socket
import threading
from typing import Optional, Dict, Any
from urllib.parse import urlparse

logger = logging.getLogger(__name__)

Expand All @@ -38,12 +40,15 @@
"rgb_image_path": None, # kept for waypoint capture compatibility
"voltage": None,
"battery_percent": None,
"coverage_total_points": None,
"coverage_complete": False,
"capture_ready_queue": [],
"lock": threading.Lock(),
"capture_ready_condition": threading.Condition(),
}
_ros_thread: Optional[threading.Thread] = None
_ros_stop = threading.Event()
DEFAULT_LOCAL_ROS_MASTER_URI = "http://127.0.0.1:11311"


def voltage_to_percent(voltage: float) -> int:
Expand All @@ -55,6 +60,32 @@ def voltage_to_percent(voltage: float) -> int:
return max(0, min(100, int(round(percent))))


def _can_reach_ros_master(uri: str, timeout_sec: float = 0.5) -> bool:
if not uri:
return False
try:
parsed = urlparse(uri)
host = parsed.hostname
port = parsed.port or 11311
if not host:
return False
with socket.create_connection((host, port), timeout=timeout_sec):
return True
except OSError:
return False


def get_ros_master_uri() -> str:
from app.config import settings

configured = (settings.ROS_MASTER_URI or os.environ.get("ROS_MASTER_URI") or "").strip()
if configured:
return configured
if _can_reach_ros_master(DEFAULT_LOCAL_ROS_MASTER_URI):
return DEFAULT_LOCAL_ROS_MASTER_URI
return ""


def get_live_rgb_bytes() -> Optional[bytes]:
"""Return latest RGB JPEG bytes from cache, or None."""
with _ros_cache["lock"]:
Expand Down Expand Up @@ -97,7 +128,7 @@ def _ros_subscriber_thread(thermal_image_save_dir: str, rgb_image_save_dir: str)
"""Run rospy node and subscribe to sensor topics; update _ros_cache."""
try:
import rospy
from std_msgs.msg import Float64, Float32, Bool
from std_msgs.msg import Float64, Float32, Bool, Int32
from sensor_msgs.msg import Image
except ImportError as e:
logger.warning("rospy not available, ROS sensor bridge disabled: %s", e)
Expand Down Expand Up @@ -233,6 +264,14 @@ def cb_capture_ready(msg):
_ros_cache["capture_ready_queue"].append(True)
_ros_cache["capture_ready_condition"].notify_all()

def cb_coverage_total(msg):
with _ros_cache["lock"]:
_ros_cache["coverage_total_points"] = int(msg.data)

def cb_coverage_complete(msg):
with _ros_cache["lock"]:
_ros_cache["coverage_complete"] = bool(msg.data)

def _set_voltage(voltage_value):
with _ros_cache["lock"]:
_ros_cache["voltage"] = voltage_value
Expand Down Expand Up @@ -260,6 +299,8 @@ def cb_voltage_battery(msg):
rospy.Subscriber("/sensors/sht40/humidity", Float64, cb_hum, queue_size=1)
rospy.Subscriber("/sensors/thermal/mean", Float64, cb_thermal_mean, queue_size=1)
rospy.Subscriber("/coverage/capture_ready", Bool, cb_capture_ready, queue_size=50)
rospy.Subscriber("/coverage/total_points", Int32, cb_coverage_total, queue_size=10)
rospy.Subscriber("/coverage/complete", Bool, cb_coverage_complete, queue_size=10)
topic_types = {}
try:
topic_types = {name: t for name, t in rospy.get_published_topics()}
Expand All @@ -286,11 +327,15 @@ def cb_voltage_battery(msg):


def start_ros_bridge(thermal_image_save_dir: str, rgb_image_save_dir: str) -> bool:
"""Start the ROS subscriber thread if ROS_MASTER_URI is set. Return True if using ROS."""
"""Start the ROS subscriber thread when a reachable ROS master is available."""
global _ros_thread
from app.config import settings
uri = (settings.ROS_MASTER_URI or os.environ.get("ROS_MASTER_URI") or "").strip()
uri = get_ros_master_uri()
if not uri:
logger.warning("ROS bridge not started: no ROS master configured or reachable")
return False
if not _can_reach_ros_master(uri):
logger.warning("ROS bridge not started: ROS master %s is not reachable", uri)
return False
if _ros_thread is not None and _ros_thread.is_alive():
return True
Expand Down Expand Up @@ -326,9 +371,25 @@ def get_latest_from_ros() -> Dict[str, Any]:
"rgb_image_path": _ros_cache["rgb_image_path"],
"voltage": _ros_cache["voltage"],
"battery_percent": _ros_cache["battery_percent"],
"coverage_total_points": _ros_cache["coverage_total_points"],
"coverage_complete": bool(_ros_cache["coverage_complete"]),
}


def get_coverage_state() -> Dict[str, Any]:
with _ros_cache["lock"]:
return {
"total_points": _ros_cache["coverage_total_points"],
"complete": bool(_ros_cache["coverage_complete"]),
}


def clear_coverage_state() -> None:
with _ros_cache["lock"]:
_ros_cache["coverage_total_points"] = None
_ros_cache["coverage_complete"] = False


def wait_for_next_capture_ready(timeout_sec: float = 0.5) -> bool:
"""Wait and consume one capture trigger from /coverage/capture_ready."""
condition = _ros_cache["capture_ready_condition"]
Expand All @@ -348,9 +409,7 @@ def clear_capture_ready_queue() -> None:


def is_ros_configured() -> bool:
from app.config import settings
uri = (settings.ROS_MASTER_URI or os.environ.get("ROS_MASTER_URI") or "").strip()
return bool(uri)
return bool(get_ros_master_uri())


def get_required_topics_status() -> Dict[str, Any]:
Expand Down
37 changes: 35 additions & 2 deletions application/backend/app/services/waypoint_capture_service.py
Original file line number Diff line number Diff line change
Expand Up @@ -24,8 +24,10 @@
is_ros_configured,
start_ros_bridge,
get_latest_from_ros,
get_coverage_state,
wait_for_next_capture_ready,
clear_capture_ready_queue,
clear_coverage_state,
save_live_rgb_to_file,
save_live_thermal_to_file,
)
Expand Down Expand Up @@ -112,6 +114,15 @@ def _mark_scan_completed(scan_id: int) -> None:
db.close()


def _sync_total_points_from_ros() -> Optional[int]:
metrics = get_coverage_state()
total_points = metrics.get("total_points")
if total_points is not None:
with _capture_state_lock:
_capture_state["total_points"] = int(total_points)
return total_points


def _capture_loop_impl(scan_id: int):
stop_event = _capture_state["stop_event"]
if not stop_event:
Expand All @@ -128,6 +139,18 @@ def _capture_loop_impl(scan_id: int):
simulate = not use_ros and not _sht40_script.exists()

while not stop_event.is_set():
if use_ros:
total_points = _sync_total_points_from_ros()
coverage_state = get_coverage_state()
with _capture_state_lock:
captured_points = int(_capture_state.get("captured_points") or 0)
if coverage_state.get("complete") and total_points is not None and captured_points >= int(total_points):
with _capture_state_lock:
_capture_state["status"] = "completed"
_mark_scan_completed(scan_id)
stop_event.set()
break

capture_ready = wait_for_next_capture_ready(timeout_sec=0.5) if use_ros else False
if stop_event.is_set():
break
Expand Down Expand Up @@ -221,8 +244,11 @@ def _capture_loop_impl(scan_id: int):
finally:
db.close()

ros_total_points = _sync_total_points_from_ros() if use_ros else None
with _capture_state_lock:
_capture_state["captured_points"] = sequence_index + 1
if ros_total_points is not None:
_capture_state["total_points"] = int(ros_total_points)
total_points = _capture_state["total_points"]
sequence_index += 1
if total_points and sequence_index >= total_points:
Expand All @@ -237,7 +263,7 @@ def _capture_loop_impl(scan_id: int):
_capture_state["status"] = "stopped"


def start_capture_loop(scan_id: int, total_points: Optional[int] = None) -> None:
def start_capture_loop(scan_id: int, total_points: Optional[int] = None, require_ros: bool = False) -> None:
"""Start background thread that captures on each ROS '/coverage/capture_ready'=true event."""
old_thread = _capture_state["thread"]
if old_thread is not None and old_thread.is_alive():
Expand All @@ -249,8 +275,11 @@ def start_capture_loop(scan_id: int, total_points: Optional[int] = None) -> None
rgb_dir = os.path.join(settings.UPLOAD_DIR, "realsense_latest")
os.makedirs(thermal_dir, exist_ok=True)
os.makedirs(rgb_dir, exist_ok=True)
use_ros = is_ros_configured() and start_ros_bridge(thermal_dir, rgb_dir)
use_ros = start_ros_bridge(thermal_dir, rgb_dir)
if require_ros and not use_ros:
raise RuntimeError("ROS bridge unavailable; refusing to simulate mission captures")
clear_capture_ready_queue()
clear_coverage_state()
logger.warning("Starting capture loop: scan_id=%d, total_points=%s, use_ros=%s", scan_id, total_points, use_ros)
_capture_state["use_ros"] = use_ros
_capture_state["stop_event"] = threading.Event()
Expand Down Expand Up @@ -295,6 +324,10 @@ def get_capture_progress() -> dict:
total_points = _capture_state.get("total_points")
status = _capture_state.get("status") or "idle"
last_capture_ready = _capture_state.get("last_capture_ready")
use_ros = bool(_capture_state.get("use_ros"))
ros_total_points = get_coverage_state().get("total_points") if use_ros else None
if ros_total_points is not None:
total_points = int(ros_total_points)
progress_percent = 0.0
if total_points and total_points > 0:
progress_percent = min(100.0, (captured_points / total_points) * 100.0)
Expand Down
8 changes: 6 additions & 2 deletions application/src/App.jsx
Original file line number Diff line number Diff line change
Expand Up @@ -190,6 +190,11 @@ function App() {

if (progress?.status === 'completed' && !finishHandledRef.current) {
finishHandledRef.current = true
try {
await apiClient.stopCoverageMission()
} catch (stopError) {
console.error('Failed to stop completed mission cleanly:', stopError)
}
setIsScanning(false)
setRobotStatus(prev => ({ ...prev, operatingState: 'Idle' }))
setScanPhase('Scan complete')
Expand Down Expand Up @@ -275,7 +280,7 @@ function App() {

const handleConfirmStartScan = async ({
areaSize, precision, totalPoints: configuredTotal,
originX = 0, originY = 0, rowSpacing = 0.8, dwellTime = 2.0, waypointTimeout = 30.0
originX = 0, originY = 0, rowSpacing = 0.5, dwellTime = 2.0, waypointTimeout = 30.0
}) => {
setShowScanConfigModal(false)
try {
Expand Down Expand Up @@ -506,4 +511,3 @@ function App() {
}

export default App

8 changes: 4 additions & 4 deletions application/src/components/ScanConfigModal.jsx
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,7 @@ const SCAN_SECONDS_PER_SQUARE_METER = 5
const SCAN_SECONDS_PER_POINT = 8
const FUEL_ESTIMATE_SECONDS_PER_POINT = 60

const WALL_MARGIN = 0.20
const WALL_MARGIN = 0.45

function calcTotalPoints(areaSize, precision, rowSpacing) {
const ew = areaSize - 2 * WALL_MARGIN
Expand All @@ -32,7 +32,7 @@ function ScanConfigModal({ open, onCancel, onConfirm }) {
const [precision, setPrecision] = useState(0.5)
const [originX, setOriginX] = useState(0)
const [originY, setOriginY] = useState(0)
const [rowSpacing, setRowSpacing] = useState(0.8)
const [rowSpacing, setRowSpacing] = useState(0.5)
const [dwellTime, setDwellTime] = useState(2.0)
const [waypointTimeout, setWaypointTimeout] = useState(30.0)

Expand Down Expand Up @@ -86,7 +86,7 @@ function ScanConfigModal({ open, onCancel, onConfirm }) {
</div>

<div className="scan-config-group">
<p className="scan-config-label">Origin (robot start position in meters)</p>
<p className="scan-config-label">Area Center in Odom (meters)</p>
<div className="scan-config-inputs">
<label className="scan-config-input-label">
X
Expand Down Expand Up @@ -121,7 +121,7 @@ function ScanConfigModal({ open, onCancel, onConfirm }) {
step="0.1"
min="0.1"
value={rowSpacing}
onChange={(e) => setRowSpacing(parseFloat(e.target.value) || 0.8)}
onChange={(e) => setRowSpacing(parseFloat(e.target.value) || 0.5)}
className="scan-config-input"
/>
</label>
Expand Down
Loading