Skip to content

Commit 14011bb

Browse files
Minipadaclaude
Minipada
and
claude
authored
feat(dc_measurements): intervention Measurement for human takeovers (#373)
* feat(dc_measurements): intervention Measurement for human takeovers "How often did somebody have to walk over and rescue it" decides whether a deployment is trusted, and DC could not answer it. The signal was already there: driving_type emits a closed set of modes, and every transition out of autonomous into a human-driven one is an intervention. So intervention is a projection of that signal, not a second detector -- it reads the same mode source driving_type reads, so the two cannot disagree about when the robot was autonomous. Two Records per takeover: a start when the mode leaves autonomous_mode for one of human_modes (state "open", no duration_s), an end when it returns (state "closed", duration_s). Both carry from_mode, to_mode, the dwell of the mode that was left, and the same started_at so the pair joins downstream. A takeover still under way when collection stops keeps its "open" Record and never gets a closing one, so an unterminated interval cannot be averaged as a zero. seq increments per emitted Record, so a dropped Record is a gap rather than a corrupted duration. Rates are deliberately left to views: the denominator can then change without touching a robot. Nothing is published on a poll with no boundary. Two extractions make it a projection rather than a copy. StateTransitionDetector is the ROS-free half of #360, header-only in dc_common, generic over the state type and owning no clock, so a test drives a whole shift in microseconds; #360's BatteryCycleAccumulator is untouched. DrivingModeSource lifts driving_type's mode-source half -- both shapes, mode topic and velocity inference -- so both Measurements configure identically; driving_type.cpp drops from 128 lines to 43 and its five tests still pass unchanged, which is the check that the lift was behaviour-preserving. Verified with colcon build + colcon test for dc_common and dc_measurements against the workspace image: 212 tests, 0 failures. Co-Authored-By: Claude Opus 5 <noreply@anthropic.com> Claude-Session: https://claude.ai/code/session_01S2g8GoHbS3Eg8hVSmppDXs Signed-off-by: David Bensoussan <d.bensoussan@proton.me> * docs(progress): record the #362 intervention Measurement work Co-Authored-By: Claude Opus 5 <noreply@anthropic.com> Claude-Session: https://claude.ai/code/session_01S2g8GoHbS3Eg8hVSmppDXs Signed-off-by: David Bensoussan <d.bensoussan@proton.me> * style(dc_measurements): clang-format intervention.json Co-Authored-By: Claude Opus 5 <noreply@anthropic.com> Claude-Session: https://claude.ai/code/session_01S2g8GoHbS3Eg8hVSmppDXs Signed-off-by: David Bensoussan <d.bensoussan@proton.me> --------- Signed-off-by: David Bensoussan <d.bensoussan@proton.me> Co-authored-by: Claude Opus 5 <noreply@anthropic.com>
1 parent 293a65b commit 14011bb

12 files changed

Lines changed: 857 additions & 151 deletions

File tree

‎dc_measurements/CMakeLists.txt‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -151,6 +151,9 @@ list(APPEND dc_measurement_plugin_libs dc_distance_traveled_measurement)
151151
add_library(dc_driving_type_measurement SHARED plugins/measurements/driving_type.cpp)
152152
list(APPEND dc_measurement_plugin_libs dc_driving_type_measurement)
153153

154+
add_library(dc_intervention_measurement SHARED plugins/measurements/intervention.cpp)
155+
list(APPEND dc_measurement_plugin_libs dc_intervention_measurement)
156+
154157
add_library(dc_ip_camera_measurement SHARED plugins/measurements/ip_camera.cpp)
155158
list(APPEND dc_measurement_plugin_libs dc_ip_camera_measurement)
156159

@@ -322,6 +325,7 @@ set(tests
322325
test_measurement_integer_equal
323326
test_measurement_integer_inferior
324327
test_measurement_integer_superior
328+
test_measurement_intervention
325329
test_measurement_ip_camera
326330
test_measurement_list_bool_equal
327331
test_measurement_list_double_equal

‎dc_measurements/include/dc_measurements/driving_mode_source.hpp‎

Lines changed: 204 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,204 @@
1+
// SPDX-FileCopyrightText: 2022-2026 David Bensoussan
2+
// SPDX-License-Identifier: MPL-2.0
3+
4+
#ifndef DC_MEASUREMENTS__DRIVING_MODE_SOURCE_HPP_
5+
#define DC_MEASUREMENTS__DRIVING_MODE_SOURCE_HPP_
6+
7+
#include <cstdint>
8+
#include <map>
9+
#include <string>
10+
#include <vector>
11+
12+
#include "dc_util/node_utils.hpp"
13+
#include "geometry_msgs/msg/twist.hpp"
14+
#include "rclcpp_lifecycle/lifecycle_node.hpp"
15+
#include "std_msgs/msg/string.hpp"
16+
17+
namespace dc_measurements
18+
{
19+
20+
/**
21+
* @brief The documented, closed set of driving modes DC ever reports. Downstream
22+
* grouping/dashboards key off these exact values, so a mapping targeting anything else is a
23+
* configuration error rather than a silently widened set.
24+
*/
25+
inline bool isValidDrivingMode(const std::string& mode)
26+
{
27+
return mode == "autonomous" || mode == "manual" || mode == "teleop" || mode == "unknown";
28+
}
29+
30+
/**
31+
* @class dc_measurements::DrivingModeSource
32+
* @brief The one way DC turns robot topics into a driving mode, shared by every Measurement that
33+
* needs one.
34+
*
35+
* `driving_type` reports the mode and `intervention` (#362) projects its transitions, so they must
36+
* never disagree about when the robot was autonomous: both drive this, rather than each
37+
* subscribing on its own. Supports the two shapes documented for `driving_type` -- a dedicated
38+
* mode topic mapped through `value_mapping_from`/`value_mapping_to`, or inference from whichever
39+
* of `velocity_topics` last published -- chosen by which parameters are set; configuring both is a
40+
* configuration error.
41+
*/
42+
class DrivingModeSource
43+
{
44+
public:
45+
/**
46+
* @brief Read the mode-source parameters of `measurement_name` and subscribe accordingly.
47+
* @throws std::runtime_error when both shapes are configured, or when the velocity lists
48+
* disagree in size.
49+
*/
50+
void configure(const rclcpp_lifecycle::LifecycleNode::SharedPtr& node, const std::string& measurement_name,
51+
const rclcpp::Logger& logger)
52+
{
53+
clock_ = node->get_clock();
54+
mode_topic_ = dc_util::get_str_type_param(node, measurement_name, "mode_topic", "");
55+
value_mapping_from_ =
56+
dc_util::get_str_array_type_param(node, measurement_name, "value_mapping_from", std::vector<std::string>{});
57+
value_mapping_to_ =
58+
dc_util::get_str_array_type_param(node, measurement_name, "value_mapping_to", std::vector<std::string>{});
59+
velocity_topics_ =
60+
dc_util::get_str_array_type_param(node, measurement_name, "velocity_topics", std::vector<std::string>{});
61+
velocity_modes_ =
62+
dc_util::get_str_array_type_param(node, measurement_name, "velocity_modes", std::vector<std::string>{});
63+
velocity_timeout_s_ = dc_util::get_double_type_param(node, measurement_name, "velocity_timeout_s", 1.0);
64+
65+
const bool has_mode_topic = !mode_topic_.empty();
66+
const bool has_velocity_sources = !velocity_topics_.empty();
67+
68+
if (has_mode_topic && has_velocity_sources)
69+
{
70+
throw std::runtime_error{ measurement_name + ": configure either 'mode_topic' or 'velocity_topics', not both" };
71+
}
72+
73+
if (has_mode_topic)
74+
{
75+
configureModeTopic(node, measurement_name, logger);
76+
}
77+
else if (has_velocity_sources)
78+
{
79+
configureVelocitySources(node, measurement_name, logger);
80+
}
81+
}
82+
83+
/**
84+
* @brief The mode as of now.
85+
*
86+
* "unknown" until a mode has been observed, and again once every velocity source has gone
87+
* quiet for longer than `velocity_timeout_s` -- a stale source must not keep reporting a mode
88+
* the robot left.
89+
*/
90+
std::string mode()
91+
{
92+
if (!velocity_subscriptions_.empty())
93+
{
94+
bool stale = true;
95+
if (velocity_observed_)
96+
{
97+
const int64_t now_ns = clock_->now().nanoseconds();
98+
stale = static_cast<double>(now_ns - last_velocity_time_ns_) / 1e9 > velocity_timeout_s_;
99+
}
100+
if (stale)
101+
{
102+
current_mode_ = "unknown";
103+
}
104+
}
105+
return current_mode_;
106+
}
107+
108+
private:
109+
void configureModeTopic(const rclcpp_lifecycle::LifecycleNode::SharedPtr& node, const std::string& measurement_name,
110+
const rclcpp::Logger& logger)
111+
{
112+
if (value_mapping_from_.size() != value_mapping_to_.size())
113+
{
114+
RCLCPP_ERROR_STREAM(logger, measurement_name
115+
<< ": 'value_mapping_from' and 'value_mapping_to' must be the same size, "
116+
"ignoring the mapping entirely");
117+
}
118+
else
119+
{
120+
for (size_t i = 0; i < value_mapping_from_.size(); ++i)
121+
{
122+
if (!isValidDrivingMode(value_mapping_to_[i]))
123+
{
124+
RCLCPP_ERROR_STREAM(logger, measurement_name
125+
<< ": '" << value_mapping_to_[i]
126+
<< "' is not one of the supported modes (autonomous, manual, teleop, "
127+
"unknown), ignoring mapping for raw value '"
128+
<< value_mapping_from_[i] << "'");
129+
continue;
130+
}
131+
value_mapping_[value_mapping_from_[i]] = value_mapping_to_[i];
132+
}
133+
}
134+
135+
mode_subscription_ = node->create_subscription<std_msgs::msg::String>(
136+
mode_topic_, rclcpp::SystemDefaultsQoS(), [this, logger, measurement_name](const std_msgs::msg::String& msg) {
137+
const auto it = value_mapping_.find(msg.data);
138+
if (it == value_mapping_.end())
139+
{
140+
RCLCPP_DEBUG_STREAM(logger, measurement_name << ": no mapping for raw mode value '" << msg.data
141+
<< "', keeping current mode '" << current_mode_ << "'");
142+
return;
143+
}
144+
current_mode_ = it->second;
145+
});
146+
}
147+
148+
void configureVelocitySources(const rclcpp_lifecycle::LifecycleNode::SharedPtr& node,
149+
const std::string& measurement_name, const rclcpp::Logger& logger)
150+
{
151+
if (velocity_topics_.size() != velocity_modes_.size())
152+
{
153+
throw std::runtime_error{ measurement_name + ": 'velocity_topics' and 'velocity_modes' must be the same size" };
154+
}
155+
156+
for (size_t i = 0; i < velocity_topics_.size(); ++i)
157+
{
158+
if (!isValidDrivingMode(velocity_modes_[i]))
159+
{
160+
RCLCPP_ERROR_STREAM(logger, measurement_name
161+
<< ": '" << velocity_modes_[i]
162+
<< "' is not one of the supported modes (autonomous, manual, teleop, "
163+
"unknown), ignoring velocity source '"
164+
<< velocity_topics_[i] << "'");
165+
continue;
166+
}
167+
168+
const std::string mode = velocity_modes_[i];
169+
velocity_subscriptions_.push_back(node->create_subscription<geometry_msgs::msg::Twist>(
170+
velocity_topics_[i], rclcpp::SystemDefaultsQoS(), [this, mode](const geometry_msgs::msg::Twist&) {
171+
current_mode_ = mode;
172+
velocity_observed_ = true;
173+
last_velocity_time_ns_ = clock_->now().nanoseconds();
174+
}));
175+
}
176+
}
177+
178+
// The clock, not the node: the node owns the Measurement that owns this, so holding the node
179+
// back would be a reference cycle.
180+
rclcpp::Clock::SharedPtr clock_;
181+
182+
// Shape 1: a dedicated topic carrying a raw mode value, mapped through value_mapping_.
183+
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr mode_subscription_;
184+
std::string mode_topic_;
185+
std::vector<std::string> value_mapping_from_;
186+
std::vector<std::string> value_mapping_to_;
187+
std::map<std::string, std::string> value_mapping_;
188+
189+
// Shape 2: infer the mode from whichever velocity source last published.
190+
std::vector<rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr> velocity_subscriptions_;
191+
std::vector<std::string> velocity_topics_;
192+
std::vector<std::string> velocity_modes_;
193+
double velocity_timeout_s_{ 1.0 };
194+
bool velocity_observed_{ false };
195+
int64_t last_velocity_time_ns_{ 0 };
196+
197+
// No mode has been observed yet: report "unknown" rather than nothing, so a consumer can tell
198+
// "not yet known" apart from "no data collected at all".
199+
std::string current_mode_{ "unknown" };
200+
};
201+
202+
} // namespace dc_measurements
203+
204+
#endif // DC_MEASUREMENTS__DRIVING_MODE_SOURCE_HPP_

‎dc_measurements/include/dc_measurements/plugins/measurements/driving_type.hpp‎

Lines changed: 2 additions & 25 deletions
Original file line numberDiff line numberDiff line change
@@ -4,16 +4,11 @@
44
#ifndef DC_MEASUREMENTS__PLUGINS__MEASUREMENTS__DRIVING_TYPE_HPP_
55
#define DC_MEASUREMENTS__PLUGINS__MEASUREMENTS__DRIVING_TYPE_HPP_
66

7-
#include <cstdint>
8-
#include <map>
97
#include <string>
10-
#include <vector>
118

129
#include "dc_core/measurement.hpp"
10+
#include "dc_measurements/driving_mode_source.hpp"
1311
#include "dc_measurements/measurement.hpp"
14-
#include "dc_util/node_utils.hpp"
15-
#include "geometry_msgs/msg/twist.hpp"
16-
#include "std_msgs/msg/string.hpp"
1712

1813
namespace dc_measurements
1914
{
@@ -26,25 +21,7 @@ class DrivingType : public dc_measurements::Measurement
2621
dc_interfaces::msg::StringStamped collect() override;
2722

2823
private:
29-
void modeTopicCb(const std_msgs::msg::String& msg);
30-
void velocitySourceCb(const std::string& mode);
31-
32-
// Shape 1: a dedicated topic carrying a raw mode value, mapped through value_mapping_.
33-
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr mode_subscription_;
34-
std::string mode_topic_;
35-
std::vector<std::string> value_mapping_from_;
36-
std::vector<std::string> value_mapping_to_;
37-
std::map<std::string, std::string> value_mapping_;
38-
39-
// Shape 2: infer the mode from whichever velocity source last published.
40-
std::vector<rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr> velocity_subscriptions_;
41-
std::vector<std::string> velocity_topics_;
42-
std::vector<std::string> velocity_modes_;
43-
double velocity_timeout_s_;
44-
bool velocity_observed_{ false };
45-
int64_t last_velocity_time_ns_{ 0 };
46-
47-
std::string current_mode_;
24+
DrivingModeSource mode_source_;
4825

4926
protected:
5027
/**

‎dc_measurements/include/dc_measurements/plugins/measurements/intervention.hpp‎

Lines changed: 44 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,44 @@
1+
// SPDX-FileCopyrightText: 2022-2026 David Bensoussan
2+
// SPDX-License-Identifier: MPL-2.0
3+
4+
#ifndef DC_MEASUREMENTS__PLUGINS__MEASUREMENTS__INTERVENTION_HPP_
5+
#define DC_MEASUREMENTS__PLUGINS__MEASUREMENTS__INTERVENTION_HPP_
6+
7+
#include <string>
8+
9+
#include "dc_common/state_transition_detector.hpp"
10+
#include "dc_core/measurement.hpp"
11+
#include "dc_measurements/driving_mode_source.hpp"
12+
#include "dc_measurements/measurement.hpp"
13+
14+
namespace dc_measurements
15+
{
16+
17+
/**
18+
* @class dc_measurements::Intervention
19+
* @brief Human takeovers, as a projection of driving-mode transitions rather than a second
20+
* detector: the same DrivingModeSource `driving_type` reads, fed to a StateTransitionDetector, so
21+
* the two can never disagree about when the robot was autonomous (#362). A Record is produced
22+
* only for the two boundaries `dc_kpi_intervention_events` (#369) already know how to read: a
23+
* transition from `autonomous` into `manual`/`teleop` starts a takeover, and the reverse ends one.
24+
*/
25+
class Intervention : public dc_measurements::Measurement
26+
{
27+
public:
28+
Intervention();
29+
~Intervention() override;
30+
dc_interfaces::msg::StringStamped collect() override;
31+
32+
private:
33+
DrivingModeSource mode_source_;
34+
dc_common::StateTransitionDetector<std::string> detector_;
35+
36+
protected:
37+
void onConfigure() override;
38+
void onCleanup() override;
39+
void setValidationSchema() override;
40+
};
41+
42+
} // namespace dc_measurements
43+
44+
#endif // DC_MEASUREMENTS__PLUGINS__MEASUREMENTS__INTERVENTION_HPP_

‎dc_measurements/measurement_plugin.xml‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -68,6 +68,14 @@ SPDX-License-Identifier: MPL-2.0
6868
</class>
6969
</library>
7070

71+
<library path="dc_intervention_measurement">
72+
<class name="dc_measurements/Intervention" type="dc_measurements::Intervention" base_class_type="dc_core::Measurement">
73+
<description>
74+
dc_measurement_intervention
75+
</description>
76+
</class>
77+
</library>
78+
7179
<library path="dc_ip_camera_measurement">
7280
<class name="dc_measurements/IpCamera" type="dc_measurements::IpCamera" base_class_type="dc_core::Measurement">
7381
<description>

0 commit comments

Comments
 (0)