[TASK] Mission Control Logic

[jorgenfj]

Description of task

Title

Add reusable mission control (start/pause/resume/restart/stop via ROS2 services) for all Yasmin FSM missions

Problem

We have multiple missions implemented as Yasmin state machines (e.g., docking), and we want common mission control functionality that can be reused across all missions/FSMs:

• Start
• Pause
• Resume
• Restart
• (optionally) Stop
• (optionally) Status

These controls should be triggered via ROS2 service calls and integrate cleanly without duplicating logic in each mission.

Goals / Requirements

• Provide a generic, reusable mission control layer that works across different missions/FSMs.

• Expose ROS2 services:

  • mission/start
  • mission/pause
  • mission/resume
  • mission/restart
  • mission/stop (optional but recommended)
  • mission/status (optional)

• Ensure pause/resume is cooperative (states can safely block/continue).

• Ensure restart and stop can cancel ongoing actions cleanly (or transition via CANCEL/ABORT outcomes).

• Integrate with existing Yasmin FSM patterns using the blackboard and ActionState wrappers.

Proposed Design

1) Add a reusable MissionControl component (library)

A thread-safe controller that tracks mission state and supports cooperative pausing:

• request(Start|Pause|Resume|Restart|Stop)

• checkpoint():

  • blocks while paused
  • returns false if stop/restart was requested

• take_restart_requested() to consume restart requests

This component is intended to be mission-agnostic and shared by all missions.

2) Add a reusable MissionRunner base node (services + FSM execution thread)

A base class that owns and runs:

• a Yasmin StateMachine
• a Yasmin Blackboard
• a worker thread that executes the SM

Mission-specific code plugs in via 3 overridable hooks:

• create_blackboard()
• build_sm(blackboard)
• reset_blackboard(blackboard) (for restart)

The runner provides the ROS2 services and forwards requests to MissionControl.

3) Mission integration pattern

• Store MissionControl in the blackboard:

  • key: "mission_control" → std::shared_ptr<MissionControl>

• In states with feedback loops / long-running work, call:

  • blackboard->get<std::shared_ptr<MissionControl>>("mission_control")->checkpoint();

• For restart/stop:

  • use checkpoint() returning false to trigger a CANCEL/ABORT transition, and/or
  • add a blackboard flag (e.g., "mission_cancel") for states to return CANCEL.

4) Expected pause semantics

• Pause should stop FSM progression (no transitions)
• Resume continues from the paused point.
• Restart resets blackboard + rebuilds SM (or returns to initial state) and starts again.
• Stop terminates mission execution cleanly and cancels actions if possible.

Notes

• How should pause interact with action servers?
• purely pause FSM progression (no new goals), vs
• also cancel/hold current goal
• Do current action servers support “hold” semantics, or only cancel/re-issue?

Suggested Workflow

No response

Specifications

• A single implementation of mission control is shared across all missions.

• New missions can adopt mission control by:

  1. inheriting from MissionRunner
  2. implementing the three hooks
  3. adding mission_control to blackboard
  4. calling checkpoint() in relevant states (feedback handlers and loops)

• Services successfully control mission execution:

  • pause blocks progress and optionally cancels/holds controller outputs
  • resume continues
  • restart resets state and starts over
  • stop cancels and exits

• No mission-specific duplication of start/pause/resume/restart service code.

Contacts

No response

Code Quality

• Every function in header files are documented (inputs/returns/exceptions)
• The project has automated tests that cover MOST of the functions and branches in functions (pytest/gtest)
• The code is documented on the wiki (provide link)

[jorgenfj]

Previous state machine implementation for inspiration: https://github.com/vortexntnu/vortex-auv/blob/main/mission/FSM/docking/src/docking.cpp

[jorgenfj]

https://github.com/uleroboticsgroup/yasmin