Goal ID and History Support

CMakeLists.txt

@@ -16,6 +16,7 @@ find_package(action_msgs REQUIRED)
 find_package(geographic_msgs REQUIRED)
 find_package(nav_msgs REQUIRED)
 find_package(ros2_medkit_msgs REQUIRED)
+find_package(unique_identifier_msgs REQUIRED)
 
 set(dependencies
   std_msgs
@@ -26,11 +27,13 @@ set(dependencies
   geographic_msgs
   action_msgs
   ros2_medkit_msgs
+  unique_identifier_msgs
 )
 
 rosidl_generate_interfaces(${PROJECT_NAME}
   # Messages
   "msg/CortexCommand.msg"
+  "msg/CortexCommandRecord.msg"
   "msg/CortexFeedback.msg"
   "msg/CortexStatus.msg"
   "msg/FlowCommand.msg"
@@ -54,6 +57,8 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "srv/ListReferenceTargets.srv"
   "srv/ListVehicleOptions.srv"
   "srv/ModifyCortexQueue.srv"
+  "srv/QueryCortexHistory.srv"
+  "srv/QueryCortexQueue.srv"
   "srv/VehicleJointCommand.srv"
   DEPENDENCIES ${dependencies}
 )

msg/CortexCommandRecord.msg

@@ -0,0 +1,53 @@
+# CortexCommandRecord
+#
+# Cortex's bookkeeping wrapper around a single CortexCommand. It carries the
+# Cortex-assigned identity, lifecycle state, and terminal outcome, and is the
+# element type of both the live queue and the command history.
+
+# Cortex-assigned identifier, unique per accepted command. Reused by the caller
+# as the goal id. Returned from ModifyCortexQueue as command_ids.
+unique_identifier_msgs/UUID id
+
+# The wrapped command.
+polymath_msgs/CortexCommand command
+
+# Short human-readable description of the command (type plus salient content).
+string summary
+
+# Lifecycle State (Enum)
+#
+# QUEUED: accepted and waiting in the queue
+# ACTIVE: currently dispatching (front of queue)
+# TERMINAL: finished; see outcome
+uint8 QUEUED=0
+uint8 ACTIVE=1
+uint8 TERMINAL=2
+uint8 state
+
+# Terminal Outcome (Enum)
+#
+# OUTCOME_UNKNOWN: not yet terminal
+# SUCCEEDED: dispatched to completion
+# FAILED: dispatch failed or aborted mid-flight
+# CANCELED: discarded by a stop
+# PREEMPTED: discarded by a preempting command
+# REJECTED: never accepted (failed validation)
+uint8 OUTCOME_UNKNOWN=0
+uint8 SUCCEEDED=1
+uint8 FAILED=2
+uint8 CANCELED=3
+uint8 PREEMPTED=4
+uint8 REJECTED=5
+uint8 outcome
+
+# Outcome context, e.g. the validation message for a rejection.
+string detail
+
+# Time the command was accepted and queued.
+builtin_interfaces/Time queued_time
+
+# Time the command became active. Zero until started.
+builtin_interfaces/Time started_time
+
+# Time the command reached a terminal state. Zero until terminal.
+builtin_interfaces/Time completed_time

msg/CortexFeedback.msg

@@ -14,8 +14,11 @@ polymath_msgs/VehicleFeedback vehicle_feedback
 # Requires localization to be up to return useful data
 geographic_msgs/GeoPoseStamped current_pose
 
-# Number in queue that is currently running
+# DEPRECATED: use current_command_id. Historically always the front (0).
 uint32 current_command_index
 
+# Id of the currently active command. Zero UUID when nothing is active.
+unique_identifier_msgs/UUID current_command_id
+
 # Number of commands remaining in the queue
 uint32 number_commands_remaining

package.xml

@@ -17,6 +17,7 @@
   <depend>sensor_msgs</depend>
   <depend>action_msgs</depend>
   <depend>ros2_medkit_msgs</depend>
+  <depend>unique_identifier_msgs</depend>
 
   <build_depend>rosidl_default_generators</build_depend>
   <exec_depend>rosidl_default_runtime</exec_depend>

srv/ModifyCortexQueue.srv

@@ -26,3 +26,8 @@ polymath_msgs/CortexCommand[] commands
 ---
 bool success
 string message
+
+# Cortex-assigned ids, aligned 1:1 with request.commands and in the same order.
+# Each is the goal id for the corresponding command. Empty when the request was
+# rejected (success is false).
+unique_identifier_msgs/UUID[] command_ids

srv/QueryCortexHistory.srv

@@ -0,0 +1,18 @@
+# QueryCortexHistory
+#
+# Returns terminal-state command records from Cortex's bounded history ring,
+# newest first, subject to the optional filters below.
+
+# Maximum number of records to return. Zero means no limit.
+uint32 max_count
+
+# Only return records completed at or after this time. Zero disables the filter.
+builtin_interfaces/Time since
+
+# Only return the record with this id. Zero UUID disables the filter.
+unique_identifier_msgs/UUID command_id
+
+---
+bool success
+string message
+polymath_msgs/CortexCommandRecord[] records

srv/QueryCortexQueue.srv

@@ -0,0 +1,17 @@
+# QueryCortexQueue
+#
+# Returns a snapshot of the live command queue plus the currently active command.
+
+---
+bool success
+string message
+
+# Queued commands in dispatch order. The active command, if any, is the front
+# entry and is reported with state ACTIVE.
+polymath_msgs/CortexCommandRecord[] queued
+
+# Id of the currently active command. Zero UUID when nothing is active.
+unique_identifier_msgs/UUID active_command_id
+
+# Number of commands remaining in the queue.
+uint32 number_commands_remaining