Merge pull request #19 from vortexntnu/feat/state-utils

Feat/state utils

Author: jorgenfj

mission/vortex_yasmin_utils/CMakeLists.txt

@@ -21,6 +21,7 @@ add_library(vortex_yasmin_utils
   src/landmark_polling_state.cpp
   src/service_trigger_wait_state.cpp
   src/waypoint_goal_state.cpp
+  src/persistent_waypoint_manager_state.cpp
 )
 
 target_include_directories(vortex_yasmin_utils PUBLIC

mission/vortex_yasmin_utils/include/vortex_yasmin_utils/first_wins_concurrence.hpp

@@ -6,6 +6,7 @@
 #include <mutex>
 #include <string>
 #include <unordered_map>
+#include <unordered_set>
 
 #include <yasmin/state.hpp>
 #include <yasmin/types.hpp>
@@ -48,14 +49,21 @@ using FirstWinsOutcomeMap =
  * @param states         Map of logical name → state to run concurrently.
  * @param default_outcome Returned when the winner's outcome has no mapping.
  * @param outcome_map    Per-state outcome translation table (see above).
+ * @param winner_states  Optional set of state names that are eligible to win.
+ *                       If empty (default), every state can win.  When
+ *                       non-empty, only a state whose name appears in this set
+ *                       can trigger "first done"; other states run alongside
+ *                       but their completion is silently ignored.
  */
 class FirstWinsConcurrence : public yasmin::State {
    public:
     YASMIN_PTR_ALIASES(FirstWinsConcurrence)
 
-    FirstWinsConcurrence(const yasmin::StateMap& states,
-                         const std::string& default_outcome,
-                         const FirstWinsOutcomeMap& outcome_map);
+    FirstWinsConcurrence(
+        const yasmin::StateMap& states,
+        const std::string& default_outcome,
+        const FirstWinsOutcomeMap& outcome_map,
+        const std::unordered_set<std::string>& winner_states = {});
 
     std::string execute(yasmin::Blackboard::SharedPtr blackboard) override;
 
@@ -65,6 +73,7 @@ class FirstWinsConcurrence : public yasmin::State {
     const yasmin::StateMap states_;
     const std::string default_outcome_;
     const FirstWinsOutcomeMap outcome_map_;
+    const std::unordered_set<std::string> winner_states_;
 
     std::atomic<bool> first_done_{false};
     std::string winning_state_name_;

mission/vortex_yasmin_utils/include/vortex_yasmin_utils/persistent_waypoint_manager_state.hpp

@@ -0,0 +1,44 @@
+#ifndef VORTEX_YASMIN_UTILS__PERSISTENT_WAYPOINT_MANAGER_STATE_HPP_
+#define VORTEX_YASMIN_UTILS__PERSISTENT_WAYPOINT_MANAGER_STATE_HPP_
+
+#include <string>
+
+#include <vortex_msgs/action/waypoint_manager.hpp>
+
+#include <yasmin_ros/action_state.hpp>
+
+namespace vortex_yasmin_utils {
+
+using WaypointManagerAction = vortex_msgs::action::WaypointManager;
+
+/**
+ * @brief Starts the WaypointManager action server in persistent mode.
+ *
+ * Sends an empty waypoint list with persistent = true, so the action server
+ * stays alive and accepts waypoints from other sources. Does not send any
+ * waypoints itself.
+ *
+ * All configuration is stored as member variables — no blackboard reads or
+ * writes — so instances are safe to use inside a yasmin concurrent container.
+ *
+ * Outcomes: SUCCEED, ABORT.
+ *
+ * @param action_server_name    Name of the WaypointManager action server.
+ * @param convergence_threshold Convergence threshold for the action server.
+ */
+class PersistentWaypointManagerState
+    : public yasmin_ros::ActionState<WaypointManagerAction> {
+   public:
+    PersistentWaypointManagerState(const std::string& action_server_name,
+                                   double convergence_threshold = 0.1);
+
+    WaypointManagerAction::Goal create_goal(
+        yasmin::Blackboard::SharedPtr blackboard);
+
+   private:
+    double convergence_threshold_;
+};
+
+}  // namespace vortex_yasmin_utils
+
+#endif  // VORTEX_YASMIN_UTILS__PERSISTENT_WAYPOINT_MANAGER_STATE_HPP_

mission/vortex_yasmin_utils/src/first_wins_concurrence.cpp

@@ -2,6 +2,7 @@
 
 #include <stdexcept>
 #include <thread>
+#include <unordered_set>
 #include <vector>
 
 #include <yasmin/logs.hpp>
@@ -27,18 +28,28 @@ yasmin::Outcomes FirstWinsConcurrence::collect_outcomes(
 FirstWinsConcurrence::FirstWinsConcurrence(
     const yasmin::StateMap& states,
     const std::string& default_outcome,
-    const FirstWinsOutcomeMap& outcome_map)
+    const FirstWinsOutcomeMap& outcome_map,
+    const std::unordered_set<std::string>& winner_states)
     : State(collect_outcomes(outcome_map, default_outcome)),
       states_(states),
       default_outcome_(default_outcome),
-      outcome_map_(outcome_map) {
+      outcome_map_(outcome_map),
+      winner_states_(winner_states) {
     for (const auto& [state_name, _] : outcome_map) {
         if (states.find(state_name) == states.end()) {
             throw std::invalid_argument(
                 "FirstWinsConcurrence: outcome_map references unknown state '" +
                 state_name + "'");
         }
     }
+    for (const auto& name : winner_states_) {
+        if (states.find(name) == states.end()) {
+            throw std::invalid_argument(
+                "FirstWinsConcurrence: winner_states references unknown "
+                "state '" +
+                name + "'");
+        }
+    }
 }
 
 std::string FirstWinsConcurrence::execute(
@@ -54,7 +65,18 @@ std::string FirstWinsConcurrence::execute(
         threads.emplace_back([this, blackboard, name = name, state = state]() {
             const std::string outcome = (*state)(blackboard);
 
-            // Only the first completion wins; subsequent ones are ignored.
+            // If winner_states is set, only those states can win the race.
+            if (!winner_states_.empty() &&
+                winner_states_.find(name) == winner_states_.end()) {
+                YASMIN_LOG_INFO(
+                    "FirstWinsConcurrence: '%s' finished with '%s' but is not "
+                    "an eligible winner, ignoring",
+                    name.c_str(), outcome.c_str());
+                return;
+            }
+
+            // Only the first eligible completion wins; subsequent ones are
+            // ignored.
             bool expected = false;
             if (first_done_.compare_exchange_strong(expected, true)) {
                 {

mission/vortex_yasmin_utils/src/landmark_converge_state.cpp

@@ -2,6 +2,7 @@
 
 #include <vortex/utils/ros/ros_conversions.hpp>
 #include <vortex/utils/waypoint_utils.hpp>
+#include <vortex_msgs/msg/landmark.hpp>
 
 namespace vortex_yasmin_utils {
 
@@ -19,10 +20,24 @@ LandmarkConvergeState::LandmarkConvergeState(
       subtype_(subtype) {}
 
 LandmarkConvergenceAction::Goal LandmarkConvergeState::create_goal(
-    yasmin::Blackboard::SharedPtr /*blackboard*/) {
+    yasmin::Blackboard::SharedPtr blackboard) {
     LandmarkConvergenceAction::Goal goal;
-    goal.type = type_;
-    goal.subtype = subtype_;
+
+    if (blackboard->contains("landmarks")) {
+        auto landmarks =
+            blackboard->get<std::vector<vortex_msgs::msg::Landmark>>(
+                "landmarks");
+        if (!landmarks.empty()) {
+            goal.type = landmarks.front().type;
+            goal.subtype = landmarks.front().subtype;
+        } else {
+            goal.type = type_;
+            goal.subtype = subtype_;
+        }
+    } else {
+        goal.type = type_;
+        goal.subtype = subtype_;
+    }
     goal.convergence_offset = vortex::utils::ros_conversions::to_pose_msg(
         convergence_goal_.convergence_offset);
     goal.convergence_threshold = convergence_goal_.convergence_threshold;

mission/vortex_yasmin_utils/src/persistent_waypoint_manager_state.cpp

@@ -0,0 +1,24 @@
+#include "vortex_yasmin_utils/persistent_waypoint_manager_state.hpp"
+
+namespace vortex_yasmin_utils {
+
+PersistentWaypointManagerState::PersistentWaypointManagerState(
+    const std::string& action_server_name,
+    double convergence_threshold)
+    : ActionState(action_server_name,
+                  std::bind(&PersistentWaypointManagerState::create_goal,
+                            this,
+                            std::placeholders::_1)),
+      convergence_threshold_(convergence_threshold) {}
+
+WaypointManagerAction::Goal PersistentWaypointManagerState::create_goal(
+    yasmin::Blackboard::SharedPtr /*blackboard*/) {
+    WaypointManagerAction::Goal goal;
+    goal.waypoints = {};
+    goal.persistent = true;
+    goal.convergence_threshold = convergence_threshold_;
+
+    return goal;
+}
+
+}  // namespace vortex_yasmin_utils