feat(dc_measurements): add Mission Measurement NavigateThroughPoses adapter

dc_measurements/CMakeLists.txt

@@ -5,6 +5,7 @@ cmake_minimum_required(VERSION 3.8)
 project(dc_measurements)
 
 set(dependencies
+  action_msgs
   cv_bridge
   dc_common
   dc_core
@@ -13,17 +14,20 @@ set(dependencies
   diagnostic_msgs
   geometry_msgs
   lifecycle_msgs
+  nav2_msgs
   nav2_util
   nlohmann_json
   nlohmann_json_schema_validator
   PkgConfig
   pluginlib
+  rclcpp_action
   rclcpp_components
   rclcpp_lifecycle
   sensor_msgs
   std_msgs
   tf2
   tf2_geometry_msgs
+  unique_identifier_msgs
   yaml_cpp_vendor
 )
 
@@ -163,6 +167,9 @@ list(APPEND dc_measurement_plugin_libs dc_ip_camera_measurement)
 add_library(dc_map_measurement SHARED plugins/measurements/map.cpp)
 list(APPEND dc_measurement_plugin_libs dc_map_measurement)
 
+add_library(dc_mission_nav2_through_poses_measurement SHARED plugins/measurements/mission_nav2_through_poses.cpp)
+list(APPEND dc_measurement_plugin_libs dc_mission_nav2_through_poses_measurement)
+
 add_library(dc_memory_measurement SHARED
   plugins/measurements/memory.cpp
   plugins/measurements/system/linux_parser.cpp
@@ -337,6 +344,7 @@ set(tests
   test_measurement_list_string_equal
   test_measurement_map
   test_measurement_memory
+  test_measurement_mission_nav2_through_poses
   test_measurement_moving
   test_measurement_network
   test_measurement_os
@@ -353,6 +361,7 @@ set(tests
   test_measurement_tcp_health
   test_measurement_thermal
   test_measurement_uptime
+  test_mission_nav2_through_poses_core
 )
 
 if(BUILD_TESTING)

dc_measurements/include/dc_measurements/plugins/measurements/mission_nav2_through_poses.hpp

@@ -0,0 +1,89 @@
+// SPDX-FileCopyrightText: 2022-2026 David Bensoussan
+// SPDX-License-Identifier: MPL-2.0
+
+#ifndef DC_MEASUREMENTS__PLUGINS__MEASUREMENTS__MISSION_NAV2_THROUGH_POSES_HPP_
+#define DC_MEASUREMENTS__PLUGINS__MEASUREMENTS__MISSION_NAV2_THROUGH_POSES_HPP_
+
+#include <array>
+#include <cstdint>
+#include <deque>
+#include <map>
+#include <mutex>
+#include <optional>
+#include <string>
+#include <utility>
+
+#include "action_msgs/msg/goal_status_array.hpp"
+#include "dc_core/measurement.hpp"
+#include "dc_measurements/measurement.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"
+#include "rclcpp/rclcpp.hpp"
+#include "unique_identifier_msgs/msg/uuid.hpp"
+
+namespace dc_measurements
+{
+
+/**
+ * @class dc_measurements::MissionNav2ThroughPoses
+ * @brief The nav2 adapter of the Mission Measurement (#396) for `NavigateThroughPoses`: the action
+ * a deployment issues when a mission is a single job through several hard-constraint poses in one
+ * call, rather than a chain of separate `NavigateToPose` goals (#387). Emits `mission_start` when a
+ * goal is first observed and `mission_end` once it reaches a terminal state, in the same Record
+ * schema #387 defines with `mission_type: "navigate_through_poses"`.
+ *
+ * A passive observer, not a second client competing for the action server's single active goal:
+ * it subscribes to the action's own status (`_action/status`) and feedback (`_action/feedback`)
+ * topics and calls its `_action/get_result` service directly for whichever goal_id just reached a
+ * terminal status -- the same standard per-action topics/services every `rclcpp_action::Server`
+ * exposes, rather than `rclcpp_action::Client`'s higher-level API, which has no supported way to
+ * attach to a goal this Measurement did not itself send.
+ */
+class MissionNav2ThroughPoses : public dc_measurements::Measurement
+{
+public:
+  using ActionT = nav2_msgs::action::NavigateThroughPoses;
+  using GetResultSrv = ActionT::Impl::GetResultService;
+  using FeedbackMsg = ActionT::Impl::FeedbackMessage;
+
+  MissionNav2ThroughPoses();
+  ~MissionNav2ThroughPoses() override;
+  dc_interfaces::msg::StringStamped collect() override;
+
+private:
+  void statusCb(const action_msgs::msg::GoalStatusArray& msg);
+  void feedbackCb(const FeedbackMsg& msg);
+  void requestResult(const std::string& goal_id_hex, const unique_identifier_msgs::msg::UUID& goal_id, GoalPhase phase,
+                     const rclcpp::Time& detected_at);
+  void handleResult(const std::string& goal_id_hex, GoalPhase phase, const rclcpp::Time& detected_at,
+                    const GetResultSrv::Response::SharedPtr& response);
+  void emit(json data, const rclcpp::Time& stamp);
+
+  std::string action_name_;
+  rclcpp::Subscription<action_msgs::msg::GoalStatusArray>::SharedPtr status_sub_;
+  rclcpp::Subscription<FeedbackMsg>::SharedPtr feedback_sub_;
+  rclcpp::Client<GetResultSrv>::SharedPtr get_result_client_;
+
+  // The status/feedback callbacks and the polling timer run in different callback groups under a
+  // multi-threaded executor, so everything they share is guarded -- same reasoning as
+  // Battery/Fault.
+  mutable std::mutex mutex_;
+  MissionNav2ThroughPosesCore core_;
+  // Last-seen `number_of_recoveries` per goal_id, from the feedback topic -- there is no recovery
+  // count on the terminal Result, only on Feedback, so it has to be captured live and carried
+  // forward to whichever mission_end Record eventually asks for it.
+  std::map<std::string, int> 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<std::pair<json, rclcpp::Time>> pending_records_;
+
+protected:
+  void onConfigure() override;
+  void onCleanup() override;
+  void setValidationSchema() override;
+};
+
+}  // namespace dc_measurements
+
+#endif  // DC_MEASUREMENTS__PLUGINS__MEASUREMENTS__MISSION_NAV2_THROUGH_POSES_HPP_

dc_measurements/include/dc_measurements/plugins/measurements/mission_nav2_through_poses_core.hpp

@@ -0,0 +1,214 @@
+// SPDX-FileCopyrightText: 2022-2026 David Bensoussan
+// SPDX-License-Identifier: MPL-2.0
+
+#ifndef DC_MEASUREMENTS__PLUGINS__MEASUREMENTS__MISSION_NAV2_THROUGH_POSES_CORE_HPP_
+#define DC_MEASUREMENTS__PLUGINS__MEASUREMENTS__MISSION_NAV2_THROUGH_POSES_CORE_HPP_
+
+#include <chrono>
+#include <cstdint>
+#include <deque>
+#include <map>
+#include <optional>
+#include <string>
+#include <vector>
+
+namespace dc_measurements
+{
+
+/// Mirrors action_msgs::msg::GoalStatus's STATUS_* constants numerically, so this header (and its
+/// unit test) stay free of any action_msgs/ROS include -- the plugin static_casts the real message
+/// field into this enum.
+enum class GoalPhase : std::uint8_t
+{
+  Unknown = 0,
+  Accepted = 1,
+  Executing = 2,
+  Canceling = 3,
+  Succeeded = 4,
+  Canceled = 5,
+  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;
+  std::uint64_t sequence;
+  MissionOutcome outcome;
+  double duration_sec;
+  std::optional<std::string> reason;
+  std::optional<std::uint16_t> error_code;
+  std::optional<int> recoveries;
+};
+
+/**
+ * @class dc_measurements::MissionNav2ThroughPosesCore
+ * @brief ROS-free interpretation of a NavigateThroughPoses goal's lifecycle, following #360's
+ * split precedent (dc_common::BatteryCycleAccumulator/StateTransitionDetector): which goal_id is
+ * new (a mission_start fact) and, once its terminal status and result are known, what its
+ * mission_end fact looks like (outcome, duration, reason). Owns no clock and no ROS types, so a
+ * unit test drives a whole mission in microseconds with no node or action server involved.
+ *
+ * Unlike dc_common::StateTransitionDetector, a mission's "start" is the very first sample seen for
+ * 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<GoalPhase> instantiation.
+ */
+class MissionNav2ThroughPosesCore
+{
+public:
+  using TimePoint = std::chrono::system_clock::time_point;
+
+  /**
+   * @brief Feed one observed status sample for `goal_id`. Returns a MissionStartFact the first
+   * time this goal_id is seen and nullopt on every later sample for the same goal_id -- only the
+   * boundary is reported, matching every other tracker in this codebase.
+   */
+  std::optional<MissionStartFact> observe(const std::string& goal_id, TimePoint at)
+  {
+    if (missions_.find(goal_id) != missions_.end())
+    {
+      return std::nullopt;
+    }
+    missions_.emplace(goal_id, Active{ at, false, false });
+    order_.push_back(goal_id);
+    prune();
+    return MissionStartFact{ goal_id, ++sequence_ };
+  }
+
+  /**
+   * @brief Whether the caller should fetch `goal_id`'s result now that it has reached a terminal
+   * phase. True only the first time a terminal phase is observed for a goal_id, so a status topic
+   * that keeps republishing a finished goal's last entry doesn't queue duplicate result fetches.
+   */
+  bool shouldRequestResult(const std::string& goal_id)
+  {
+    auto it = missions_.find(goal_id);
+    if (it == missions_.end() || it->second.finished || it->second.result_requested)
+    {
+      return false;
+    }
+    it->second.result_requested = true;
+    return true;
+  }
+
+  /**
+   * @brief `goal_id`'s mission_end fact, once its terminal phase and result are known. `outcome`
+   * follows #387/#388's contract: CANCELED maps to cancelled, ABORTED to aborted (both carrying
+   * `error_msg`/`error_code` as `reason`/`error_code`), and SUCCEEDED maps to succeeded unless
+   * `error_code` is non-zero, in which case it is failed (also carrying reason/error_code) -- an
+   * application-level failure nav2 reported without aborting the goal status itself.
+   */
+  MissionEndFact end(const std::string& goal_id, GoalPhase terminal_phase, std::uint16_t error_code,
+                     const std::string& error_msg, std::optional<int> recoveries, TimePoint at)
+  {
+    MissionEndFact fact;
+    fact.mission_id = goal_id;
+    fact.sequence = ++sequence_;
+    fact.recoveries = recoveries;
+
+    auto it = missions_.find(goal_id);
+    const TimePoint started_at = (it != missions_.end()) ? it->second.started_at : at;
+    fact.duration_sec =
+        std::chrono::duration<double>(at > started_at ? at - started_at : TimePoint::duration::zero()).count();
+
+    switch (terminal_phase)
+    {
+      case GoalPhase::Canceled:
+        fact.outcome = MissionOutcome::Cancelled;
+        break;
+      case GoalPhase::Aborted:
+        fact.outcome = MissionOutcome::Aborted;
+        fact.reason = error_msg;
+        fact.error_code = error_code;
+        break;
+      case GoalPhase::Succeeded:
+      default:
+        if (error_code != 0)
+        {
+          fact.outcome = MissionOutcome::Failed;
+          fact.reason = error_msg;
+          fact.error_code = error_code;
+        }
+        else
+        {
+          fact.outcome = MissionOutcome::Succeeded;
+        }
+        break;
+    }
+
+    if (it != missions_.end())
+    {
+      it->second.finished = true;
+    }
+    return fact;
+  }
+
+  /// goal_ids still open (mission_start emitted, no mission_end yet) -- for onCleanup()'s shutdown
+  /// warning, mirroring Fault's fault_started_at_ map.
+  std::vector<std::string> openMissionIds() const
+  {
+    std::vector<std::string> open;
+    for (const auto& [id, active] : missions_)
+    {
+      if (!active.finished)
+      {
+        open.push_back(id);
+      }
+    }
+    return open;
+  }
+
+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;
+    bool finished{ false };
+    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<std::string, Active> missions_;
+  std::deque<std::string> order_;
+  std::uint64_t sequence_{ 0 };
+};
+
+}  // namespace dc_measurements
+
+#endif  // DC_MEASUREMENTS__PLUGINS__MEASUREMENTS__MISSION_NAV2_THROUGH_POSES_CORE_HPP_

dc_measurements/measurement_plugin.xml

@@ -100,6 +100,14 @@ SPDX-License-Identifier: MPL-2.0
         </class>
     </library>
 
+    <library path="dc_mission_nav2_through_poses_measurement">
+        <class name="dc_measurements/MissionNav2ThroughPoses" type="dc_measurements::MissionNav2ThroughPoses" base_class_type="dc_core::Measurement">
+            <description>
+                dc_measurement_mission_nav2_through_poses
+            </description>
+        </class>
+    </library>
+
     <library path="dc_memory_measurement">
         <class name="dc_measurements/Memory" type="dc_measurements::Memory" base_class_type="dc_core::Measurement">
             <description>