diff --git a/dc_measurements/include/dc_measurements/measurement.hpp b/dc_measurements/include/dc_measurements/measurement.hpp index 59f8d2348..51f5fcac6 100644 --- a/dc_measurements/include/dc_measurements/measurement.hpp +++ b/dc_measurements/include/dc_measurements/measurement.hpp @@ -50,6 +50,30 @@ enum class Status : int8_t RUNNING = 3, }; +// A json_validator schema_loader resolving a "$ref" naming a sibling file (e.g. +// "mission_base.json") against `schema_dir` -- the directory the root schema itself was loaded +// from. Every schema in this codebase but the Mission Measurement family keeps to same-file +// "#/$defs/..." refs, which nlohmann_json_schema_validator resolves on its own; this loader is +// only ever invoked for a schema that references another file, letting a family of adapters +// (#305/ADR-0010's shared mission_start/mission_end contract) share one base schema instead of +// duplicating its properties per adapter. +inline nlohmann::json_schema::schema_loader makeSchemaFileLoader(const std::string& schema_dir) +{ + return [schema_dir](const nlohmann::json_uri& id, json& value) { + std::string filename = id.path(); + if (!filename.empty() && filename.front() == '/') + { + filename.erase(0, 1); + } + std::ifstream f(schema_dir + "/" + filename); + if (!f) + { + throw std::runtime_error{ "could not load schema '" + filename + "' referenced from " + schema_dir }; + } + value = json::parse(f); + }; +} + /** * @class nav2_behaviors::Behavior * @brief An action server Behavior base class implementing the action server and basic factory. @@ -144,7 +168,8 @@ class Measurement : public dc_core::Measurement void validateSchema(const std::string& package_name, const std::string& json_filename) { std::string package_share_directory = ament_index_cpp::get_package_share_directory(package_name); - std::string path = package_share_directory + "/plugins/measurements/json/" + json_filename; + std::string schema_dir = package_share_directory + "/plugins/measurements/json"; + std::string path = schema_dir + "/" + json_filename; std::ifstream f(path.c_str()); try { @@ -155,6 +180,7 @@ class Measurement : public dc_core::Measurement RCLCPP_INFO_STREAM(logger_, "schema: " << schema_); try { + validator_ = json_validator(makeSchemaFileLoader(schema_dir)); validator_.set_root_schema(schema_); } catch (const std::exception& e) @@ -180,6 +206,8 @@ class Measurement : public dc_core::Measurement RCLCPP_INFO_STREAM(logger_, "schema: " << schema_); try { + const std::string schema_dir = std::filesystem::path(json_schema_path).parent_path().string(); + validator_ = json_validator(makeSchemaFileLoader(schema_dir)); validator_.set_root_schema(schema_); } catch (const std::exception& e) diff --git a/dc_measurements/include/dc_measurements/mission_outcome.hpp b/dc_measurements/include/dc_measurements/mission_outcome.hpp new file mode 100644 index 000000000..768651b60 --- /dev/null +++ b/dc_measurements/include/dc_measurements/mission_outcome.hpp @@ -0,0 +1,53 @@ +// SPDX-FileCopyrightText: 2022-2026 David Bensoussan +// SPDX-License-Identifier: MPL-2.0 + +#ifndef DC_MEASUREMENTS__MISSION_OUTCOME_HPP_ +#define DC_MEASUREMENTS__MISSION_OUTCOME_HPP_ + +#include +#include + +namespace dc_measurements +{ + +/// The outcome a Mission Record reports on mission_end, per the mission lifecycle contract +/// (#305, recorded in ADR-0010). Shared by every Mission Measurement adapter (nav2's three +/// action variants, Open-RMF) so the vocabulary -- and what a caller can do with it -- stays one +/// type rather than four independently-declared look-alikes. +enum class MissionOutcome +{ + Succeeded, + Failed, + Cancelled, + Aborted, +}; + +/// The JSON-Record spelling of a MissionOutcome, per #305/ADR-0010's Record shape. +inline std::string missionOutcomeName(MissionOutcome outcome) +{ + switch (outcome) + { + case MissionOutcome::Succeeded: + return "succeeded"; + case MissionOutcome::Failed: + return "failed"; + case MissionOutcome::Cancelled: + return "cancelled"; + case MissionOutcome::Aborted: + return "aborted"; + } + return "unknown"; +} + +/// A mission_start fact: identical across every adapter (nav2's three action variants, +/// Open-RMF) -- only what varies per source (the terminal outcome, its reason, extra fields +/// like recoveries/missed_waypoints) lives in each adapter's own MissionEndFact. +struct MissionStartFact +{ + std::string mission_id; + std::uint64_t sequence{ 0 }; +}; + +} // namespace dc_measurements + +#endif // DC_MEASUREMENTS__MISSION_OUTCOME_HPP_ diff --git a/dc_measurements/include/dc_measurements/mission_record_json.hpp b/dc_measurements/include/dc_measurements/mission_record_json.hpp new file mode 100644 index 000000000..67f7eaf75 --- /dev/null +++ b/dc_measurements/include/dc_measurements/mission_record_json.hpp @@ -0,0 +1,58 @@ +// SPDX-FileCopyrightText: 2022-2026 David Bensoussan +// SPDX-License-Identifier: MPL-2.0 + +#ifndef DC_MEASUREMENTS__MISSION_RECORD_JSON_HPP_ +#define DC_MEASUREMENTS__MISSION_RECORD_JSON_HPP_ + +#include +#include +#include +#include + +#include "dc_measurements/mission_outcome.hpp" + +namespace dc_measurements +{ + +/// The mission_start Record every Mission Measurement adapter emits, per #305/ADR-0010's shared +/// Record contract. +inline nlohmann::json missionStartJson(const std::string& mission_id, const std::string& mission_type, + std::uint64_t sequence) +{ + nlohmann::json data; + data["event"] = "mission_start"; + data["mission_id"] = mission_id; + data["mission_type"] = mission_type; + data["sequence"] = sequence; + return data; +} + +/// The mission_end Record fields common to every adapter (#305/ADR-0010). A caller with its own +/// extra fields (nav2's `recoveries`, FollowWaypoints' `missed_waypoints`) adds them to the +/// returned object before enqueuing it. +inline nlohmann::json missionEndJsonBase(const std::string& mission_id, const std::string& mission_type, + std::uint64_t sequence, MissionOutcome outcome, double duration_sec, + const std::optional& reason, + const std::optional& error_code) +{ + nlohmann::json data; + data["event"] = "mission_end"; + data["mission_id"] = mission_id; + data["mission_type"] = mission_type; + data["sequence"] = sequence; + data["outcome"] = missionOutcomeName(outcome); + data["duration_sec"] = duration_sec; + if (reason.has_value()) + { + data["reason"] = *reason; + } + if (error_code.has_value()) + { + data["error_code"] = *error_code; + } + return data; +} + +} // namespace dc_measurements + +#endif // DC_MEASUREMENTS__MISSION_RECORD_JSON_HPP_ diff --git a/dc_measurements/include/dc_measurements/mission_registry.hpp b/dc_measurements/include/dc_measurements/mission_registry.hpp new file mode 100644 index 000000000..d6e3bb76f --- /dev/null +++ b/dc_measurements/include/dc_measurements/mission_registry.hpp @@ -0,0 +1,103 @@ +// SPDX-FileCopyrightText: 2022-2026 David Bensoussan +// SPDX-License-Identifier: MPL-2.0 + +#ifndef DC_MEASUREMENTS__MISSION_REGISTRY_HPP_ +#define DC_MEASUREMENTS__MISSION_REGISTRY_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +namespace dc_measurements +{ + +/// Bookkeeping shared by every Mission Measurement core that can track more than one mission at +/// once (MissionNav2ThroughPosesCore, MissionOpenRmfCore): a bounded id -> ActivePayload map, +/// oldest-finished-first eviction once `max_tracked` ids have ever been seen, and the monotonic +/// sequence counter every mission_start/mission_end fact carries. `ActivePayload` is whatever +/// per-mission state one core needs beyond "when did it start and is it finished" -- it must +/// expose a `bool finished` member; PrunedMissionMap never interprets any other field. +/// +/// Owns no clock and no ROS/json type, matching every other tracker/core in this codebase: a +/// unit test drives it in microseconds with no node or action server involved. +template +class PrunedMissionMap +{ +public: + explicit PrunedMissionMap(std::size_t max_tracked = 256) : max_tracked_(max_tracked) + { + } + + ActivePayload* find(const std::string& id) + { + auto it = missions_.find(id); + return it == missions_.end() ? nullptr : &it->second; + } + + /// Starts tracking `id`. Caller must already have checked find(id) == nullptr -- mirrors every + /// existing core's own observe()/end() call pattern, so this never silently overwrites an + /// in-flight mission's state. + ActivePayload& insert(const std::string& id, ActivePayload payload) + { + auto [it, inserted] = missions_.emplace(id, std::move(payload)); + order_.push_back(id); + prune(); + return it->second; + } + + std::uint64_t nextSequence() + { + return ++sequence_; + } + + /// ids whose payload is not yet finished -- for onCleanup()'s "still running at shutdown" + /// warning, the same shape every Mission core already reports it in. + std::vector openIds() const + { + std::vector open; + for (const auto& [id, payload] : missions_) + { + if (!payload.finished) + { + open.push_back(id); + } + } + return open; + } + +private: + // ids never repeat in practice (UUIDs / Open-RMF booking ids), so a long-running instance's map + // would otherwise grow without bound; only ever prunes finished missions, oldest first. + void prune() + { + while (order_.size() > max_tracked_) + { + const auto& oldest = order_.front(); + auto it = missions_.find(oldest); + if (it != missions_.end() && it->second.finished) + { + missions_.erase(it); + order_.pop_front(); + } + else + { + // The oldest tracked id is still open (a very long-running mission) -- leave it rather + // than dropping an active mission's tracking state. + break; + } + } + } + + std::size_t max_tracked_; + std::map missions_; + std::deque order_; + std::uint64_t sequence_{ 0 }; +}; + +} // namespace dc_measurements + +#endif // DC_MEASUREMENTS__MISSION_REGISTRY_HPP_ diff --git a/dc_measurements/include/dc_measurements/mission_uuid.hpp b/dc_measurements/include/dc_measurements/mission_uuid.hpp new file mode 100644 index 000000000..346446fc0 --- /dev/null +++ b/dc_measurements/include/dc_measurements/mission_uuid.hpp @@ -0,0 +1,53 @@ +// SPDX-FileCopyrightText: 2022-2026 David Bensoussan +// SPDX-License-Identifier: MPL-2.0 + +#ifndef DC_MEASUREMENTS__MISSION_UUID_HPP_ +#define DC_MEASUREMENTS__MISSION_UUID_HPP_ + +#include +#include +#include +#include +#include + +#include "unique_identifier_msgs/msg/uuid.hpp" + +namespace dc_measurements +{ + +/// A goal UUID as RFC-4122-style dashed lowercase hex (`xxxxxxxx-xxxx-xxxx-xxxx-xxxxxxxxxxxx`). +/// Used as `mission_id` by the single-goal-at-a-time nav2 adapters (NavigateToPose, +/// FollowWaypoints), which format directly from the action's own UUID message. +inline std::string missionGoalIdDashed(const unique_identifier_msgs::msg::UUID& uuid) +{ + std::ostringstream oss; + oss << std::hex << std::setfill('0'); + for (std::size_t i = 0; i < uuid.uuid.size(); ++i) + { + oss << std::setw(2) << static_cast(uuid.uuid[i]); + if (i == 3 || i == 5 || i == 7 || i == 9) + { + oss << '-'; + } + } + return oss.str(); +} + +/// A goal UUID as plain (non-dashed) lowercase hex. Used as `mission_id` by +/// MissionNav2ThroughPoses, which formats from `goal_info.goal_id.uuid` directly rather than the +/// full UUID message type. Kept distinct from missionGoalIdDashed() rather than unified: an +/// already-shipped adapter's mission_id format is a Record-shape detail downstream consumers may +/// already depend on, not an implementation detail free to change underneath them. +inline std::string missionGoalIdHex(const std::array& uuid) +{ + std::ostringstream oss; + for (auto byte : uuid) + { + oss << std::hex << std::setfill('0') << std::setw(2) << static_cast(byte); + } + return oss.str(); +} + +} // namespace dc_measurements + +#endif // DC_MEASUREMENTS__MISSION_UUID_HPP_ diff --git a/dc_measurements/include/dc_measurements/pending_record_queue.hpp b/dc_measurements/include/dc_measurements/pending_record_queue.hpp new file mode 100644 index 000000000..26f5adf77 --- /dev/null +++ b/dc_measurements/include/dc_measurements/pending_record_queue.hpp @@ -0,0 +1,64 @@ +// SPDX-FileCopyrightText: 2022-2026 David Bensoussan +// SPDX-License-Identifier: MPL-2.0 + +#ifndef DC_MEASUREMENTS__PENDING_RECORD_QUEUE_HPP_ +#define DC_MEASUREMENTS__PENDING_RECORD_QUEUE_HPP_ + +#include +#include +#include +#include + +#include "rclcpp/time.hpp" + +namespace dc_measurements +{ + +/// A bounded FIFO of collected-but-not-yet-published Records, drained one per poll via pop() -- +/// the shape every Measurement that can emit more than one Record between polls (Fault, +/// Ros2ControlStatus, the Mission adapters) already needed, previously reimplemented per plugin. +/// One Record leaves per poll, so a source producing Records far faster than the polling interval +/// would otherwise queue without bound: past `max_size`, push() drops the oldest entry -- the +/// most recent boundaries are the ones still worth reporting -- and reports that back to the +/// caller, which owns the actual logger/measurement-name-specific warning text. +class PendingRecordQueue +{ +public: + explicit PendingRecordQueue(std::size_t max_size = 64) : max_size_(max_size) + { + } + + /// Queues `data` for emission at `stamp`. Returns true when this push dropped the oldest queued + /// entry to stay within `max_size`, so the caller can log its own throttled warning. + bool push(nlohmann::json data, const rclcpp::Time& stamp) + { + records_.emplace_back(std::move(data), stamp); + if (records_.size() > max_size_) + { + records_.pop_front(); + return true; + } + return false; + } + + bool empty() const + { + return records_.empty(); + } + + /// Pops the oldest queued entry. Caller must check empty() first. + std::pair pop() + { + auto record = std::move(records_.front()); + records_.pop_front(); + return record; + } + +private: + std::size_t max_size_; + std::deque> records_; +}; + +} // namespace dc_measurements + +#endif // DC_MEASUREMENTS__PENDING_RECORD_QUEUE_HPP_ diff --git a/dc_measurements/include/dc_measurements/plugins/measurements/fault.hpp b/dc_measurements/include/dc_measurements/plugins/measurements/fault.hpp index b0277d1b6..063d2d771 100644 --- a/dc_measurements/include/dc_measurements/plugins/measurements/fault.hpp +++ b/dc_measurements/include/dc_measurements/plugins/measurements/fault.hpp @@ -6,16 +6,15 @@ #include #include -#include #include #include #include -#include #include #include "dc_common/state_transition_detector.hpp" #include "dc_core/measurement.hpp" #include "dc_measurements/measurement.hpp" +#include "dc_measurements/pending_record_queue.hpp" #include "dc_util/node_utils.hpp" #include "diagnostic_msgs/msg/diagnostic_array.hpp" #include "diagnostic_msgs/msg/diagnostic_status.hpp" @@ -59,7 +58,7 @@ class Fault : public dc_measurements::Measurement std::map fault_started_at_; // Records wait here for a poll to carry them out, one per poll, so they travel the same publish // path (Conditions, buffering, Group) as every other Record. - std::deque> pending_records_; + PendingRecordQueue pending_records_; // Per-Record, not per-component: a global counter across every watched component, so a // consumer can tell a dropped Record apart from a component that never changed. diff --git a/dc_measurements/include/dc_measurements/plugins/measurements/mission_follow_waypoints_tracker.hpp b/dc_measurements/include/dc_measurements/plugins/measurements/mission_follow_waypoints_tracker.hpp index 111bad50f..2bc91408b 100644 --- a/dc_measurements/include/dc_measurements/plugins/measurements/mission_follow_waypoints_tracker.hpp +++ b/dc_measurements/include/dc_measurements/plugins/measurements/mission_follow_waypoints_tracker.hpp @@ -12,6 +12,7 @@ #include #include "dc_common/state_transition_detector.hpp" +#include "dc_measurements/mission_outcome.hpp" namespace dc_measurements { @@ -25,18 +26,6 @@ enum class MissionTerminalStatus Aborted, }; -/// The outcome a Record actually reports, per #387's schema. Distinct from MissionTerminalStatus: -/// a goal that reaches Succeeded with a non-zero error_code is reported as Failed rather than -/// Succeeded (nav2's WaypointFollower can finish its action goal successfully while still having -/// missed one or more waypoints). -enum class MissionOutcome -{ - Succeeded, - Failed, - Cancelled, - Aborted, -}; - /// One entry of FollowWaypoints::Result.missed_waypoints (nav2_msgs/msg/MissedWaypoint on Jazzy), /// stripped of the goal pose -- not part of #389's schema. Jazzy's MissedWaypoint is just /// index/goal/error_code -- no per-waypoint status enum or error_msg the way an older/rolling nav2 @@ -48,12 +37,6 @@ struct WaypointOutcome std::uint16_t error_code{ 0 }; }; -struct MissionStartFact -{ - std::string mission_id; - std::uint64_t sequence{ 0 }; -}; - struct MissionEndFact { std::string mission_id; diff --git a/dc_measurements/include/dc_measurements/plugins/measurements/mission_nav2.hpp b/dc_measurements/include/dc_measurements/plugins/measurements/mission_nav2.hpp index 9145a6d75..8cc75a223 100644 --- a/dc_measurements/include/dc_measurements/plugins/measurements/mission_nav2.hpp +++ b/dc_measurements/include/dc_measurements/plugins/measurements/mission_nav2.hpp @@ -4,17 +4,17 @@ #ifndef DC_MEASUREMENTS__PLUGINS__MEASUREMENTS__MISSION_NAV2_HPP_ #define DC_MEASUREMENTS__PLUGINS__MEASUREMENTS__MISSION_NAV2_HPP_ -#include #include #include #include #include -#include #include "action_msgs/msg/goal_status.hpp" #include "action_msgs/msg/goal_status_array.hpp" #include "dc_core/measurement.hpp" #include "dc_measurements/measurement.hpp" +#include "dc_measurements/mission_record_json.hpp" +#include "dc_measurements/pending_record_queue.hpp" #include "dc_measurements/plugins/measurements/mission_nav2_tracker.hpp" #include "dc_util/node_utils.hpp" #include "nav2_msgs/action/navigate_to_pose.hpp" @@ -58,8 +58,6 @@ class MissionNav2 : public dc_measurements::Measurement const GetResultService::Response::SharedPtr& response); // Caller holds mutex_. void enqueue(json data, const rclcpp::Time& stamp); - static json missionStartJson(const MissionStartFact& fact); - static json missionEndJson(const MissionEndFact& fact); std::string action_name_; rclcpp::Subscription::SharedPtr status_sub_; @@ -79,7 +77,7 @@ class MissionNav2 : public dc_measurements::Measurement std::optional last_recoveries_; // Records wait here for a poll to carry them out, one per poll, so they travel the same publish // path (Conditions, buffering, Group) as every other Record. - std::deque> pending_records_; + PendingRecordQueue pending_records_; protected: void onConfigure() override; diff --git a/dc_measurements/include/dc_measurements/plugins/measurements/mission_nav2_follow_waypoints.hpp b/dc_measurements/include/dc_measurements/plugins/measurements/mission_nav2_follow_waypoints.hpp index b5b1f5290..15c30a500 100644 --- a/dc_measurements/include/dc_measurements/plugins/measurements/mission_nav2_follow_waypoints.hpp +++ b/dc_measurements/include/dc_measurements/plugins/measurements/mission_nav2_follow_waypoints.hpp @@ -4,18 +4,18 @@ #ifndef DC_MEASUREMENTS__PLUGINS__MEASUREMENTS__MISSION_NAV2_FOLLOW_WAYPOINTS_HPP_ #define DC_MEASUREMENTS__PLUGINS__MEASUREMENTS__MISSION_NAV2_FOLLOW_WAYPOINTS_HPP_ -#include #include #include #include #include -#include #include #include "action_msgs/msg/goal_status.hpp" #include "action_msgs/msg/goal_status_array.hpp" #include "dc_core/measurement.hpp" #include "dc_measurements/measurement.hpp" +#include "dc_measurements/mission_record_json.hpp" +#include "dc_measurements/pending_record_queue.hpp" #include "dc_measurements/plugins/measurements/mission_follow_waypoints_tracker.hpp" #include "dc_util/node_utils.hpp" #include "nav2_msgs/action/follow_waypoints.hpp" @@ -63,8 +63,6 @@ class MissionNav2FollowWaypoints : public dc_measurements::Measurement const GetResultService::Response::SharedPtr& response); // Caller holds mutex_. void enqueue(json data, const rclcpp::Time& stamp); - static json missionStartJson(const MissionStartFact& fact); - static json missionEndJson(const MissionEndFact& fact); std::string action_name_; rclcpp::Subscription::SharedPtr status_sub_; @@ -79,7 +77,7 @@ class MissionNav2FollowWaypoints : public dc_measurements::Measurement std::set result_requested_; // Records wait here for a poll to carry them out, one per poll, so they travel the same publish // path (Conditions, buffering, Group) as every other Record. - std::deque> pending_records_; + PendingRecordQueue pending_records_; protected: void onConfigure() override; diff --git a/dc_measurements/include/dc_measurements/plugins/measurements/mission_nav2_through_poses.hpp b/dc_measurements/include/dc_measurements/plugins/measurements/mission_nav2_through_poses.hpp index 02533a53c..39fa1bc19 100644 --- a/dc_measurements/include/dc_measurements/plugins/measurements/mission_nav2_through_poses.hpp +++ b/dc_measurements/include/dc_measurements/plugins/measurements/mission_nav2_through_poses.hpp @@ -6,16 +6,16 @@ #include #include -#include #include #include #include #include -#include #include "action_msgs/msg/goal_status_array.hpp" #include "dc_core/measurement.hpp" #include "dc_measurements/measurement.hpp" +#include "dc_measurements/mission_record_json.hpp" +#include "dc_measurements/pending_record_queue.hpp" #include "dc_measurements/plugins/measurements/mission_nav2_through_poses_core.hpp" #include "dc_util/node_utils.hpp" #include "nav2_msgs/action/navigate_through_poses.hpp" @@ -76,7 +76,7 @@ class MissionNav2ThroughPoses : public dc_measurements::Measurement std::map last_recoveries_; // Records wait here for a poll to carry them out, one per poll, so they travel the same publish // path (Conditions, buffering, Group) as every other Record. - std::deque> pending_records_; + PendingRecordQueue pending_records_; protected: void onConfigure() override; diff --git a/dc_measurements/include/dc_measurements/plugins/measurements/mission_nav2_through_poses_core.hpp b/dc_measurements/include/dc_measurements/plugins/measurements/mission_nav2_through_poses_core.hpp index 96864052e..28ef249df 100644 --- a/dc_measurements/include/dc_measurements/plugins/measurements/mission_nav2_through_poses_core.hpp +++ b/dc_measurements/include/dc_measurements/plugins/measurements/mission_nav2_through_poses_core.hpp @@ -6,12 +6,13 @@ #include #include -#include -#include #include #include #include +#include "dc_measurements/mission_outcome.hpp" +#include "dc_measurements/mission_registry.hpp" + namespace dc_measurements { @@ -29,20 +30,6 @@ enum class GoalPhase : std::uint8_t Aborted = 6, }; -enum class MissionOutcome -{ - Succeeded, - Failed, - Cancelled, - Aborted, -}; - -struct MissionStartFact -{ - std::string mission_id; - std::uint64_t sequence; -}; - struct MissionEndFact { std::string mission_id; @@ -66,6 +53,10 @@ struct MissionEndFact * a goal_id, not a transition away from some prior baseline -- there is no natural idle state a * goal_id holds before it exists -- so this is a small bespoke tracker rather than a * StateTransitionDetector instantiation. + * + * The bounded id -> state bookkeeping (oldest-finished-first eviction, the sequence counter) is + * PrunedMissionMap, shared with MissionOpenRmfCore -- the two cores differ only in what a tracked + * mission's Active payload holds and how a terminal outcome is derived, which stays here. */ class MissionNav2ThroughPosesCore { @@ -79,14 +70,12 @@ class MissionNav2ThroughPosesCore */ std::optional observe(const std::string& goal_id, TimePoint at) { - if (missions_.find(goal_id) != missions_.end()) + if (registry_.find(goal_id) != nullptr) { return std::nullopt; } - missions_.emplace(goal_id, Active{ at, false, false }); - order_.push_back(goal_id); - prune(); - return MissionStartFact{ goal_id, ++sequence_ }; + registry_.insert(goal_id, Active{ at, false, false }); + return MissionStartFact{ goal_id, registry_.nextSequence() }; } /** @@ -96,12 +85,12 @@ class MissionNav2ThroughPosesCore */ bool shouldRequestResult(const std::string& goal_id) { - auto it = missions_.find(goal_id); - if (it == missions_.end() || it->second.finished || it->second.result_requested) + auto* active = registry_.find(goal_id); + if (active == nullptr || active->finished || active->result_requested) { return false; } - it->second.result_requested = true; + active->result_requested = true; return true; } @@ -117,11 +106,11 @@ class MissionNav2ThroughPosesCore { MissionEndFact fact; fact.mission_id = goal_id; - fact.sequence = ++sequence_; + fact.sequence = registry_.nextSequence(); fact.recoveries = recoveries; - auto it = missions_.find(goal_id); - const TimePoint started_at = (it != missions_.end()) ? it->second.started_at : at; + auto* active = registry_.find(goal_id); + const TimePoint started_at = (active != nullptr) ? active->started_at : at; fact.duration_sec = std::chrono::duration(at > started_at ? at - started_at : TimePoint::duration::zero()).count(); @@ -150,9 +139,9 @@ class MissionNav2ThroughPosesCore break; } - if (it != missions_.end()) + if (active != nullptr) { - it->second.finished = true; + active->finished = true; } return fact; } @@ -161,22 +150,10 @@ class MissionNav2ThroughPosesCore /// warning, mirroring Fault's fault_started_at_ map. std::vector openMissionIds() const { - std::vector open; - for (const auto& [id, active] : missions_) - { - if (!active.finished) - { - open.push_back(id); - } - } - return open; + return registry_.openIds(); } private: - // goal_ids never repeat in practice (they're UUIDs), so a long-running instance's map would - // otherwise grow without bound; only ever prunes finished missions, oldest first. - static constexpr std::size_t kMaxTracked = 256; - struct Active { TimePoint started_at; @@ -184,29 +161,7 @@ class MissionNav2ThroughPosesCore bool result_requested{ false }; }; - void prune() - { - while (order_.size() > kMaxTracked) - { - const auto& oldest = order_.front(); - auto it = missions_.find(oldest); - if (it != missions_.end() && it->second.finished) - { - missions_.erase(it); - order_.pop_front(); - } - else - { - // The oldest tracked goal_id is still open (a very long-running mission) -- leave it - // rather than dropping an active mission's tracking state. - break; - } - } - } - - std::map missions_; - std::deque order_; - std::uint64_t sequence_{ 0 }; + PrunedMissionMap registry_; }; } // namespace dc_measurements diff --git a/dc_measurements/include/dc_measurements/plugins/measurements/mission_nav2_tracker.hpp b/dc_measurements/include/dc_measurements/plugins/measurements/mission_nav2_tracker.hpp index 85e83bee2..65e0cea87 100644 --- a/dc_measurements/include/dc_measurements/plugins/measurements/mission_nav2_tracker.hpp +++ b/dc_measurements/include/dc_measurements/plugins/measurements/mission_nav2_tracker.hpp @@ -11,6 +11,7 @@ #include #include "dc_common/state_transition_detector.hpp" +#include "dc_measurements/mission_outcome.hpp" namespace dc_measurements { @@ -24,24 +25,6 @@ enum class MissionTerminalStatus Aborted, }; -/// The outcome a Record actually reports, per the mission lifecycle contract (#305, recorded in -/// ADR-0010). Distinct from MissionTerminalStatus: a goal that reaches Succeeded with a non-zero -/// error_code is reported as Failed rather than Succeeded -- an application-level failure nav2 -/// reported without aborting the goal status itself. -enum class MissionOutcome -{ - Succeeded, - Failed, - Cancelled, - Aborted, -}; - -struct MissionStartFact -{ - std::string mission_id; - std::uint64_t sequence{ 0 }; -}; - struct MissionEndFact { std::string mission_id; diff --git a/dc_measurements/include/dc_measurements/plugins/measurements/mission_open_rmf.hpp b/dc_measurements/include/dc_measurements/plugins/measurements/mission_open_rmf.hpp index cf0f54db3..aa4ec7690 100644 --- a/dc_measurements/include/dc_measurements/plugins/measurements/mission_open_rmf.hpp +++ b/dc_measurements/include/dc_measurements/plugins/measurements/mission_open_rmf.hpp @@ -4,15 +4,15 @@ #ifndef DC_MEASUREMENTS__PLUGINS__MEASUREMENTS__MISSION_OPEN_RMF_HPP_ #define DC_MEASUREMENTS__PLUGINS__MEASUREMENTS__MISSION_OPEN_RMF_HPP_ -#include #include #include #include -#include #include "dc_common/websocket_json_client.hpp" #include "dc_core/measurement.hpp" #include "dc_measurements/measurement.hpp" +#include "dc_measurements/mission_record_json.hpp" +#include "dc_measurements/pending_record_queue.hpp" #include "dc_measurements/plugins/measurements/mission_open_rmf_core.hpp" #include "dc_util/node_utils.hpp" #include "rclcpp/rclcpp.hpp" @@ -63,7 +63,7 @@ class MissionOpenRmf : public dc_measurements::Measurement // Records wait here for a poll to carry them out, one per poll, so they travel the same publish // path (Conditions, buffering, Group) as every other Record -- same shape as // MissionNav2ThroughPoses::pending_records_. - std::deque> pending_records_; + PendingRecordQueue pending_records_; protected: void onConfigure() override; diff --git a/dc_measurements/include/dc_measurements/plugins/measurements/mission_open_rmf_core.hpp b/dc_measurements/include/dc_measurements/plugins/measurements/mission_open_rmf_core.hpp index 3beafbc70..5bd6b6bb8 100644 --- a/dc_measurements/include/dc_measurements/plugins/measurements/mission_open_rmf_core.hpp +++ b/dc_measurements/include/dc_measurements/plugins/measurements/mission_open_rmf_core.hpp @@ -6,12 +6,14 @@ #include #include -#include #include #include #include #include +#include "dc_measurements/mission_outcome.hpp" +#include "dc_measurements/mission_registry.hpp" + namespace dc_measurements { @@ -59,20 +61,6 @@ inline RmfTaskStatus parseRmfTaskStatus(const std::string& status) return it == kByName.end() ? RmfTaskStatus::UnknownStatus : it->second; } -enum class MissionOutcome -{ - Succeeded, - Failed, - Cancelled, - Aborted, -}; - -struct MissionStartFact -{ - std::string mission_id; - std::uint64_t sequence; -}; - struct MissionEndFact { std::string mission_id; @@ -132,6 +120,10 @@ struct TaskStateSample * settles on as the end of the task's life; they are absent from `task_state.json`'s modeled * terminal set (`failed`/`canceled`/`killed`/`completed`/`skipped`). A task observed as * `blocked`/`error` stays open here; only a later terminal status closes it. + * + * The bounded id -> state bookkeeping (oldest-finished-first eviction, the sequence counter) is + * PrunedMissionMap, shared with MissionNav2ThroughPosesCore -- the two cores differ only in what a + * tracked mission's Active payload holds and how a terminal outcome is derived, which stays here. */ class MissionOpenRmfCore { @@ -159,22 +151,19 @@ class MissionOpenRmfCore ObserveResult observe(const TaskStateSample& sample, TimePoint at) { ObserveResult result; - auto it = missions_.find(sample.mission_id); + auto* active = registry_.find(sample.mission_id); - if (it == missions_.end()) + if (active == nullptr) { if (!isActive(sample.status)) { return result; } - missions_.emplace(sample.mission_id, Active{ at, false }); - order_.push_back(sample.mission_id); - prune(); - result.start = MissionStartFact{ sample.mission_id, ++sequence_ }; - it = missions_.find(sample.mission_id); + active = ®istry_.insert(sample.mission_id, Active{ at, false }); + result.start = MissionStartFact{ sample.mission_id, registry_.nextSequence() }; } - if (it->second.finished) + if (active->finished) { return result; } @@ -183,7 +172,7 @@ class MissionOpenRmfCore { MissionEndFact fact; fact.mission_id = sample.mission_id; - fact.sequence = ++sequence_; + fact.sequence = registry_.nextSequence(); fact.outcome = outcomeFor(sample.status); fact.reason = sample.reason; fact.error_code = sample.error_code; @@ -195,12 +184,12 @@ class MissionOpenRmfCore } else { - const TimePoint started_at = it->second.started_at; + const TimePoint started_at = active->started_at; fact.duration_sec = std::chrono::duration(at > started_at ? at - started_at : TimePoint::duration::zero()).count(); } - it->second.finished = true; + active->finished = true; result.end = fact; } @@ -211,23 +200,10 @@ class MissionOpenRmfCore /// shutdown warning, mirroring MissionNav2ThroughPosesCore::openMissionIds(). std::vector openMissionIds() const { - std::vector open; - for (const auto& [id, active] : missions_) - { - if (!active.finished) - { - open.push_back(id); - } - } - return open; + return registry_.openIds(); } private: - // mission_ids never repeat in practice (Open-RMF booking IDs are monotonically issued), so a - // long-running instance's map would otherwise grow without bound; only ever prunes finished - // missions, oldest first -- same policy as MissionNav2ThroughPosesCore. - static constexpr std::size_t kMaxTracked = 256; - struct Active { TimePoint started_at; @@ -280,29 +256,7 @@ class MissionOpenRmfCore } } - void prune() - { - while (order_.size() > kMaxTracked) - { - const auto& oldest = order_.front(); - auto it = missions_.find(oldest); - if (it != missions_.end() && it->second.finished) - { - missions_.erase(it); - order_.pop_front(); - } - else - { - // The oldest tracked mission_id is still open (a very long-running task); leave it rather - // than dropping an active mission's tracking state. - break; - } - } - } - - std::map missions_; - std::deque order_; - std::uint64_t sequence_{ 0 }; + PrunedMissionMap registry_; }; } // namespace dc_measurements diff --git a/dc_measurements/include/dc_measurements/plugins/measurements/ros2_control_status.hpp b/dc_measurements/include/dc_measurements/plugins/measurements/ros2_control_status.hpp index b784c9494..7ae70c2ea 100644 --- a/dc_measurements/include/dc_measurements/plugins/measurements/ros2_control_status.hpp +++ b/dc_measurements/include/dc_measurements/plugins/measurements/ros2_control_status.hpp @@ -5,17 +5,17 @@ #define DC_MEASUREMENTS__PLUGINS__MEASUREMENTS__ROS2_CONTROL_STATUS_HPP_ #include -#include #include #include #include -#include +#include #include "controller_manager_msgs/msg/controller_manager_activity.hpp" #include "controller_manager_msgs/msg/named_lifecycle_state.hpp" #include "dc_common/state_transition_detector.hpp" #include "dc_core/measurement.hpp" #include "dc_measurements/measurement.hpp" +#include "dc_measurements/pending_record_queue.hpp" #include "dc_util/node_utils.hpp" #include "rclcpp/rclcpp.hpp" @@ -55,7 +55,7 @@ class Ros2ControlStatus : public dc_measurements::Measurement std::map> hardware_component_detectors_; // Records wait here for a poll to carry them out, one per poll, so they travel the same publish // path (Conditions, buffering, Group) as every other Record. - std::deque> pending_records_; + PendingRecordQueue pending_records_; // Per-Record, not per-component: a global counter across every controller and hardware // component, so a consumer can tell a dropped Record apart from one that never changed. diff --git a/dc_measurements/plugins/measurements/fault.cpp b/dc_measurements/plugins/measurements/fault.cpp index 0c3b40a3c..4453b3e6f 100644 --- a/dc_measurements/plugins/measurements/fault.cpp +++ b/dc_measurements/plugins/measurements/fault.cpp @@ -13,8 +13,6 @@ namespace dc_measurements namespace { -constexpr size_t kMaxPendingRecords = 64; - double secondsOf(const std::chrono::system_clock::duration& d) { return std::chrono::duration(d).count(); @@ -141,19 +139,16 @@ void Fault::diagnosticsCb(const diagnostic_msgs::msg::DiagnosticArray& msg) fault_started_at_.erase(status.name); } - pending_records_.emplace_back(std::move(data), stamp); - } - - // One Record leaves per poll, so a component (or set of components) flapping far faster than - // the polling interval would otherwise queue without bound. The oldest goes first: the recent - // transitions are the ones still worth reporting. - while (pending_records_.size() > kMaxPendingRecords) - { - pending_records_.pop_front(); - RCLCPP_WARN_STREAM_THROTTLE(logger_, *getNode()->get_clock(), 10000, - "Measurement " << measurement_name_ - << ": fault Records are arriving faster than the polling interval " - "can report them; dropping the oldest."); + // One Record leaves per poll, so a component (or set of components) flapping far faster than + // the polling interval would otherwise queue without bound. The oldest goes first: the recent + // transitions are the ones still worth reporting. + if (pending_records_.push(std::move(data), stamp)) + { + RCLCPP_WARN_STREAM_THROTTLE(logger_, *getNode()->get_clock(), 10000, + "Measurement " << measurement_name_ + << ": fault Records are arriving faster than the polling interval " + "can report them; dropping the oldest."); + } } } @@ -168,8 +163,7 @@ dc_interfaces::msg::StringStamped Fault::collect() return msg; } - auto record = std::move(pending_records_.front()); - pending_records_.pop_front(); + auto record = pending_records_.pop(); msg.header.stamp = record.second; msg.data = record.first.dump(-1, ' ', true); return msg; diff --git a/dc_measurements/plugins/measurements/json/mission_base.json b/dc_measurements/plugins/measurements/json/mission_base.json new file mode 100644 index 000000000..6e4771484 --- /dev/null +++ b/dc_measurements/plugins/measurements/json/mission_base.json @@ -0,0 +1,51 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "MissionLifecycleBase", + "description": + "Property definitions shared by every Mission Measurement adapter's Record (#305, recorded in ADR-0010): nav2's NavigateToPose/NavigateThroughPoses/FollowWaypoints adapters and Open-RMF. Not a validatable schema on its own -- referenced via allOf/$ref by each adapter's own schema, which adds its own mission_type/required/conditional rules on top.", + "properties": { + "event": { + "description": "Which boundary of the mission this Record reports", + "type": "string", + "enum": [ + "mission_start", + "mission_end" + ] + }, + "mission_id": { + "description": + "This adapter's identifier for the mission -- an action goal UUID for the nav2 adapters, Open-RMF's booking.id for its adapter", + "type": "string" + }, + "sequence": { + "description": + "Monotonically increasing number of this Record, across every mission this Measurement instance has seen (not per mission_id), so a dropped Record is detectable", + "type": "integer", + "minimum": 1 + }, + "outcome": { + "description": "How the mission ended -- see the adapter's own schema for what maps to which value", + "type": "string", + "enum": [ + "succeeded", + "failed", + "cancelled", + "aborted" + ] + }, + "reason": { + "description": "Human-readable detail on why the mission ended the way it did, when the source provided one", + "type": "string" + }, + "error_code": { + "description": "The source's own numeric error code, carried through verbatim, when it provided one", + "type": "integer", + "minimum": 0 + }, + "duration_sec": { + "description": "How long the mission ran, from its mission_start boundary to its mission_end boundary", + "type": "number", + "minimum": 0 + } + } +} diff --git a/dc_measurements/plugins/measurements/json/mission_nav2.json b/dc_measurements/plugins/measurements/json/mission_nav2.json index eaac7b234..d2960da84 100644 --- a/dc_measurements/plugins/measurements/json/mission_nav2.json +++ b/dc_measurements/plugins/measurements/json/mission_nav2.json @@ -3,70 +3,32 @@ "title": "MissionNav2", "description": "One nav2 NavigateToPose goal's lifecycle, watched passively: a mission_start Record when it is accepted, a mission_end Record when it reaches a terminal state. The base Record contract every Mission Measurement nav2 adapter shares (NavigateThroughPoses, FollowWaypoints)", - "properties": { - "event": { - "description": "What this Record is: a mission being accepted, or one reaching a terminal state", - "type": "string", - "enum": [ - "mission_start", - "mission_end" - ] - }, - "mission_id": { - "description": "The NavigateToPose goal UUID, stringified", - "type": "string" - }, - "mission_type": { - "description": - "Always 'navigate_to_pose' for this adapter -- other Mission Measurement adapters set their own value", - "type": "string", - "const": "navigate_to_pose" - }, - "sequence": { - "description": - "Monotonically increasing transition number for this Measurement instance, so a dropped Record is detectable rather than silently corrupting a duration", - "type": "integer", - "minimum": 1 + "allOf": [ + { + "$ref": "mission_base.json" }, - "outcome": { - "description": - "How the mission ended. 'failed' is a goal that reached GoalStatus SUCCEEDED but still carried a non-zero error_code, distinct from 'aborted' (the goal status itself)", - "type": "string", - "enum": [ - "succeeded", - "failed", - "cancelled", - "aborted" + { + "properties": { + "mission_type": { + "description": + "Always 'navigate_to_pose' for this adapter -- other Mission Measurement adapters set their own value", + "type": "string", + "const": "navigate_to_pose" + }, + "recoveries": { + "description": + "NavigateToPose::Feedback.number_of_recoveries at mission completion, when feedback was seen before the terminal state", + "type": "integer", + "minimum": 0 + } + }, + "required": [ + "event", + "mission_id", + "mission_type", + "sequence" ] }, - "reason": { - "description": "Human-readable failure reason, verbatim from NavigateToPose::Result.error_msg", - "type": "string" - }, - "error_code": { - "description": "nav2's own numeric NavigateToPose::Result.error_code, carried through verbatim", - "type": "integer", - "minimum": 0 - }, - "duration_sec": { - "description": "How long the mission ran, from acceptance to its terminal state", - "type": "number", - "minimum": 0 - }, - "recoveries": { - "description": - "NavigateToPose::Feedback.number_of_recoveries at mission completion, when feedback was seen before the terminal state", - "type": "integer", - "minimum": 0 - } - }, - "required": [ - "event", - "mission_id", - "mission_type", - "sequence" - ], - "allOf": [ { "if": { "properties": { diff --git a/dc_measurements/plugins/measurements/json/mission_nav2_follow_waypoints.json b/dc_measurements/plugins/measurements/json/mission_nav2_follow_waypoints.json index 5519550a5..63ee954ee 100644 --- a/dc_measurements/plugins/measurements/json/mission_nav2_follow_waypoints.json +++ b/dc_measurements/plugins/measurements/json/mission_nav2_follow_waypoints.json @@ -3,88 +3,50 @@ "title": "MissionNav2FollowWaypoints", "description": "One nav2 FollowWaypoints goal's lifecycle, watched passively: a mission_start Record when it is accepted, a mission_end Record when it reaches a terminal state", - "properties": { - "event": { - "description": "What this Record is: a mission being accepted, or one reaching a terminal state", - "type": "string", - "enum": [ - "mission_start", - "mission_end" - ] - }, - "mission_id": { - "description": "The FollowWaypoints goal UUID, stringified", - "type": "string" - }, - "mission_type": { - "description": - "Always 'follow_waypoints' for this adapter -- other Mission Measurement adapters set their own value", - "type": "string", - "const": "follow_waypoints" - }, - "sequence": { - "description": - "Monotonically increasing transition number for this Measurement instance, so a dropped Record is detectable rather than silently corrupting a duration", - "type": "integer", - "minimum": 1 + "allOf": [ + { + "$ref": "mission_base.json" }, - "outcome": { - "description": - "How the mission ended. 'failed' is a goal that reached GoalStatus SUCCEEDED but still carried a non-zero error_code (nav2's WaypointFollower can finish having missed a waypoint without the action itself aborting)", - "type": "string", - "enum": [ - "succeeded", - "failed", - "cancelled", - "aborted" + { + "properties": { + "mission_type": { + "description": + "Always 'follow_waypoints' for this adapter -- other Mission Measurement adapters set their own value", + "type": "string", + "const": "follow_waypoints" + }, + "missed_waypoints": { + "description": + "One entry per waypoint FollowWaypoints::Result.missed_waypoints reports -- which specific stops were not completed, and nav2's per-waypoint error code for each", + "type": "array", + "items": { + "type": "object", + "properties": { + "index": { + "description": "Position of this waypoint in the goal's pose list", + "type": "integer", + "minimum": 0 + }, + "error_code": { + "description": "nav2's MissedWaypoint.error_code for this waypoint", + "type": "integer", + "minimum": 0 + } + }, + "required": [ + "index", + "error_code" + ] + } + } + }, + "required": [ + "event", + "mission_id", + "mission_type", + "sequence" ] }, - "reason": { - "description": "Human-readable failure reason, verbatim from FollowWaypoints::Result.error_msg", - "type": "string" - }, - "error_code": { - "description": "nav2's own numeric FollowWaypoints::Result.error_code, carried through verbatim", - "type": "integer", - "minimum": 0 - }, - "duration_sec": { - "description": "How long the mission ran, from acceptance to its terminal state", - "type": "number", - "minimum": 0 - }, - "missed_waypoints": { - "description": - "One entry per waypoint FollowWaypoints::Result.missed_waypoints reports -- which specific stops were not completed, and nav2's per-waypoint error code for each", - "type": "array", - "items": { - "type": "object", - "properties": { - "index": { - "description": "Position of this waypoint in the goal's pose list", - "type": "integer", - "minimum": 0 - }, - "error_code": { - "description": "nav2's MissedWaypoint.error_code for this waypoint", - "type": "integer", - "minimum": 0 - } - }, - "required": [ - "index", - "error_code" - ] - } - } - }, - "required": [ - "event", - "mission_id", - "mission_type", - "sequence" - ], - "allOf": [ { "if": { "properties": { diff --git a/dc_measurements/plugins/measurements/json/mission_nav2_through_poses.json b/dc_measurements/plugins/measurements/json/mission_nav2_through_poses.json index 703b1c6bc..3da9d2352 100644 --- a/dc_measurements/plugins/measurements/json/mission_nav2_through_poses.json +++ b/dc_measurements/plugins/measurements/json/mission_nav2_through_poses.json @@ -3,66 +3,29 @@ "title": "MissionNav2ThroughPoses", "description": "One nav2 NavigateThroughPoses mission's lifecycle: a mission_start Record when the goal is first observed, a mission_end Record once it reaches a terminal state. Shares #387's NavigateToPose Record contract; mission_type distinguishes the source action", - "properties": { - "event": { - "description": "Which boundary of the mission this Record reports", - "type": "string", - "enum": [ - "mission_start", - "mission_end" - ] - }, - "mission_id": { - "description": "The nav2 goal UUID, stringified as lowercase hex", - "type": "string" - }, - "mission_type": { - "description": "What kind of mission this was; the nav2 adapter always sets 'navigate_through_poses'", - "type": "string" - }, - "sequence": { - "description": - "Monotonically increasing number of this Record, across every mission this Measurement instance has seen (not per mission_id), so a dropped Record is detectable", - "type": "integer", - "minimum": 1 + "allOf": [ + { + "$ref": "mission_base.json" }, - "outcome": { - "description": - "How the mission ended. 'failed' is a non-zero NavigateThroughPoses::Result.error_code on an otherwise-succeeded goal status, distinct from 'aborted' (the goal status itself)", - "type": "string", - "enum": [ - "succeeded", - "failed", - "cancelled", - "aborted" + { + "properties": { + "mission_type": { + "description": "What kind of mission this was; the nav2 adapter always sets 'navigate_through_poses'", + "type": "string" + }, + "recoveries": { + "description": + "NavigateThroughPoses::Feedback.number_of_recoveries at mission completion, when feedback was seen", + "type": "integer", + "minimum": 0 + } + }, + "required": [ + "event", + "mission_id", + "sequence" ] }, - "reason": { - "description": "NavigateThroughPoses::Result.error_msg, verbatim. Present when outcome is 'failed' or 'aborted'", - "type": "string" - }, - "error_code": { - "description": "NavigateThroughPoses::Result.error_code, carried through verbatim. Present alongside 'reason'", - "type": "integer", - "minimum": 0 - }, - "duration_sec": { - "description": "How long the mission ran, from when the goal was first observed to its terminal status", - "type": "number", - "minimum": 0 - }, - "recoveries": { - "description": "NavigateThroughPoses::Feedback.number_of_recoveries at mission completion, when feedback was seen", - "type": "integer", - "minimum": 0 - } - }, - "required": [ - "event", - "mission_id", - "sequence" - ], - "allOf": [ { "if": { "properties": { diff --git a/dc_measurements/plugins/measurements/json/mission_open_rmf.json b/dc_measurements/plugins/measurements/json/mission_open_rmf.json index 9d3e09feb..73d4f6a42 100644 --- a/dc_measurements/plugins/measurements/json/mission_open_rmf.json +++ b/dc_measurements/plugins/measurements/json/mission_open_rmf.json @@ -3,64 +3,23 @@ "title": "MissionOpenRmf", "description": "One Open-RMF task's lifecycle: a mission_start Record when its status first leaves queued/standby into an active state, a mission_end Record once it reaches a terminal status. Shares #387's mission_start/mission_end Record contract; mission_type carries Open-RMF's own task category", - "properties": { - "event": { - "description": "Which boundary of the mission this Record reports", - "type": "string", - "enum": [ - "mission_start", - "mission_end" - ] - }, - "mission_id": { - "description": "Open-RMF's booking.id for the task", - "type": "string" - }, - "mission_type": { - "description": "Open-RMF's task category (e.g. 'delivery', 'patrol'), verbatim", - "type": "string" - }, - "sequence": { - "description": - "Monotonically increasing number of this Record, across every mission this Measurement instance has seen (not per mission_id), so a dropped Record is detectable", - "type": "integer", - "minimum": 1 + "allOf": [ + { + "$ref": "mission_base.json" }, - "outcome": { - "description": - "How the mission ended. 'completed'->succeeded, 'failed'->failed, 'canceled'->cancelled, 'killed'->aborted. Open-RMF's 'skipped' status also maps to 'cancelled': DC's outcome contract (#305/#387) has four values, not five, and 'cancelled' -- closed without completing the work, not an error -- is the closer fit than 'succeeded' or a failure outcome", - "type": "string", - "enum": [ - "succeeded", - "failed", - "cancelled", - "aborted" + { + "properties": { + "mission_type": { + "description": "Open-RMF's task category (e.g. 'delivery', 'patrol'), verbatim", + "type": "string" + } + }, + "required": [ + "event", + "mission_id", + "sequence" ] }, - "reason": { - "description": - "Best-effort detail carried verbatim from whichever field Open-RMF actually populated for this outcome: dispatch.errors (or the top-level detail) for 'failed', cancellation.labels for 'cancelled', killed.labels for 'aborted'. Not guaranteed present for every outcome -- unlike nav2's always-populated error_msg, Open-RMF does not always supply one", - "type": "string" - }, - "error_code": { - "description": - "Open-RMF's own dispatch error code, carried through verbatim when a 'failed' outcome's dispatch.errors supplied one", - "type": "integer", - "minimum": 0 - }, - "duration_sec": { - "description": - "How long the mission ran. Uses Open-RMF's own unix_millis_start_time/unix_millis_finish_time when the source provided both; falls back to this Measurement's own locally observed start/end timestamps otherwise", - "type": "number", - "minimum": 0 - } - }, - "required": [ - "event", - "mission_id", - "sequence" - ], - "allOf": [ { "if": { "properties": { diff --git a/dc_measurements/plugins/measurements/mission_nav2.cpp b/dc_measurements/plugins/measurements/mission_nav2.cpp index f1f032a73..7e60eb97d 100644 --- a/dc_measurements/plugins/measurements/mission_nav2.cpp +++ b/dc_measurements/plugins/measurements/mission_nav2.cpp @@ -3,8 +3,7 @@ #include "dc_measurements/plugins/measurements/mission_nav2.hpp" -#include -#include +#include "dc_measurements/mission_uuid.hpp" namespace dc_measurements { @@ -12,39 +11,6 @@ namespace dc_measurements namespace { -constexpr size_t kMaxPendingRecords = 64; - -std::string uuidToString(const unique_identifier_msgs::msg::UUID& uuid) -{ - std::ostringstream oss; - oss << std::hex << std::setfill('0'); - for (size_t i = 0; i < uuid.uuid.size(); ++i) - { - oss << std::setw(2) << static_cast(uuid.uuid[i]); - if (i == 3 || i == 5 || i == 7 || i == 9) - { - oss << '-'; - } - } - return oss.str(); -} - -std::string outcomeName(MissionOutcome outcome) -{ - switch (outcome) - { - case MissionOutcome::Succeeded: - return "succeeded"; - case MissionOutcome::Failed: - return "failed"; - case MissionOutcome::Cancelled: - return "cancelled"; - case MissionOutcome::Aborted: - return "aborted"; - } - return "unknown"; -} - MissionTerminalStatus terminalStatusOf(int8_t action_status) { switch (action_status) @@ -110,49 +76,13 @@ void MissionNav2::setValidationSchema() } } -json MissionNav2::missionStartJson(const MissionStartFact& fact) -{ - json data; - data["event"] = "mission_start"; - data["mission_id"] = fact.mission_id; - data["mission_type"] = "navigate_to_pose"; - data["sequence"] = fact.sequence; - return data; -} - -json MissionNav2::missionEndJson(const MissionEndFact& fact) -{ - json data; - data["event"] = "mission_end"; - data["mission_id"] = fact.mission_id; - data["mission_type"] = "navigate_to_pose"; - data["sequence"] = fact.sequence; - data["outcome"] = outcomeName(fact.outcome); - data["duration_sec"] = fact.duration_sec; - if (fact.reason.has_value()) - { - data["reason"] = *fact.reason; - } - if (fact.error_code.has_value()) - { - data["error_code"] = *fact.error_code; - } - if (fact.recoveries.has_value()) - { - data["recoveries"] = *fact.recoveries; - } - return data; -} - void MissionNav2::enqueue(json data, const rclcpp::Time& stamp) { - pending_records_.emplace_back(std::move(data), stamp); // One Record leaves per poll, so missions starting/ending far faster than the polling interval // would otherwise queue without bound. The oldest goes first: the recent boundaries are the ones // still worth reporting. - while (pending_records_.size() > kMaxPendingRecords) + if (pending_records_.push(std::move(data), stamp)) { - pending_records_.pop_front(); RCLCPP_WARN_STREAM_THROTTLE(logger_, *getNode()->get_clock(), 10000, "Measurement " << measurement_name_ << ": mission Records are arriving faster than the polling interval " @@ -168,7 +98,7 @@ void MissionNav2::statusCb(const action_msgs::msg::GoalStatusArray& msg) const std::lock_guard lock(mutex_); for (const auto& entry : msg.status_list) { - const std::string goal_id = uuidToString(entry.goal_info.goal_id); + const std::string goal_id = missionGoalIdDashed(entry.goal_info.goal_id); if (entry.status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED || entry.status == action_msgs::msg::GoalStatus::STATUS_EXECUTING || @@ -177,7 +107,7 @@ void MissionNav2::statusCb(const action_msgs::msg::GoalStatusArray& msg) const auto start = tracker_->startMission(goal_id, at); if (start.has_value()) { - enqueue(missionStartJson(*start), now); + enqueue(missionStartJson(start->mission_id, "navigate_to_pose", start->sequence), now); } else if (!tracker_->activeMissionId().has_value() || *tracker_->activeMissionId() != goal_id) { @@ -254,7 +184,13 @@ void MissionNav2::handleResultResponse(const std::string& goal_id, const rclcpp: response->result.error_msg, recoveries, at); if (end.has_value()) { - enqueue(missionEndJson(*end), terminal_at); + json data = missionEndJsonBase(end->mission_id, "navigate_to_pose", end->sequence, end->outcome, end->duration_sec, + end->reason, end->error_code); + if (end->recoveries.has_value()) + { + data["recoveries"] = *end->recoveries; + } + enqueue(std::move(data), terminal_at); } } @@ -268,8 +204,7 @@ dc_interfaces::msg::StringStamped MissionNav2::collect() { return msg; } - auto record = std::move(pending_records_.front()); - pending_records_.pop_front(); + auto record = pending_records_.pop(); msg.header.stamp = record.second; msg.data = record.first.dump(-1, ' ', true); return msg; diff --git a/dc_measurements/plugins/measurements/mission_nav2_follow_waypoints.cpp b/dc_measurements/plugins/measurements/mission_nav2_follow_waypoints.cpp index 059a240d8..2e0051490 100644 --- a/dc_measurements/plugins/measurements/mission_nav2_follow_waypoints.cpp +++ b/dc_measurements/plugins/measurements/mission_nav2_follow_waypoints.cpp @@ -3,9 +3,7 @@ #include "dc_measurements/plugins/measurements/mission_nav2_follow_waypoints.hpp" -#include -#include - +#include "dc_measurements/mission_uuid.hpp" #include "nav2_msgs/msg/missed_waypoint.hpp" namespace dc_measurements @@ -14,39 +12,6 @@ namespace dc_measurements namespace { -constexpr size_t kMaxPendingRecords = 64; - -std::string uuidToString(const unique_identifier_msgs::msg::UUID& uuid) -{ - std::ostringstream oss; - oss << std::hex << std::setfill('0'); - for (size_t i = 0; i < uuid.uuid.size(); ++i) - { - oss << std::setw(2) << static_cast(uuid.uuid[i]); - if (i == 3 || i == 5 || i == 7 || i == 9) - { - oss << '-'; - } - } - return oss.str(); -} - -std::string outcomeName(MissionOutcome outcome) -{ - switch (outcome) - { - case MissionOutcome::Succeeded: - return "succeeded"; - case MissionOutcome::Failed: - return "failed"; - case MissionOutcome::Cancelled: - return "cancelled"; - case MissionOutcome::Aborted: - return "aborted"; - } - return "unknown"; -} - MissionTerminalStatus terminalStatusOf(int8_t action_status) { switch (action_status) @@ -108,48 +73,10 @@ void MissionNav2FollowWaypoints::setValidationSchema() } } -json MissionNav2FollowWaypoints::missionStartJson(const MissionStartFact& fact) -{ - json data; - data["event"] = "mission_start"; - data["mission_id"] = fact.mission_id; - data["mission_type"] = "follow_waypoints"; - data["sequence"] = fact.sequence; - return data; -} - -json MissionNav2FollowWaypoints::missionEndJson(const MissionEndFact& fact) -{ - json data; - data["event"] = "mission_end"; - data["mission_id"] = fact.mission_id; - data["mission_type"] = "follow_waypoints"; - data["sequence"] = fact.sequence; - data["outcome"] = outcomeName(fact.outcome); - data["duration_sec"] = fact.duration_sec; - if (fact.reason.has_value()) - { - data["reason"] = *fact.reason; - } - if (fact.error_code.has_value()) - { - data["error_code"] = *fact.error_code; - } - json missed = json::array(); - for (const auto& waypoint : fact.missed_waypoints) - { - missed.push_back({ { "index", waypoint.index }, { "error_code", waypoint.error_code } }); - } - data["missed_waypoints"] = missed; - return data; -} - void MissionNav2FollowWaypoints::enqueue(json data, const rclcpp::Time& stamp) { - pending_records_.emplace_back(std::move(data), stamp); - while (pending_records_.size() > kMaxPendingRecords) + if (pending_records_.push(std::move(data), stamp)) { - pending_records_.pop_front(); RCLCPP_WARN_STREAM_THROTTLE(logger_, *getNode()->get_clock(), 10000, "Measurement " << measurement_name_ << ": mission Records are arriving faster than the polling interval " @@ -165,7 +92,7 @@ void MissionNav2FollowWaypoints::statusCb(const action_msgs::msg::GoalStatusArra const std::lock_guard lock(mutex_); for (const auto& entry : msg.status_list) { - const std::string goal_id = uuidToString(entry.goal_info.goal_id); + const std::string goal_id = missionGoalIdDashed(entry.goal_info.goal_id); if (entry.status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED || entry.status == action_msgs::msg::GoalStatus::STATUS_EXECUTING || @@ -174,7 +101,7 @@ void MissionNav2FollowWaypoints::statusCb(const action_msgs::msg::GoalStatusArra const auto start = tracker_->startMission(goal_id, at); if (start.has_value()) { - enqueue(missionStartJson(*start), now); + enqueue(missionStartJson(start->mission_id, "follow_waypoints", start->sequence), now); } else if (!tracker_->activeMissionId().has_value() || *tracker_->activeMissionId() != goal_id) { @@ -247,7 +174,15 @@ void MissionNav2FollowWaypoints::handleResultResponse(const std::string& goal_id response->result.error_msg, std::move(missed), at); if (end.has_value()) { - enqueue(missionEndJson(*end), terminal_at); + json data = missionEndJsonBase(end->mission_id, "follow_waypoints", end->sequence, end->outcome, end->duration_sec, + end->reason, end->error_code); + json missed_json = json::array(); + for (const auto& waypoint : end->missed_waypoints) + { + missed_json.push_back({ { "index", waypoint.index }, { "error_code", waypoint.error_code } }); + } + data["missed_waypoints"] = missed_json; + enqueue(std::move(data), terminal_at); } } @@ -261,8 +196,7 @@ dc_interfaces::msg::StringStamped MissionNav2FollowWaypoints::collect() { return msg; } - auto record = std::move(pending_records_.front()); - pending_records_.pop_front(); + auto record = pending_records_.pop(); msg.header.stamp = record.second; msg.data = record.first.dump(-1, ' ', true); return msg; diff --git a/dc_measurements/plugins/measurements/mission_nav2_through_poses.cpp b/dc_measurements/plugins/measurements/mission_nav2_through_poses.cpp index 4124c725e..c2d65f507 100644 --- a/dc_measurements/plugins/measurements/mission_nav2_through_poses.cpp +++ b/dc_measurements/plugins/measurements/mission_nav2_through_poses.cpp @@ -3,45 +3,11 @@ #include "dc_measurements/plugins/measurements/mission_nav2_through_poses.hpp" -#include -#include +#include "dc_measurements/mission_uuid.hpp" namespace dc_measurements { -namespace -{ - -constexpr size_t kMaxPendingRecords = 64; - -std::string uuidToHex(const std::array& uuid) -{ - std::ostringstream oss; - for (auto byte : uuid) - { - oss << std::hex << std::setfill('0') << std::setw(2) << static_cast(byte); - } - return oss.str(); -} - -std::string outcomeName(MissionOutcome outcome) -{ - switch (outcome) - { - case MissionOutcome::Succeeded: - return "succeeded"; - case MissionOutcome::Failed: - return "failed"; - case MissionOutcome::Cancelled: - return "cancelled"; - case MissionOutcome::Aborted: - default: - return "aborted"; - } -} - -} // namespace - MissionNav2ThroughPoses::MissionNav2ThroughPoses() : dc_measurements::Measurement() { } @@ -53,8 +19,11 @@ void MissionNav2ThroughPoses::onConfigure() auto node = getNode(); action_name_ = dc_util::get_str_type_param(node, measurement_name_, "action_name", "navigate_through_poses"); + // Matches the QoS an action server publishes its status topic with (reliable, transient_local): + // a late-joining watcher still gets the current goal's last-known status rather than waiting for + // the next change -- same reasoning as MissionNav2/MissionNav2FollowWaypoints. status_sub_ = node->create_subscription( - action_name_ + "/_action/status", rclcpp::QoS(10), + action_name_ + "/_action/status", rclcpp::QoS(rclcpp::KeepLast(1)).reliable().transient_local(), std::bind(&MissionNav2ThroughPoses::statusCb, this, std::placeholders::_1)); feedback_sub_ = node->create_subscription(action_name_ + "/_action/feedback", rclcpp::QoS(10), std::bind(&MissionNav2ThroughPoses::feedbackCb, this, @@ -83,13 +52,11 @@ void MissionNav2ThroughPoses::setValidationSchema() void MissionNav2ThroughPoses::emit(json data, const rclcpp::Time& stamp) { - pending_records_.emplace_back(std::move(data), stamp); // One Record leaves per poll, so missions starting/ending far faster than the polling interval // would otherwise queue without bound. The oldest goes first: the recent boundaries are the ones // still worth reporting. - while (pending_records_.size() > kMaxPendingRecords) + if (pending_records_.push(std::move(data), stamp)) { - pending_records_.pop_front(); RCLCPP_WARN_STREAM_THROTTLE(logger_, *getNode()->get_clock(), 10000, "Measurement " << measurement_name_ << ": mission Records are arriving faster than the polling interval " @@ -104,7 +71,7 @@ void MissionNav2ThroughPoses::statusCb(const action_msgs::msg::GoalStatusArray& for (const auto& status : msg.status_list) { - const std::string goal_id_hex = uuidToHex(status.goal_info.goal_id.uuid); + const std::string goal_id_hex = missionGoalIdHex(status.goal_info.goal_id.uuid); std::optional start; { @@ -113,11 +80,7 @@ void MissionNav2ThroughPoses::statusCb(const action_msgs::msg::GoalStatusArray& } if (start.has_value()) { - json data; - data["event"] = "mission_start"; - data["mission_id"] = start->mission_id; - data["mission_type"] = "navigate_through_poses"; - data["sequence"] = start->sequence; + json data = missionStartJson(start->mission_id, "navigate_through_poses", start->sequence); const std::lock_guard lock(mutex_); emit(std::move(data), stamp); } @@ -132,7 +95,7 @@ void MissionNav2ThroughPoses::statusCb(const action_msgs::msg::GoalStatusArray& void MissionNav2ThroughPoses::feedbackCb(const FeedbackMsg& msg) { - const std::string goal_id_hex = uuidToHex(msg.goal_id.uuid); + const std::string goal_id_hex = missionGoalIdHex(msg.goal_id.uuid); const std::lock_guard lock(mutex_); last_recoveries_[goal_id_hex] = msg.feedback.number_of_recoveries; } @@ -177,21 +140,8 @@ void MissionNav2ThroughPoses::handleResult(const std::string& goal_id_hex, GoalP fact = core_.end(goal_id_hex, phase, response->result.error_code, response->result.error_msg, recoveries, at); } - json data; - data["event"] = "mission_end"; - data["mission_id"] = fact.mission_id; - data["mission_type"] = "navigate_through_poses"; - data["sequence"] = fact.sequence; - data["outcome"] = outcomeName(fact.outcome); - data["duration_sec"] = fact.duration_sec; - if (fact.reason.has_value()) - { - data["reason"] = *fact.reason; - } - if (fact.error_code.has_value()) - { - data["error_code"] = *fact.error_code; - } + json data = missionEndJsonBase(fact.mission_id, "navigate_through_poses", fact.sequence, fact.outcome, + fact.duration_sec, fact.reason, fact.error_code); if (fact.recoveries.has_value()) { data["recoveries"] = *fact.recoveries; @@ -212,8 +162,7 @@ dc_interfaces::msg::StringStamped MissionNav2ThroughPoses::collect() return msg; } - auto record = std::move(pending_records_.front()); - pending_records_.pop_front(); + auto record = pending_records_.pop(); msg.header.stamp = record.second; msg.data = record.first.dump(-1, ' ', true); return msg; diff --git a/dc_measurements/plugins/measurements/mission_open_rmf.cpp b/dc_measurements/plugins/measurements/mission_open_rmf.cpp index 93856fb77..ca9631956 100644 --- a/dc_measurements/plugins/measurements/mission_open_rmf.cpp +++ b/dc_measurements/plugins/measurements/mission_open_rmf.cpp @@ -11,24 +11,6 @@ namespace dc_measurements namespace { -constexpr size_t kMaxPendingRecords = 64; - -std::string outcomeName(MissionOutcome outcome) -{ - switch (outcome) - { - case MissionOutcome::Succeeded: - return "succeeded"; - case MissionOutcome::Failed: - return "failed"; - case MissionOutcome::Cancelled: - return "cancelled"; - case MissionOutcome::Aborted: - default: - return "aborted"; - } -} - // `cancellation.labels`/`killed.labels` are string arrays ("a single value like `dashboard`, or a // key-value pair like `app=dashboard`" per rmf_api_msgs' own schema) -- joined verbatim rather // than picking just the first, so none of the source's own detail is silently dropped. @@ -167,11 +149,9 @@ void MissionOpenRmf::setValidationSchema() void MissionOpenRmf::emit(json data, const rclcpp::Time& stamp) { - pending_records_.emplace_back(std::move(data), stamp); // One Record leaves per poll, same overflow policy as MissionNav2ThroughPoses::emit(). - while (pending_records_.size() > kMaxPendingRecords) + if (pending_records_.push(std::move(data), stamp)) { - pending_records_.pop_front(); RCLCPP_WARN_STREAM_THROTTLE(logger_, *getNode()->get_clock(), 10000, "Measurement " << measurement_name_ << ": mission Records are arriving faster than the polling interval " @@ -222,32 +202,16 @@ void MissionOpenRmf::handleMessage(const nlohmann::json& message) if (observed.start.has_value()) { - json data; - data["event"] = "mission_start"; - data["mission_id"] = observed.start->mission_id; - data["mission_type"] = sample.mission_type; - data["sequence"] = observed.start->sequence; + json data = missionStartJson(observed.start->mission_id, sample.mission_type, observed.start->sequence); const std::lock_guard lock(mutex_); emit(std::move(data), stamp); } if (observed.end.has_value()) { - json data; - data["event"] = "mission_end"; - data["mission_id"] = observed.end->mission_id; - data["mission_type"] = sample.mission_type; - data["sequence"] = observed.end->sequence; - data["outcome"] = outcomeName(observed.end->outcome); - data["duration_sec"] = observed.end->duration_sec; - if (observed.end->reason.has_value()) - { - data["reason"] = *observed.end->reason; - } - if (observed.end->error_code.has_value()) - { - data["error_code"] = *observed.end->error_code; - } + json data = + missionEndJsonBase(observed.end->mission_id, sample.mission_type, observed.end->sequence, observed.end->outcome, + observed.end->duration_sec, observed.end->reason, observed.end->error_code); const std::lock_guard lock(mutex_); emit(std::move(data), stamp); } @@ -264,8 +228,7 @@ dc_interfaces::msg::StringStamped MissionOpenRmf::collect() return msg; } - auto record = std::move(pending_records_.front()); - pending_records_.pop_front(); + auto record = pending_records_.pop(); msg.header.stamp = record.second; msg.data = record.first.dump(-1, ' ', true); return msg; diff --git a/dc_measurements/plugins/measurements/ros2_control_status.cpp b/dc_measurements/plugins/measurements/ros2_control_status.cpp index d7171ddb0..885f6c15f 100644 --- a/dc_measurements/plugins/measurements/ros2_control_status.cpp +++ b/dc_measurements/plugins/measurements/ros2_control_status.cpp @@ -10,7 +10,6 @@ namespace dc_measurements namespace { -constexpr size_t kMaxPendingRecords = 64; constexpr const char* kControllerType = "controller"; constexpr const char* kHardwareComponentType = "hardware_component"; @@ -136,7 +135,17 @@ void Ros2ControlStatus::processEntries(const std::vectordwell); data["open"] = starts; - pending_records_.emplace_back(std::move(data), stamp); + // One Record leaves per poll, so a controller manager flapping far faster than the polling + // interval would otherwise queue without bound. The oldest goes first: the recent transitions + // are the ones still worth reporting. + if (pending_records_.push(std::move(data), stamp)) + { + RCLCPP_WARN_STREAM_THROTTLE(logger_, *getNode()->get_clock(), 10000, + "Measurement " + << measurement_name_ + << ": ros2_control_status Records are arriving faster than the polling interval " + "can report them; dropping the oldest."); + } } } @@ -147,19 +156,6 @@ void Ros2ControlStatus::activityCb(const controller_manager_msgs::msg::Controlle const std::lock_guard lock(mutex_); processEntries(msg.controllers, kControllerType, stamp, controller_detectors_); processEntries(msg.hardware_components, kHardwareComponentType, stamp, hardware_component_detectors_); - - // One Record leaves per poll, so a controller manager flapping far faster than the polling - // interval would otherwise queue without bound. The oldest goes first: the recent transitions - // are the ones still worth reporting. - while (pending_records_.size() > kMaxPendingRecords) - { - pending_records_.pop_front(); - RCLCPP_WARN_STREAM_THROTTLE(logger_, *getNode()->get_clock(), 10000, - "Measurement " - << measurement_name_ - << ": ros2_control_status Records are arriving faster than the polling interval " - "can report them; dropping the oldest."); - } } dc_interfaces::msg::StringStamped Ros2ControlStatus::collect() @@ -173,8 +169,7 @@ dc_interfaces::msg::StringStamped Ros2ControlStatus::collect() return msg; } - auto record = std::move(pending_records_.front()); - pending_records_.pop_front(); + auto record = pending_records_.pop(); msg.header.stamp = record.second; msg.data = record.first.dump(-1, ' ', true); return msg; diff --git a/dc_measurements/test/test_measurement_mission_nav2.cpp b/dc_measurements/test/test_measurement_mission_nav2.cpp index 550595e24..6d1e9410a 100644 --- a/dc_measurements/test/test_measurement_mission_nav2.cpp +++ b/dc_measurements/test/test_measurement_mission_nav2.cpp @@ -14,6 +14,7 @@ #include "ament_index_cpp/get_package_share_directory.hpp" #include "dc_interfaces/msg/string_stamped.hpp" +#include "dc_measurements/measurement.hpp" #include "dc_measurements/measurement_server.hpp" #include "dc_util/json_utils.hpp" #include "nav2_msgs/action/navigate_to_pose.hpp" @@ -181,11 +182,15 @@ class MeasurementMissionNav2Test : public ::testing::Test static void expectValidatesAgainstSchema(const nlohmann::json& record) { - const std::string path = ament_index_cpp::get_package_share_directory("dc_measurements") + - "/plugins/measurements/json/mission_nav2.json"; + const std::string schema_dir = + ament_index_cpp::get_package_share_directory("dc_measurements") + "/plugins/measurements/json"; + const std::string path = schema_dir + "/mission_nav2.json"; std::ifstream schema_file(path); ASSERT_TRUE(schema_file.good()) << "Schema not installed at " << path; - nlohmann::json_schema::json_validator validator; + // mission_nav2.json's allOf $refs mission_base.json (#305/ADR-0010's shared property + // definitions), so this independent re-validation needs the same loader + // dc_measurements::Measurement::validateSchema() uses in production. + nlohmann::json_schema::json_validator validator(dc_measurements::makeSchemaFileLoader(schema_dir)); validator.set_root_schema(nlohmann::json::parse(schema_file)); EXPECT_NO_THROW(validator.validate(record)) << record.dump(); } diff --git a/dc_measurements/test/test_measurement_mission_nav2_follow_waypoints.cpp b/dc_measurements/test/test_measurement_mission_nav2_follow_waypoints.cpp index a2e048df3..0d0d2a5ae 100644 --- a/dc_measurements/test/test_measurement_mission_nav2_follow_waypoints.cpp +++ b/dc_measurements/test/test_measurement_mission_nav2_follow_waypoints.cpp @@ -14,6 +14,7 @@ #include "ament_index_cpp/get_package_share_directory.hpp" #include "dc_interfaces/msg/string_stamped.hpp" +#include "dc_measurements/measurement.hpp" #include "dc_measurements/measurement_server.hpp" #include "dc_util/json_utils.hpp" #include "nav2_msgs/action/follow_waypoints.hpp" @@ -175,11 +176,15 @@ class MeasurementMissionNav2FollowWaypointsTest : public ::testing::Test static void expectValidatesAgainstSchema(const nlohmann::json& record) { - const std::string path = ament_index_cpp::get_package_share_directory("dc_measurements") + - "/plugins/measurements/json/mission_nav2_follow_waypoints.json"; + const std::string schema_dir = + ament_index_cpp::get_package_share_directory("dc_measurements") + "/plugins/measurements/json"; + const std::string path = schema_dir + "/mission_nav2_follow_waypoints.json"; std::ifstream schema_file(path); ASSERT_TRUE(schema_file.good()) << "Schema not installed at " << path; - nlohmann::json_schema::json_validator validator; + // mission_nav2_follow_waypoints.json's allOf $refs mission_base.json (#305/ADR-0010's shared + // property definitions), so this independent re-validation needs the same loader + // dc_measurements::Measurement::validateSchema() uses in production. + nlohmann::json_schema::json_validator validator(dc_measurements::makeSchemaFileLoader(schema_dir)); validator.set_root_schema(nlohmann::json::parse(schema_file)); EXPECT_NO_THROW(validator.validate(record)) << record.dump(); } diff --git a/dc_measurements/test/test_measurement_mission_nav2_through_poses.cpp b/dc_measurements/test/test_measurement_mission_nav2_through_poses.cpp index 37b5d4cec..b96e21694 100644 --- a/dc_measurements/test/test_measurement_mission_nav2_through_poses.cpp +++ b/dc_measurements/test/test_measurement_mission_nav2_through_poses.cpp @@ -15,6 +15,7 @@ #include "ament_index_cpp/get_package_share_directory.hpp" #include "dc_interfaces/msg/string_stamped.hpp" +#include "dc_measurements/measurement.hpp" #include "dc_measurements/measurement_server.hpp" #include "dc_util/json_utils.hpp" #include "nav2_msgs/action/navigate_through_poses.hpp" @@ -239,11 +240,15 @@ class MeasurementMissionNav2ThroughPosesTest : public ::testing::Test static void expectValidatesAgainstSchema(const nlohmann::json& record) { - const std::string path = ament_index_cpp::get_package_share_directory("dc_measurements") + - "/plugins/measurements/json/mission_nav2_through_poses.json"; + const std::string schema_dir = + ament_index_cpp::get_package_share_directory("dc_measurements") + "/plugins/measurements/json"; + const std::string path = schema_dir + "/mission_nav2_through_poses.json"; std::ifstream schema_file(path); ASSERT_TRUE(schema_file.good()) << "Schema not installed at " << path; - nlohmann::json_schema::json_validator validator; + // mission_nav2_through_poses.json's allOf $refs mission_base.json (#305/ADR-0010's shared + // property definitions), so this independent re-validation needs the same loader + // dc_measurements::Measurement::validateSchema() uses in production. + nlohmann::json_schema::json_validator validator(dc_measurements::makeSchemaFileLoader(schema_dir)); validator.set_root_schema(nlohmann::json::parse(schema_file)); EXPECT_NO_THROW(validator.validate(record)) << record.dump(); } diff --git a/dc_measurements/test/test_measurement_mission_open_rmf.cpp b/dc_measurements/test/test_measurement_mission_open_rmf.cpp index da09bfaec..98f62c042 100644 --- a/dc_measurements/test/test_measurement_mission_open_rmf.cpp +++ b/dc_measurements/test/test_measurement_mission_open_rmf.cpp @@ -29,6 +29,7 @@ #include "ament_index_cpp/get_package_share_directory.hpp" #include "dc_interfaces/msg/string_stamped.hpp" +#include "dc_measurements/measurement.hpp" #include "dc_measurements/measurement_server.hpp" #include "dc_util/json_utils.hpp" @@ -261,11 +262,15 @@ class MeasurementMissionOpenRmfTest : public ::testing::Test static void expectValidatesAgainstSchema(const nlohmann::json& record) { - const std::string path = ament_index_cpp::get_package_share_directory("dc_measurements") + - "/plugins/measurements/json/mission_open_rmf.json"; + const std::string schema_dir = + ament_index_cpp::get_package_share_directory("dc_measurements") + "/plugins/measurements/json"; + const std::string path = schema_dir + "/mission_open_rmf.json"; std::ifstream schema_file(path); ASSERT_TRUE(schema_file.good()) << "Schema not installed at " << path; - nlohmann::json_schema::json_validator validator; + // mission_open_rmf.json's allOf $refs mission_base.json (#305/ADR-0010's shared property + // definitions), so this independent re-validation needs the same loader + // dc_measurements::Measurement::validateSchema() uses in production. + nlohmann::json_schema::json_validator validator(dc_measurements::makeSchemaFileLoader(schema_dir)); validator.set_root_schema(nlohmann::json::parse(schema_file)); EXPECT_NO_THROW(validator.validate(record)) << record.dump(); }