added landmarkwaypointstate

mission/vortex_yasmin_utils/CMakeLists.txt

@@ -19,6 +19,7 @@ add_library(vortex_yasmin_utils
   src/first_wins_concurrence.cpp
   src/landmark_converge_state.cpp
   src/landmark_polling_state.cpp
+  src/landmark_waypoint_state.cpp
   src/service_trigger_wait_state.cpp
   src/waypoint_goal_state.cpp
   src/persistent_waypoint_manager_state.cpp

mission/vortex_yasmin_utils/include/vortex_yasmin_utils/landmark_waypoint_state.hpp

@@ -0,0 +1,51 @@
+#ifndef VORTEX_YASMIN_UTILS__LANDMARK_WAYPOINT_STATE_HPP_
+#define VORTEX_YASMIN_UTILS__LANDMARK_WAYPOINT_STATE_HPP_
+
+#include <string>
+
+#include <vortex/utils/waypoint_utils.hpp>
+#include <vortex_msgs/action/waypoint_manager.hpp>
+
+#include <yasmin_ros/action_state.hpp>
+
+namespace vortex_yasmin_utils {
+
+using WaypointManagerAction = vortex_msgs::action::WaypointManager;
+
+/**
+ * @brief Sends a waypoint to the WaypointManager based on a BB landmark pose +
+ * offset.
+ *
+ * Reads the landmark vector posted by LandmarkPollingState from the blackboard,
+ * applies the configured pose offset to the first landmark's position, and
+ * sends the resulting waypoint to the WaypointManager action server.
+ *
+ * The offset is expressed in the landmark frame and is loaded from a YAML file
+ * via load_waypoint_goal_from_yaml() before constructing this state.
+ *
+ * Outcomes: SUCCEED, ABORT.
+ *
+ * @param action_server_name  Name of the WaypointManager action server.
+ * @param waypoint_goal       Offset pose, waypoint mode, and convergence
+ *                            threshold (loaded from YAML).
+ * @param landmarks_bb_key    Blackboard key where LandmarkPollingState stores
+ *                            the landmark vector (default: "landmarks").
+ */
+class LandmarkWaypointState
+    : public yasmin_ros::ActionState<WaypointManagerAction> {
+   public:
+    LandmarkWaypointState(const std::string& action_server_name,
+                          vortex::utils::waypoints::WaypointGoal waypoint_goal,
+                          const std::string& landmarks_bb_key = "landmarks");
+
+    WaypointManagerAction::Goal create_goal(
+        yasmin::Blackboard::SharedPtr blackboard);
+
+   private:
+    vortex::utils::waypoints::WaypointGoal waypoint_goal_;
+    std::string landmarks_bb_key_;
+};
+
+}  // namespace vortex_yasmin_utils
+
+#endif  // VORTEX_YASMIN_UTILS__LANDMARK_WAYPOINT_STATE_HPP_

mission/vortex_yasmin_utils/src/landmark_waypoint_state.cpp

@@ -0,0 +1,59 @@
+#include "vortex_yasmin_utils/landmark_waypoint_state.hpp"
+
+#include <stdexcept>
+
+#include <vortex/utils/ros/ros_conversions.hpp>
+#include <vortex/utils/ros/waypoint_ros_conversions.hpp>
+#include <vortex/utils/waypoint_utils.hpp>
+#include <vortex_msgs/msg/landmark.hpp>
+#include <vortex_msgs/msg/waypoint.hpp>
+#include <vortex_msgs/msg/waypoint_mode.hpp>
+
+namespace vortex_yasmin_utils {
+
+LandmarkWaypointState::LandmarkWaypointState(
+    const std::string& action_server_name,
+    vortex::utils::waypoints::WaypointGoal waypoint_goal,
+    const std::string& landmarks_bb_key)
+    : ActionState(action_server_name,
+                  std::bind(&LandmarkWaypointState::create_goal,
+                            this,
+                            std::placeholders::_1)),
+      waypoint_goal_(std::move(waypoint_goal)),
+      landmarks_bb_key_(landmarks_bb_key) {}
+
+WaypointManagerAction::Goal LandmarkWaypointState::create_goal(
+    yasmin::Blackboard::SharedPtr blackboard) {
+    if (!blackboard->contains(landmarks_bb_key_)) {
+        throw std::runtime_error("LandmarkWaypointState: blackboard key '" +
+                                 landmarks_bb_key_ + "' not found");
+    }
+
+    auto landmarks = blackboard->get<std::vector<vortex_msgs::msg::Landmark>>(
+        landmarks_bb_key_);
+
+    if (landmarks.empty()) {
+        throw std::runtime_error(
+            "LandmarkWaypointState: landmark vector is empty");
+    }
+
+    auto landmark_pose = vortex::utils::ros_conversions::ros_pose_to_pose(
+        landmarks.front().pose.pose);
+
+    auto target_pose = vortex::utils::waypoints::apply_pose_offset(
+        landmark_pose, waypoint_goal_.pose);
+
+    vortex_msgs::msg::Waypoint wp;
+    wp.pose = vortex::utils::ros_conversions::to_pose_msg(target_pose);
+    wp.waypoint_mode =
+        vortex::utils::waypoints::waypoint_mode_to_ros(waypoint_goal_.mode);
+
+    WaypointManagerAction::Goal goal;
+    goal.waypoints = {wp};
+    goal.persistent = false;
+    goal.convergence_threshold = waypoint_goal_.convergence_threshold;
+
+    return goal;
+}
+
+}  // namespace vortex_yasmin_utils