8a12e3bf50
- state_estimator was an unimplemented stub (empty callbacks, never published /rov/state) - removed node, entry point, launch block - motion_controller + mission_executor: /rov/state -> /odometry/filtered (the working robot_localization EKF output) - Eliminates redundant/competing state estimators; single source of truth - Verified: EKF publishes ~11Hz, both consumers subscribed
641 lines
24 KiB
Python
641 lines
24 KiB
Python
#!/usr/bin/env python3
|
||
"""
|
||
Mission Executor Node — Argonaut 3
|
||
====================================
|
||
Manages inspection mission state machine per ROV_Failsafe_Design_v2.0.
|
||
|
||
Sequences inspection waypoints, triggers image capture, maintains the
|
||
breadcrumb position buffer used by the failsafe return-to-safe path,
|
||
and responds correctly to each failsafe action level.
|
||
|
||
States: IDLE -> RUNNING -> PAUSED | COMPLETE | ABORTED
|
||
|
||
Failsafe integration:
|
||
ACTION_HOVER -> pause mission, hold position
|
||
ACTION_RETURN_TO_SAFE -> abort mission, return via breadcrumb path
|
||
ACTION_EMERGENCY_SURFACE -> abort mission, emergency surface immediately
|
||
|
||
Breadcrumb system:
|
||
Position logged every 2s from mission start.
|
||
Circular buffer of 1800 entries (1 hour of operation).
|
||
Used by return-to-safe to reverse the entry path safely.
|
||
|
||
Topics subscribed:
|
||
/odometry/filtered (nav_msgs/Odometry) — fused state from EKF
|
||
/rov/failsafe (rov_interfaces/FailsafeStatus) — safety override
|
||
|
||
Topics published:
|
||
/rov/cmd_vel (geometry_msgs/Twist) — velocity setpoints
|
||
/rov/mission/status (rov_interfaces/MissionStatus) — mission state
|
||
/rov/mission/waypoint (rov_interfaces/InspectionWaypoint) — current target
|
||
|
||
Services provided:
|
||
/rov/mission/command (rov_interfaces/MissionCommand) — start/pause/abort/resume
|
||
"""
|
||
|
||
import math
|
||
import yaml
|
||
import collections
|
||
from dataclasses import dataclass, field
|
||
from typing import List, Optional
|
||
|
||
import rclpy
|
||
from rclpy.node import Node
|
||
from geometry_msgs.msg import Twist, Point
|
||
from nav_msgs.msg import Odometry
|
||
from std_msgs.msg import Header
|
||
from rov_interfaces.msg import MissionStatus, FailsafeStatus, InspectionWaypoint
|
||
from rov_interfaces.srv import MissionCommand
|
||
|
||
|
||
# ---------------------------------------------------------------------------
|
||
# Breadcrumb entry
|
||
# ---------------------------------------------------------------------------
|
||
|
||
@dataclass
|
||
class Breadcrumb:
|
||
"""Single position record for the return-to-safe path."""
|
||
timestamp_s: float # ROS time in seconds
|
||
x: float # metres
|
||
y: float # metres
|
||
z: float # metres (negative = depth)
|
||
heading_deg: float # degrees
|
||
confidence: float # 0.0–1.0 (placeholder until full state estimation)
|
||
|
||
|
||
# ---------------------------------------------------------------------------
|
||
# Breadcrumb buffer
|
||
# ---------------------------------------------------------------------------
|
||
|
||
class BreadcrumbBuffer:
|
||
"""
|
||
Circular buffer of position records logged during mission.
|
||
1800 entries at 2s interval = 1 hour of operation.
|
||
Older entries are discarded as new ones are added.
|
||
"""
|
||
|
||
CAPACITY = 1800 # entries
|
||
LOG_INTERVAL_S = 2.0 # seconds between entries
|
||
MIN_CONFIDENCE = 0.3 # entries below this are flagged during reversal
|
||
|
||
def __init__(self):
|
||
self._buffer: collections.deque = collections.deque(maxlen=self.CAPACITY)
|
||
self._last_log_time: float = 0.0
|
||
|
||
def update(self, now_s: float, position: Point, heading_deg: float,
|
||
confidence: float = 1.0):
|
||
"""Log a breadcrumb if the log interval has elapsed."""
|
||
if now_s - self._last_log_time >= self.LOG_INTERVAL_S:
|
||
self._buffer.append(Breadcrumb(
|
||
timestamp_s=now_s,
|
||
x=position.x,
|
||
y=position.y,
|
||
z=position.z,
|
||
heading_deg=heading_deg,
|
||
confidence=confidence,
|
||
))
|
||
self._last_log_time = now_s
|
||
|
||
def get_return_path(self) -> List[Breadcrumb]:
|
||
"""
|
||
Return the breadcrumb buffer in reverse order for path reversal.
|
||
Entries below MIN_CONFIDENCE are filtered — skipped in favour of
|
||
the next reliable entry to reduce navigation uncertainty.
|
||
"""
|
||
reliable = [b for b in self._buffer if b.confidence >= self.MIN_CONFIDENCE]
|
||
return list(reversed(reliable))
|
||
|
||
def clear(self):
|
||
"""Clear all entries — called at mission start."""
|
||
self._buffer.clear()
|
||
self._last_log_time = 0.0
|
||
|
||
@property
|
||
def entry_count(self) -> int:
|
||
"""Number of entries currently in buffer."""
|
||
return len(self._buffer)
|
||
|
||
|
||
# ---------------------------------------------------------------------------
|
||
# Mission executor node
|
||
# ---------------------------------------------------------------------------
|
||
|
||
class MissionExecutor(Node):
|
||
"""
|
||
Executes inspection missions from a waypoint sequence.
|
||
Integrates with the failsafe monitor and maintains breadcrumb buffer.
|
||
"""
|
||
|
||
# Internal mission states
|
||
STATE_IDLE = 'IDLE'
|
||
STATE_RUNNING = 'RUNNING'
|
||
STATE_PAUSED = 'PAUSED'
|
||
STATE_COMPLETE = 'COMPLETE'
|
||
STATE_ABORTED = 'ABORTED'
|
||
|
||
def __init__(self):
|
||
super().__init__('mission_executor')
|
||
|
||
# ------------------------------------------------------------------
|
||
# Parameters (from dev.yaml / field.yaml)
|
||
# ------------------------------------------------------------------
|
||
|
||
self.declare_parameter('waypoint_arrival_radius_m', 0.5)
|
||
self.declare_parameter('waypoint_timeout_s', 60.0)
|
||
self.declare_parameter('image_capture_delay_s', 1.0)
|
||
|
||
self._arrival_radius = self.get_parameter('waypoint_arrival_radius_m').value
|
||
self._wp_timeout_s = self.get_parameter('waypoint_timeout_s').value
|
||
self._capture_delay_s = self.get_parameter('image_capture_delay_s').value
|
||
|
||
# ------------------------------------------------------------------
|
||
# Mission state
|
||
# ------------------------------------------------------------------
|
||
|
||
self.state = self.STATE_IDLE
|
||
self.mission_id = ''
|
||
self.waypoints: List[InspectionWaypoint] = []
|
||
self.current_wp_index = 0
|
||
|
||
# Entry point — set when mission starts, used for safe zone reference
|
||
self.entry_point: Optional[Point] = None
|
||
|
||
# Operation mode — affects return-to-safe path choice
|
||
self.operation_mode = 'tethered' # 'tethered' or 'untethered'
|
||
|
||
# ------------------------------------------------------------------
|
||
# Sensor state
|
||
# ------------------------------------------------------------------
|
||
|
||
self.current_position: Optional[Point] = None
|
||
self.current_heading_deg: float = 0.0
|
||
|
||
# ------------------------------------------------------------------
|
||
# Failsafe state
|
||
# ------------------------------------------------------------------
|
||
|
||
self.last_failsafe_action = FailsafeStatus.ACTION_NONE
|
||
self.last_assessment_state = FailsafeStatus.ASSESSMENT_GREEN
|
||
|
||
# ------------------------------------------------------------------
|
||
# Breadcrumb buffer
|
||
# ------------------------------------------------------------------
|
||
|
||
self.breadcrumbs = BreadcrumbBuffer()
|
||
|
||
# ------------------------------------------------------------------
|
||
# Waypoint timing
|
||
# ------------------------------------------------------------------
|
||
|
||
self.wp_start_time: Optional[float] = None # When current wp navigation began
|
||
self.capture_start_time: Optional[float] = None # When image capture loiter began
|
||
self.capturing_image: bool = False
|
||
|
||
# ------------------------------------------------------------------
|
||
# Subscribers
|
||
# ------------------------------------------------------------------
|
||
|
||
self.create_subscription(
|
||
Odometry, '/odometry/filtered',
|
||
self._state_callback, 10)
|
||
|
||
self.create_subscription(
|
||
FailsafeStatus, '/rov/failsafe',
|
||
self._failsafe_callback, 10)
|
||
|
||
# ------------------------------------------------------------------
|
||
# Publishers
|
||
# ------------------------------------------------------------------
|
||
|
||
self.cmd_pub = self.create_publisher(
|
||
Twist, '/rov/cmd_vel', 10)
|
||
|
||
self.status_pub = self.create_publisher(
|
||
MissionStatus, '/rov/mission/status', 10)
|
||
|
||
self.waypoint_pub = self.create_publisher(
|
||
InspectionWaypoint, '/rov/mission/waypoint', 10)
|
||
|
||
# ------------------------------------------------------------------
|
||
# Service
|
||
# ------------------------------------------------------------------
|
||
|
||
self.create_service(
|
||
MissionCommand, '/rov/mission/command',
|
||
self._command_callback)
|
||
|
||
# ------------------------------------------------------------------
|
||
# Timers
|
||
# ------------------------------------------------------------------
|
||
|
||
# Mission loop at 10 Hz
|
||
self.create_timer(0.1, self._mission_loop)
|
||
|
||
# Status publisher at 2 Hz
|
||
self.create_timer(0.5, self._publish_status)
|
||
|
||
self.get_logger().info('MissionExecutor started')
|
||
|
||
# ------------------------------------------------------------------
|
||
# Subscriber callbacks
|
||
# ------------------------------------------------------------------
|
||
|
||
def _state_callback(self, msg: Odometry):
|
||
"""Update current position and heading from state estimator."""
|
||
self.current_position = msg.pose.pose.position
|
||
|
||
# Extract heading from quaternion (yaw only — simplified)
|
||
q = msg.pose.pose.orientation
|
||
siny_cosp = 2.0 * (q.w * q.z + q.x * q.y)
|
||
cosy_cosp = 1.0 - 2.0 * (q.y * q.y + q.z * q.z)
|
||
self.current_heading_deg = math.degrees(math.atan2(siny_cosp, cosy_cosp))
|
||
|
||
# Log breadcrumb if mission is active
|
||
if self.state == self.STATE_RUNNING and self.current_position is not None:
|
||
now_s = self.get_clock().now().nanoseconds / 1e9
|
||
self.breadcrumbs.update(
|
||
now_s,
|
||
self.current_position,
|
||
self.current_heading_deg,
|
||
)
|
||
|
||
def _failsafe_callback(self, msg: FailsafeStatus):
|
||
"""
|
||
Respond to failsafe actions per design spec priority.
|
||
Different actions require different mission responses.
|
||
"""
|
||
action = msg.action
|
||
assessment_state = msg.assessment_state
|
||
|
||
# Store for use in mission loop
|
||
self.last_failsafe_action = action
|
||
self.last_assessment_state = assessment_state
|
||
|
||
# --- Emergency surface: abort immediately, no controlled return ---
|
||
if action == FailsafeStatus.ACTION_EMERGENCY_SURFACE:
|
||
if self.state not in (self.STATE_ABORTED, self.STATE_COMPLETE):
|
||
self.get_logger().error(
|
||
'EMERGENCY SURFACE commanded — aborting mission immediately')
|
||
self._abort_mission('Emergency surface — failsafe')
|
||
return
|
||
|
||
# --- Return to safe: abort mission, initiate breadcrumb reversal ---
|
||
if action == FailsafeStatus.ACTION_RETURN_TO_SAFE:
|
||
if self.state not in (self.STATE_ABORTED, self.STATE_COMPLETE):
|
||
self.get_logger().warn(
|
||
f'RETURN TO SAFE commanded — aborting mission '
|
||
f'(breadcrumbs: {self.breadcrumbs.entry_count})')
|
||
self._abort_mission('Return to safe — failsafe')
|
||
# Breadcrumb path handed off to motion controller here
|
||
# (Phase 3+ — motion controller implements path reversal)
|
||
return
|
||
|
||
# --- Hover: pause mission, hold position ---
|
||
if action == FailsafeStatus.ACTION_HOVER:
|
||
if self.state == self.STATE_RUNNING:
|
||
self.get_logger().warn('HOVER commanded — pausing mission')
|
||
self.state = self.STATE_PAUSED
|
||
self._stop_motion()
|
||
return
|
||
|
||
# --- No action: if paused by failsafe and now clear, resume ---
|
||
if action == FailsafeStatus.ACTION_NONE:
|
||
if (self.state == self.STATE_PAUSED and
|
||
assessment_state == FailsafeStatus.ASSESSMENT_GREEN):
|
||
self.get_logger().info(
|
||
'Failsafe cleared (GREEN) — resuming mission')
|
||
self.state = self.STATE_RUNNING
|
||
|
||
# ------------------------------------------------------------------
|
||
# Service callback — mission commands
|
||
# ------------------------------------------------------------------
|
||
|
||
def _command_callback(self, request, response):
|
||
"""
|
||
Handle mission commands: START, PAUSE, RESUME, ABORT, LOAD.
|
||
LOAD accepts a YAML waypoint file path via parameters[0].
|
||
"""
|
||
cmd = request.command.upper()
|
||
|
||
if cmd == 'START':
|
||
if self.state != self.STATE_IDLE:
|
||
response.success = False
|
||
response.message = f'Cannot START — current state: {self.state}'
|
||
return response
|
||
|
||
if not self.waypoints:
|
||
response.success = False
|
||
response.message = 'Cannot START — no waypoints loaded'
|
||
return response
|
||
|
||
# Record entry point at mission start
|
||
self.entry_point = (
|
||
self.current_position if self.current_position is not None
|
||
else Point()
|
||
)
|
||
|
||
# Set operation mode from parameters if provided
|
||
if request.parameters:
|
||
self.operation_mode = request.parameters[0]
|
||
|
||
self.breadcrumbs.clear()
|
||
self.current_wp_index = 0
|
||
self.mission_id = request.mission_id
|
||
self.state = self.STATE_RUNNING
|
||
self.wp_start_time = self.get_clock().now().nanoseconds / 1e9
|
||
|
||
self.get_logger().info(
|
||
f'Mission {self.mission_id} started — '
|
||
f'{len(self.waypoints)} waypoints, '
|
||
f'mode={self.operation_mode}'
|
||
)
|
||
response.success = True
|
||
response.message = f'Mission {self.mission_id} started'
|
||
|
||
elif cmd == 'LOAD':
|
||
# Load waypoints from YAML file path in parameters[0]
|
||
if not request.parameters:
|
||
response.success = False
|
||
response.message = 'LOAD requires parameters[0] = YAML file path'
|
||
return response
|
||
|
||
loaded, message = self._load_waypoints(request.parameters[0])
|
||
response.success = loaded
|
||
response.message = message
|
||
|
||
elif cmd == 'PAUSE':
|
||
if self.state != self.STATE_RUNNING:
|
||
response.success = False
|
||
response.message = f'Cannot PAUSE — current state: {self.state}'
|
||
return response
|
||
self.state = self.STATE_PAUSED
|
||
self._stop_motion()
|
||
response.success = True
|
||
response.message = 'Mission paused'
|
||
|
||
elif cmd == 'RESUME':
|
||
if self.state != self.STATE_PAUSED:
|
||
response.success = False
|
||
response.message = f'Cannot RESUME — current state: {self.state}'
|
||
return response
|
||
self.state = self.STATE_RUNNING
|
||
self.wp_start_time = self.get_clock().now().nanoseconds / 1e9
|
||
response.success = True
|
||
response.message = 'Mission resumed'
|
||
|
||
elif cmd == 'ABORT':
|
||
self._abort_mission('Operator abort command')
|
||
response.success = True
|
||
response.message = 'Mission aborted'
|
||
|
||
else:
|
||
response.success = False
|
||
response.message = f'Unknown command: {cmd}'
|
||
|
||
return response
|
||
|
||
# ------------------------------------------------------------------
|
||
# Mission execution loop — 10 Hz
|
||
# ------------------------------------------------------------------
|
||
|
||
def _mission_loop(self):
|
||
"""Main mission execution loop."""
|
||
if self.state != self.STATE_RUNNING:
|
||
return
|
||
|
||
if not self.waypoints:
|
||
self.get_logger().info('No waypoints — mission complete')
|
||
self.state = self.STATE_COMPLETE
|
||
return
|
||
|
||
if self.current_wp_index >= len(self.waypoints):
|
||
self.get_logger().info('All waypoints reached — mission complete')
|
||
self.state = self.STATE_COMPLETE
|
||
self._stop_motion()
|
||
return
|
||
|
||
if self.current_position is None:
|
||
self.get_logger().warn('No position estimate — waiting')
|
||
return
|
||
|
||
current_wp = self.waypoints[self.current_wp_index]
|
||
now_s = self.get_clock().now().nanoseconds / 1e9
|
||
|
||
# --- Publish current waypoint target ---
|
||
self.waypoint_pub.publish(current_wp)
|
||
|
||
# --- Check waypoint timeout ---
|
||
if (self.wp_start_time is not None and
|
||
now_s - self.wp_start_time > self._wp_timeout_s):
|
||
self.get_logger().warn(
|
||
f'Waypoint {current_wp.waypoint_id} timed out after '
|
||
f'{self._wp_timeout_s}s — advancing to next'
|
||
)
|
||
self._advance_waypoint(now_s)
|
||
return
|
||
|
||
# --- Check arrival at waypoint ---
|
||
distance = self._distance_to(current_wp.position)
|
||
|
||
if distance > self._arrival_radius:
|
||
# Not yet arrived — navigate toward waypoint
|
||
self._navigate_toward(current_wp)
|
||
return
|
||
|
||
# --- Arrived at waypoint ---
|
||
self._stop_motion()
|
||
|
||
if current_wp.capture_image and not self.capturing_image:
|
||
# Start image capture loiter
|
||
self.capturing_image = True
|
||
self.capture_start_time = now_s
|
||
self.get_logger().info(
|
||
f'Arrived at {current_wp.waypoint_id} — '
|
||
f'capturing image (loiter {current_wp.loiter_time_s}s)'
|
||
)
|
||
return
|
||
|
||
if self.capturing_image:
|
||
# Wait for loiter time before advancing
|
||
elapsed_capture = now_s - self.capture_start_time
|
||
if elapsed_capture < current_wp.loiter_time_s + self._capture_delay_s:
|
||
return
|
||
self.capturing_image = False
|
||
self.get_logger().info(
|
||
f'Image capture complete at {current_wp.waypoint_id}')
|
||
|
||
# Advance to next waypoint
|
||
self._advance_waypoint(now_s)
|
||
|
||
# ------------------------------------------------------------------
|
||
# Navigation helpers
|
||
# ------------------------------------------------------------------
|
||
|
||
def _navigate_toward(self, waypoint: InspectionWaypoint):
|
||
"""
|
||
Publish proportional velocity command toward the waypoint.
|
||
Simple P-controller — replaced by full motion planner in Phase 3+.
|
||
Max speed: 0.3 m/s in XY, 0.1 m/s in Z.
|
||
"""
|
||
MAX_XY = 0.3 # m/s
|
||
MAX_Z = 0.1 # m/s
|
||
KP = 0.5 # proportional gain
|
||
|
||
dx = waypoint.position.x - self.current_position.x
|
||
dy = waypoint.position.y - self.current_position.y
|
||
dz = waypoint.position.z - self.current_position.z
|
||
|
||
# Clamp to max speeds
|
||
vx = max(-MAX_XY, min(MAX_XY, KP * dx))
|
||
vy = max(-MAX_XY, min(MAX_XY, KP * dy))
|
||
vz = max(-MAX_Z, min(MAX_Z, KP * dz))
|
||
|
||
cmd = Twist()
|
||
cmd.linear.x = vx
|
||
cmd.linear.y = vy
|
||
cmd.linear.z = vz
|
||
self.cmd_pub.publish(cmd)
|
||
|
||
def _stop_motion(self):
|
||
"""Publish zero velocity to halt the ROV."""
|
||
self.cmd_pub.publish(Twist()) # All zeros
|
||
|
||
def _distance_to(self, target: Point) -> float:
|
||
"""Euclidean distance from current position to target."""
|
||
if self.current_position is None:
|
||
return float('inf')
|
||
dx = target.x - self.current_position.x
|
||
dy = target.y - self.current_position.y
|
||
dz = target.z - self.current_position.z
|
||
return math.sqrt(dx*dx + dy*dy + dz*dz)
|
||
|
||
def _advance_waypoint(self, now_s: float):
|
||
"""Move to the next waypoint in the sequence."""
|
||
self.current_wp_index += 1
|
||
self.wp_start_time = now_s
|
||
self.capturing_image = False
|
||
|
||
if self.current_wp_index < len(self.waypoints):
|
||
next_wp = self.waypoints[self.current_wp_index]
|
||
self.get_logger().info(
|
||
f'Advancing to waypoint {self.current_wp_index}: '
|
||
f'{next_wp.waypoint_id}'
|
||
)
|
||
|
||
def _abort_mission(self, reason: str):
|
||
"""Abort the mission and stop motion."""
|
||
self.state = self.STATE_ABORTED
|
||
self._stop_motion()
|
||
self.get_logger().warn(f'Mission aborted: {reason}')
|
||
|
||
# ------------------------------------------------------------------
|
||
# Waypoint loading
|
||
# ------------------------------------------------------------------
|
||
|
||
def _load_waypoints(self, yaml_path: str):
|
||
"""
|
||
Load InspectionWaypoint list from a YAML mission file.
|
||
Expected format:
|
||
waypoints:
|
||
- id: "WP1"
|
||
x: 1.0
|
||
y: 2.0
|
||
z: -5.0
|
||
heading_deg: 90.0
|
||
standoff_m: 0.5
|
||
loiter_time_s: 3.0
|
||
capture_image: true
|
||
panel_id: "P1"
|
||
"""
|
||
try:
|
||
with open(yaml_path, 'r') as f:
|
||
data = yaml.safe_load(f)
|
||
|
||
if 'waypoints' not in data:
|
||
return False, f'No waypoints key in {yaml_path}'
|
||
|
||
self.waypoints = []
|
||
for entry in data['waypoints']:
|
||
wp = InspectionWaypoint()
|
||
wp.header.stamp = self.get_clock().now().to_msg()
|
||
wp.waypoint_id = str(entry.get('id', ''))
|
||
wp.position.x = float(entry.get('x', 0.0))
|
||
wp.position.y = float(entry.get('y', 0.0))
|
||
wp.position.z = float(entry.get('z', 0.0))
|
||
wp.heading_deg = float(entry.get('heading_deg', 0.0))
|
||
wp.standoff_m = float(entry.get('standoff_m', 0.5))
|
||
wp.loiter_time_s = float(entry.get('loiter_time_s', 2.0))
|
||
wp.capture_image = bool(entry.get('capture_image', False))
|
||
wp.panel_id = str(entry.get('panel_id', ''))
|
||
self.waypoints.append(wp)
|
||
|
||
msg = f'Loaded {len(self.waypoints)} waypoints from {yaml_path}'
|
||
self.get_logger().info(msg)
|
||
return True, msg
|
||
|
||
except FileNotFoundError:
|
||
return False, f'Mission file not found: {yaml_path}'
|
||
except Exception as e:
|
||
return False, f'Failed to load waypoints: {str(e)}'
|
||
|
||
# ------------------------------------------------------------------
|
||
# Status publishing — 2 Hz
|
||
# ------------------------------------------------------------------
|
||
|
||
def _publish_status(self):
|
||
"""Publish current mission status."""
|
||
msg = MissionStatus()
|
||
msg.header.stamp = self.get_clock().now().to_msg()
|
||
msg.mission_id = self.mission_id
|
||
|
||
# Map state string to uint8
|
||
msg.state = {
|
||
self.STATE_IDLE: MissionStatus.STATE_IDLE,
|
||
self.STATE_RUNNING: MissionStatus.STATE_RUNNING,
|
||
self.STATE_PAUSED: MissionStatus.STATE_PAUSED,
|
||
self.STATE_COMPLETE: MissionStatus.STATE_COMPLETE,
|
||
self.STATE_ABORTED: MissionStatus.STATE_ABORTED,
|
||
}.get(self.state, MissionStatus.STATE_IDLE)
|
||
|
||
# Progress
|
||
total = len(self.waypoints)
|
||
msg.progress_percent = (
|
||
(self.current_wp_index / total * 100.0) if total > 0 else 0.0
|
||
)
|
||
|
||
# Current task description
|
||
if self.state == self.STATE_RUNNING and total > 0:
|
||
wp_id = (self.waypoints[self.current_wp_index].waypoint_id
|
||
if self.current_wp_index < total else 'complete')
|
||
msg.current_task = f'WP {self.current_wp_index + 1}/{total}: {wp_id}'
|
||
else:
|
||
msg.current_task = self.state
|
||
|
||
msg.message = (
|
||
f'{self.state} | '
|
||
f'WP {self.current_wp_index}/{total} | '
|
||
f'Breadcrumbs: {self.breadcrumbs.entry_count}'
|
||
)
|
||
self.status_pub.publish(msg)
|
||
|
||
|
||
# ---------------------------------------------------------------------------
|
||
# Entry point
|
||
# ---------------------------------------------------------------------------
|
||
|
||
def main(args=None):
|
||
"""ROS2 node entry point."""
|
||
rclpy.init(args=args)
|
||
node = MissionExecutor()
|
||
try:
|
||
rclpy.spin(node)
|
||
except KeyboardInterrupt:
|
||
pass
|
||
finally:
|
||
node.destroy_node()
|
||
rclpy.shutdown()
|
||
|
||
|
||
if __name__ == '__main__':
|
||
main()
|