1414
1515#pragma once
1616
17+ #include < array>
1718#include < cstdint>
1819#include < deque>
1920#include < map>
@@ -43,37 +44,71 @@ namespace ros2_medkit_action_status_bridge {
4344// / Status mapping (action_msgs/msg/GoalStatus):
4445// / - ABORTED (6) -> fault (severity configurable, default ERROR)
4546// / - CANCELED (5) -> fault only if canceled_is_fault (usually intentional)
46- // / - SUCCEEDED (4)-> PASSED (heals the per-action ABORTED code) if enabled
47+ // / - SUCCEEDED (4)-> PASSED (heals the per-action fault code) if enabled
48+ // /
49+ // / Fault state is per-ACTION, not per-goal: every message is scanned for the net
50+ // / state of the whole GoalStatusArray and a fault is raised/healed only on the
51+ // / action-level transition. See `derive_state`.
4752class ActionStatusBridgeNode : public rclcpp ::Node {
4853 public:
4954 explicit ActionStatusBridgeNode (const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
5055
56+ // / Net fault state of an action, derived from a whole GoalStatusArray.
57+ // / - kUnknown: no terminal goal in the array yet (no transition)
58+ // / - kHealthy: array has terminal goals and none of them are failing
59+ // / - kFailed: at least one goal is failing (ABORTED, or CANCELED when
60+ // / canceled_is_fault)
61+ enum class ActionState { kUnknown , kHealthy , kFailed };
62+
5163 // / Derive the action name from a `/<action>/_action/status` topic name.
5264 // / Returns empty when the topic is not an action status topic.
5365 static std::string action_name_from_status_topic (const std::string & topic);
5466
55- // / Build the ABORTED fault code for an action name.
56- // / Format: <PREFIX>_<ACTION>_ABORTED, charset/length per medkit rules.
57- std::string aborted_fault_code (const std::string & action_name) const ;
67+ // / Build the fault code for an action name and terminal status.
68+ // / Format: <PREFIX>_<ACTION>_<ABORTED|CANCELED>, charset/length per medkit
69+ // / rules. `canceled` selects the CANCELED suffix.
70+ std::string fault_code_for (const std::string & action_name, bool canceled) const ;
5871
5972 // / Lowercase hex of a 16-byte goal UUID, for dedup keys and short display.
6073 static std::string uuid_to_hex (const std::array<uint8_t , 16 > & uuid);
6174
75+ // / Scan a whole GoalStatusArray and return the action-level net state.
76+ // / `canceled_is_fault` decides whether CANCELED counts as failing. Order of
77+ // / the goals in the array does not affect the result (any failing goal wins).
78+ static ActionState derive_state (const action_msgs::msg::GoalStatusArray & msg, bool canceled_is_fault);
79+
80+ // / Update per-action state from a message and act on the transition only.
81+ // / Returns the state that was reported on (kFailed on raise, kHealthy on
82+ // / heal) or kUnknown when nothing was reported. Side-effect free w.r.t.
83+ // / reporting when `reporter` is null (test seam): the transition decision and
84+ // / stored per-action state still update, so tests assert on the return value.
85+ ActionState apply_message (const std::string & action_name, const action_msgs::msg::GoalStatusArray & msg,
86+ ros2_medkit_fault_reporter::FaultReporter * reporter);
87+
6288 private:
6389 void rescan_actions ();
64- void status_callback (const std::string & action_name,
65- const action_msgs::msg::GoalStatusArray::ConstSharedPtr & msg);
90+ void status_callback (const std::string & action_name, const action_msgs::msg::GoalStatusArray::ConstSharedPtr & msg);
6691
6792 ros2_medkit_fault_reporter::FaultReporter * reporter_for (const std::string & action_name);
6893
69- // / Returns true if this (goal, status) pair was not handled before, marking
70- // / it handled. Bounded to avoid unbounded growth.
71- bool mark_handled (const std::string & goal_status_key);
94+ // / Resolve the action server's node FQN from its status-topic publisher, for
95+ // / use as the fault source_id so faults associate with the gateway's SOVD
96+ // / entity. Falls back to the action name if no publisher is visible.
97+ std::string server_fqn_for_action (const std::string & action_name);
98+
99+ // / Returns true if this (goal, status) pair was not logged before, marking it
100+ // / logged. Bounded to avoid unbounded growth. Suppresses duplicate LOG lines
101+ // / only; never gates the action-level state transition.
102+ bool mark_logged (const std::string & goal_status_key);
72103
73104 bool action_is_eligible (const std::string & action_name) const ;
74105
75106 void load_parameters ();
76107
108+ // / Drop subscriptions, reporters and per-action state for actions whose status
109+ // / topic has vanished from `present_topics`.
110+ void prune_vanished (const std::map<std::string, std::string> & present_topics);
111+
77112 static std::string to_upper_snake (const std::string & in, size_t max_len);
78113
79114 rclcpp::TimerBase::SharedPtr rescan_timer_;
@@ -82,11 +117,15 @@ class ActionStatusBridgeNode : public rclcpp::Node {
82117 std::map<std::string, std::unique_ptr<ros2_medkit_fault_reporter::FaultReporter>> reporters_;
83118 std::mutex reporters_mutex_;
84119
85- // Bounded dedup of handled (goal_id:status) keys.
86- std::unordered_set<std::string> handled_;
87- std::deque<std::string> handled_order_;
88- std::mutex handled_mutex_;
89- size_t handled_capacity_;
120+ // Last reported action-level state, keyed by action name. Drives transitions.
121+ std::map<std::string, ActionState> last_reported_state_;
122+ std::mutex state_mutex_;
123+
124+ // Bounded dedup of logged (goal_id:status) keys (LOG suppression only).
125+ std::unordered_set<std::string> logged_;
126+ std::deque<std::string> logged_order_;
127+ std::mutex logged_mutex_;
128+ size_t logged_capacity_;
90129
91130 // Configuration
92131 uint8_t aborted_severity_;
@@ -96,6 +135,29 @@ class ActionStatusBridgeNode : public rclcpp::Node {
96135 std::string code_prefix_;
97136 std::vector<std::string> exclude_actions_;
98137 std::vector<std::string> include_only_actions_;
138+
139+ friend class ActionStatusBridgeTestAccess ;
140+ };
141+
142+ // / Test-only accessor for the bridge's private maps and prune path. Lets unit
143+ // / tests exercise rescan add+prune without a live action graph.
144+ class ActionStatusBridgeTestAccess {
145+ public:
146+ explicit ActionStatusBridgeTestAccess (ActionStatusBridgeNode * node) : node_(node) {
147+ }
148+
149+ // / Seed a watched action (subscription placeholder + per-action state) as if
150+ // / rescan had discovered its `/<action>/_action/status` topic.
151+ void add_watched (const std::string & action_name);
152+
153+ // / Run the prune pass against a set of still-present action names.
154+ void prune_to (const std::vector<std::string> & present_action_names);
155+
156+ bool is_watched (const std::string & action_name) const ;
157+ bool has_state (const std::string & action_name) const ;
158+
159+ private:
160+ ActionStatusBridgeNode * node_;
99161};
100162
101163} // namespace ros2_medkit_action_status_bridge
0 commit comments