Skip to content

Commit 4fda00c

Browse files
mfaferek93mfaferek93
committed
feat(action_status_bridge): add aborted-goal to faults bridge
New ros2_medkit_action_status_bridge: watches */_action/status and turns terminal ABORTED goal states into FaultManager faults, healing on SUCCEEDED. Catches action-result failures that never reach /rosout or /diagnostics (MoveIt, Nav2, ros2_control). Refs #421
1 parent c6fef9e commit 4fda00c

10 files changed

Lines changed: 730 additions & 0 deletions

File tree

‎src/ros2_medkit_action_status_bridge/CHANGELOG.rst‎

Lines changed: 11 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,11 @@
1+
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
2+
Changelog for package ros2_medkit_action_status_bridge
3+
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
4+
5+
Forthcoming
6+
-----------
7+
* Initial release: generic action-status bridge. Watches every
8+
``/<action>/_action/status`` topic and turns ABORTED goals into FaultManager
9+
faults (``<PREFIX>_<ACTION>_ABORTED``), heals on SUCCEEDED, with per-goal
10+
dedup. Captures the terminal action verdict that the diagnostic and log
11+
bridges cannot see.

‎src/ros2_medkit_action_status_bridge/CMakeLists.txt‎

Lines changed: 112 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,112 @@
1+
# Copyright 2026 mfaferek93, bburda
2+
#
3+
# Licensed under the Apache License, Version 2.0 (the "License");
4+
# you may not use this file except in compliance with the License.
5+
# You may obtain a copy of the License at
6+
#
7+
# http://www.apache.org/licenses/LICENSE-2.0
8+
#
9+
# Unless required by applicable law or agreed to in writing, software
10+
# distributed under the License is distributed on an "AS IS" BASIS,
11+
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
# See the License for the specific language governing permissions and
13+
# limitations under the License.
14+
15+
cmake_minimum_required(VERSION 3.8)
16+
project(ros2_medkit_action_status_bridge)
17+
18+
set(CMAKE_CXX_STANDARD 17)
19+
set(CMAKE_CXX_STANDARD_REQUIRED ON)
20+
set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
21+
22+
find_package(ros2_medkit_cmake REQUIRED)
23+
include(ROS2MedkitCcache)
24+
include(ROS2MedkitSanitizers)
25+
include(ROS2MedkitLinting)
26+
include(ROS2MedkitWarnings)
27+
28+
option(ENABLE_COVERAGE "Enable code coverage reporting" OFF)
29+
if(ENABLE_COVERAGE)
30+
message(STATUS "Code coverage enabled")
31+
add_compile_options(--coverage -O0 -g)
32+
add_link_options(--coverage)
33+
endif()
34+
35+
find_package(ament_cmake REQUIRED)
36+
37+
include(ROS2MedkitCompat)
38+
39+
find_package(rclcpp REQUIRED)
40+
find_package(action_msgs REQUIRED)
41+
find_package(ros2_medkit_msgs REQUIRED)
42+
find_package(ros2_medkit_fault_reporter REQUIRED)
43+
44+
add_library(action_status_bridge_lib SHARED
45+
src/action_status_bridge_node.cpp
46+
)
47+
48+
target_include_directories(action_status_bridge_lib PUBLIC
49+
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
50+
$<INSTALL_INTERFACE:include>
51+
)
52+
53+
medkit_target_dependencies(action_status_bridge_lib
54+
rclcpp
55+
action_msgs
56+
ros2_medkit_msgs
57+
ros2_medkit_fault_reporter
58+
)
59+
60+
add_executable(action_status_bridge_node src/main.cpp)
61+
target_link_libraries(action_status_bridge_node action_status_bridge_lib)
62+
medkit_target_dependencies(action_status_bridge_node rclcpp)
63+
64+
install(TARGETS action_status_bridge_node
65+
DESTINATION lib/${PROJECT_NAME}
66+
)
67+
68+
install(TARGETS action_status_bridge_lib
69+
EXPORT export_${PROJECT_NAME}
70+
ARCHIVE DESTINATION lib
71+
LIBRARY DESTINATION lib
72+
RUNTIME DESTINATION bin
73+
)
74+
75+
install(DIRECTORY include/
76+
DESTINATION include
77+
)
78+
79+
install(DIRECTORY launch config
80+
DESTINATION share/${PROJECT_NAME}
81+
)
82+
83+
ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
84+
ament_export_dependencies(rclcpp action_msgs ros2_medkit_msgs ros2_medkit_fault_reporter)
85+
86+
if(BUILD_TESTING)
87+
find_package(ament_lint_auto REQUIRED)
88+
find_package(ament_cmake_gtest REQUIRED)
89+
90+
set(ament_cmake_clang_format_CONFIG_FILE "${CMAKE_CURRENT_SOURCE_DIR}/../../.clang-format")
91+
list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_uncrustify ament_cmake_cpplint ament_cmake_clang_tidy)
92+
ament_lint_auto_find_test_dependencies()
93+
94+
ros2_medkit_clang_tidy()
95+
96+
include(ROS2MedkitTestDomain)
97+
medkit_init_test_domains(START 90 END 99)
98+
99+
ament_add_gtest(test_action_status_bridge test/test_action_status_bridge.cpp)
100+
target_link_libraries(test_action_status_bridge action_status_bridge_lib)
101+
medkit_target_dependencies(test_action_status_bridge rclcpp action_msgs ros2_medkit_msgs)
102+
medkit_set_test_domain(test_action_status_bridge)
103+
104+
if(ENABLE_COVERAGE)
105+
target_compile_options(test_action_status_bridge PRIVATE --coverage -O0 -g)
106+
target_link_options(test_action_status_bridge PRIVATE --coverage)
107+
endif()
108+
109+
ros2_medkit_relax_vendor_warnings()
110+
endif()
111+
112+
ament_package()

‎src/ros2_medkit_action_status_bridge/README.md‎

Lines changed: 60 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,60 @@
1+
# ros2_medkit_action_status_bridge
2+
3+
Generic drop-in bridge that turns terminal ROS 2 action goal states into
4+
structured medkit faults. It watches the `/<action>/_action/status`
5+
(`action_msgs/msg/GoalStatusArray`) topic that **every** action server
6+
publishes, so it works across Nav2, MoveIt 2, ros2_control's controller
7+
actions, and any custom action with **no per-project code**.
8+
9+
## Why this matters
10+
11+
For the core ROS 2 league the authoritative "the goal failed" verdict lives on
12+
the action-result channel, not on `/diagnostics` or `/rosout`:
13+
14+
- Nav2 `NavigateToPose` -> `GoalStatus = ABORTED`
15+
- MoveIt `MoveGroup` -> `GoalStatus = ABORTED` (with `MoveItErrorCode` in the result)
16+
- ros2_control controller actions -> aborted goals
17+
18+
Neither the diagnostic bridge nor the log bridge sees this. This bridge does,
19+
generically, by observing the goal-status topic.
20+
21+
## What it does
22+
23+
- Discovers every action on the graph by scanning for `*/_action/status`
24+
topics (re-scanned on a timer to catch actions that appear later).
25+
- `ABORTED (6)` -> fault (`SEVERITY_ERROR` by default).
26+
- `CANCELED (5)` -> fault only if `canceled_is_fault` (off by default; cancel is
27+
usually intentional).
28+
- `SUCCEEDED (4)` -> `PASSED` to heal the action's ABORTED code (if enabled).
29+
- `fault_code` is `<PREFIX>_<ACTION>_ABORTED`, e.g.
30+
`ACTION_NAVIGATE_TO_POSE_ABORTED`. `source_id` is the action name.
31+
- Per-goal dedup keyed on `goal_id` so a latched terminal status is reported
32+
once, not on every status publication.
33+
34+
## Scope: the terminal verdict, not the reason
35+
36+
This bridge delivers the generic "it aborted" event. The action-specific
37+
*reason* (e.g. `MoveItErrorCode.val = -26 START_STATE_IN_COLLISION`, Nav2
38+
`error_code` on Iron/Jazzy) lives in the action result message and is a separate
39+
enrichment concern (a future action-result reader using runtime message
40+
introspection / dynmsg). Per-project plugins are only needed for human-readable
41+
labels, not to surface the fault.
42+
43+
## Run it
44+
45+
```bash
46+
ros2 launch ros2_medkit_action_status_bridge action_status_bridge.launch.py
47+
```
48+
49+
## Configuration (`config/action_status_bridge.yaml`)
50+
51+
| Param | Default | Meaning |
52+
|-------|---------|---------|
53+
| `aborted_severity` | `2` (ERROR) | severity of an aborted goal |
54+
| `canceled_is_fault` | `false` | treat CANCELED as a fault |
55+
| `heal_on_succeeded` | `true` | send PASSED on a successful goal |
56+
| `rescan_period_sec` | `2.0` | how often to look for new actions |
57+
| `code_prefix` | `ACTION` | prefix for generated codes |
58+
| `exclude_actions` | `[]` | action-name substrings to skip |
59+
| `include_only_actions` | `[]` | if set, only watch these |
60+
| `dedup_capacity` | `4096` | remembered goal/status keys |

‎src/ros2_medkit_action_status_bridge/config/action_status_bridge.yaml‎

Lines changed: 19 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,19 @@
1+
action_status_bridge:
2+
ros__parameters:
3+
# Severity for an ABORTED goal (0=INFO 1=WARN 2=ERROR 3=CRITICAL).
4+
aborted_severity: 2
5+
# Treat a CANCELED goal as a fault. Off by default (cancel is usually
6+
# an intentional operator/client action, not a failure).
7+
canceled_is_fault: false
8+
# On a SUCCEEDED goal, send PASSED to heal the action's ABORTED fault code.
9+
heal_on_succeeded: true
10+
# How often to rescan the graph for new action status topics.
11+
rescan_period_sec: 2.0
12+
# Prefix for generated fault codes (<PREFIX>_<ACTION>_ABORTED).
13+
code_prefix: "ACTION"
14+
# Action-name substrings to skip.
15+
exclude_actions: []
16+
# If non-empty, ONLY watch actions matching these substrings.
17+
include_only_actions: []
18+
# Max remembered (goal_id:status) keys for dedup.
19+
dedup_capacity: 4096

‎src/ros2_medkit_action_status_bridge/include/ros2_medkit_action_status_bridge/action_status_bridge_node.hpp‎

Lines changed: 101 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,101 @@
1+
// Copyright 2026 mfaferek93, bburda
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#pragma once
16+
17+
#include <cstdint>
18+
#include <deque>
19+
#include <map>
20+
#include <memory>
21+
#include <mutex>
22+
#include <string>
23+
#include <unordered_set>
24+
#include <vector>
25+
26+
#include "rclcpp/rclcpp.hpp"
27+
#include "action_msgs/msg/goal_status_array.hpp"
28+
#include "ros2_medkit_fault_reporter/fault_reporter.hpp"
29+
30+
namespace ros2_medkit_action_status_bridge {
31+
32+
/// Bridge node that turns terminal ROS 2 action goal states into FaultManager
33+
/// faults. Generic across every action: it observes the per-action
34+
/// `/<action>/_action/status` topic (`action_msgs/msg/GoalStatusArray`) that
35+
/// every action server publishes, so no per-project code is needed.
36+
///
37+
/// This catches the authoritative "the goal failed" verdict that neither the
38+
/// /diagnostics bridge nor the /rosout log bridge can see - e.g. a Nav2
39+
/// NavigateToPose aborting or a MoveIt MoveGroup goal aborting. The *reason*
40+
/// (action-specific error code in the result) is a separate enrichment concern;
41+
/// this bridge delivers the generic terminal status.
42+
///
43+
/// Status mapping (action_msgs/msg/GoalStatus):
44+
/// - ABORTED (6) -> fault (severity configurable, default ERROR)
45+
/// - CANCELED (5) -> fault only if canceled_is_fault (usually intentional)
46+
/// - SUCCEEDED (4)-> PASSED (heals the per-action ABORTED code) if enabled
47+
class ActionStatusBridgeNode : public rclcpp::Node {
48+
public:
49+
explicit ActionStatusBridgeNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
50+
51+
/// Derive the action name from a `/<action>/_action/status` topic name.
52+
/// Returns empty when the topic is not an action status topic.
53+
static std::string action_name_from_status_topic(const std::string & topic);
54+
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;
58+
59+
/// Lowercase hex of a 16-byte goal UUID, for dedup keys and short display.
60+
static std::string uuid_to_hex(const std::array<uint8_t, 16> & uuid);
61+
62+
private:
63+
void rescan_actions();
64+
void status_callback(const std::string & action_name,
65+
const action_msgs::msg::GoalStatusArray::ConstSharedPtr & msg);
66+
67+
ros2_medkit_fault_reporter::FaultReporter * reporter_for(const std::string & action_name);
68+
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);
72+
73+
bool action_is_eligible(const std::string & action_name) const;
74+
75+
void load_parameters();
76+
77+
static std::string to_upper_snake(const std::string & in, size_t max_len);
78+
79+
rclcpp::TimerBase::SharedPtr rescan_timer_;
80+
std::map<std::string, rclcpp::Subscription<action_msgs::msg::GoalStatusArray>::SharedPtr> subs_;
81+
82+
std::map<std::string, std::unique_ptr<ros2_medkit_fault_reporter::FaultReporter>> reporters_;
83+
std::mutex reporters_mutex_;
84+
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_;
90+
91+
// Configuration
92+
uint8_t aborted_severity_;
93+
bool canceled_is_fault_;
94+
bool heal_on_succeeded_;
95+
double rescan_period_sec_;
96+
std::string code_prefix_;
97+
std::vector<std::string> exclude_actions_;
98+
std::vector<std::string> include_only_actions_;
99+
};
100+
101+
} // namespace ros2_medkit_action_status_bridge

‎src/ros2_medkit_action_status_bridge/launch/action_status_bridge.launch.py‎

Lines changed: 37 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,37 @@
1+
# Copyright 2026 mfaferek93, bburda
2+
#
3+
# Licensed under the Apache License, Version 2.0 (the "License");
4+
# you may not use this file except in compliance with the License.
5+
# You may obtain a copy of the License at
6+
#
7+
# http://www.apache.org/licenses/LICENSE-2.0
8+
#
9+
# Unless required by applicable law or agreed to in writing, software
10+
# distributed under the License is distributed on an "AS IS" BASIS,
11+
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
# See the License for the specific language governing permissions and
13+
# limitations under the License.
14+
15+
import os
16+
17+
from ament_index_python.packages import get_package_share_directory
18+
from launch import LaunchDescription
19+
from launch_ros.actions import Node
20+
21+
22+
def generate_launch_description():
23+
config_file = os.path.join(
24+
get_package_share_directory('ros2_medkit_action_status_bridge'),
25+
'config',
26+
'action_status_bridge.yaml'
27+
)
28+
29+
return LaunchDescription([
30+
Node(
31+
package='ros2_medkit_action_status_bridge',
32+
executable='action_status_bridge_node',
33+
name='action_status_bridge',
34+
output='screen',
35+
parameters=[config_file],
36+
),
37+
])

‎src/ros2_medkit_action_status_bridge/package.xml‎

Lines changed: 31 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,31 @@
1+
<?xml version="1.0"?>
2+
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
3+
<package format="3">
4+
<name>ros2_medkit_action_status_bridge</name>
5+
<version>0.5.0</version>
6+
<description>Bridge node turning terminal ROS2 action goal states (aborted) into FaultManager faults</description>
7+
8+
<maintainer email="michal.faferek@selfpatch.ai">mfaferek93</maintainer>
9+
<license>Apache-2.0</license>
10+
11+
<buildtool_depend>ament_cmake</buildtool_depend>
12+
<buildtool_depend>ros2_medkit_cmake</buildtool_depend>
13+
14+
<depend>rclcpp</depend>
15+
<depend>action_msgs</depend>
16+
<depend>ros2_medkit_msgs</depend>
17+
<depend>ros2_medkit_fault_reporter</depend>
18+
19+
<test_depend>ament_lint_auto</test_depend>
20+
<test_depend>ament_lint_common</test_depend>
21+
<test_depend>ament_cmake_clang_format</test_depend>
22+
<test_depend>ament_cmake_clang_tidy</test_depend>
23+
<test_depend>ament_cmake_gtest</test_depend>
24+
<test_depend>launch_testing_ament_cmake</test_depend>
25+
<test_depend>launch_testing_ros</test_depend>
26+
<test_depend>ros2_medkit_fault_manager</test_depend>
27+
28+
<export>
29+
<build_type>ament_cmake</build_type>
30+
</export>
31+
</package>

0 commit comments

Comments
 (0)