Feat/yasmin fsm utils

mission/vortex_yasmin_utils/CMakeLists.txt

@@ -0,0 +1,68 @@
+cmake_minimum_required(VERSION 3.8)
+project(vortex_yasmin_utils)
+
+set(CMAKE_CXX_STANDARD 20)
+add_compile_options(-Wall -Wextra -Wpedantic)
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_action REQUIRED)
+find_package(yasmin REQUIRED)
+find_package(yasmin_ros REQUIRED)
+find_package(vortex_msgs REQUIRED)
+find_package(vortex_utils REQUIRED)
+find_package(vortex_utils_ros REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(std_srvs REQUIRED)
+
+add_library(vortex_yasmin_utils
+  src/landmark_converge_state.cpp
+  src/landmark_polling_state.cpp
+  src/trigger_wait_state.cpp
+)
+
+target_include_directories(vortex_yasmin_utils PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+
+ament_target_dependencies(vortex_yasmin_utils PUBLIC
+  rclcpp
+  rclcpp_action
+  yasmin
+  yasmin_ros
+  vortex_msgs
+  vortex_utils
+  vortex_utils_ros
+  geometry_msgs
+  std_srvs
+)
+
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
+
+install(
+  TARGETS vortex_yasmin_utils
+  EXPORT vortex_yasmin_utils_targets
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+ament_export_targets(vortex_yasmin_utils_targets)
+ament_export_dependencies(
+  rclcpp
+  rclcpp_action
+  yasmin
+  yasmin_ros
+  vortex_msgs
+  vortex_utils
+  vortex_utils_ros
+  geometry_msgs
+  std_srvs
+)
+ament_export_include_directories(include)
+
+ament_package()

mission/vortex_yasmin_utils/include/vortex_yasmin_utils/landmark_converge_state.hpp

@@ -0,0 +1,45 @@
+#ifndef VORTEX_YASMIN_UTILS__LANDMARK_CONVERGE_STATE_HPP_
+#define VORTEX_YASMIN_UTILS__LANDMARK_CONVERGE_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 Converges on the first landmark in a vector read from the blackboard.
+ *
+ * Reads a std::vector<vortex_msgs::msg::Landmark> from the blackboard
+ * under the caller-specified key, takes the first element, applies the
+ * convergence offset, and sends the resulting waypoint to the
+ * WaypointManager action server.
+ *
+ * @param action_server_name  Name of the WaypointManager action server.
+ * @param convergence_goal    Offset, mode, and threshold for convergence.
+ * @param landmarks_bb_key    Blackboard key for the input landmarks vector.
+ */
+class LandmarkConvergeState
+    : public yasmin_ros::ActionState<WaypointManagerAction> {
+   public:
+    LandmarkConvergeState(
+        const std::string& action_server_name,
+        vortex::utils::waypoints::LandmarkConvergenceGoal convergence_goal,
+        const std::string& landmarks_bb_key);
+
+    WaypointManagerAction::Goal create_goal(
+        yasmin::Blackboard::SharedPtr blackboard);
+
+   private:
+    vortex::utils::waypoints::LandmarkConvergenceGoal convergence_goal_;
+    std::string landmarks_bb_key_;
+};
+
+}  // namespace vortex_yasmin_utils
+
+#endif  // VORTEX_YASMIN_UTILS__LANDMARK_CONVERGE_STATE_HPP_

mission/vortex_yasmin_utils/include/vortex_yasmin_utils/landmark_polling_state.hpp

@@ -0,0 +1,55 @@
+#ifndef VORTEX_YASMIN_UTILS__LANDMARK_POLLING_STATE_HPP_
+#define VORTEX_YASMIN_UTILS__LANDMARK_POLLING_STATE_HPP_
+
+#include <string>
+
+#include <vortex_msgs/action/landmark_polling.hpp>
+#include <vortex_msgs/msg/landmark_subtype.hpp>
+#include <vortex_msgs/msg/landmark_type.hpp>
+
+#include <yasmin_ros/action_state.hpp>
+
+namespace vortex_yasmin_utils {
+
+using LandmarkPollingAction = vortex_msgs::action::LandmarkPolling;
+
+/**
+ * @brief Polls for landmarks of a given type/subtype.
+ *
+ * On success, stores the full vector of landmarks on the blackboard
+ * under the caller-specified key.
+ *
+ * Outcomes: caller-specified success outcome, ABORT.
+ *
+ * @param action_server_name  Name of the LandmarkPolling action server.
+ * @param type                Landmark type to poll for.
+ * @param subtype             Landmark subtype to poll for.
+ * @param landmarks_bb_key    Blackboard key for the output landmarks vector.
+ * @param success_outcome     Outcome string returned on success.
+ */
+class LandmarkPollingState
+    : public yasmin_ros::ActionState<LandmarkPollingAction> {
+   public:
+    LandmarkPollingState(
+        const std::string& action_server_name,
+        vortex_msgs::msg::LandmarkType type,
+        vortex_msgs::msg::LandmarkSubtype subtype,
+        const std::string& landmarks_bb_key,
+        const std::string& success_outcome = "landmarks_found");
+
+    LandmarkPollingAction::Goal create_goal(
+        yasmin::Blackboard::SharedPtr blackboard);
+
+    std::string result_handler(yasmin::Blackboard::SharedPtr blackboard,
+                               LandmarkPollingAction::Result::SharedPtr result);
+
+   private:
+    vortex_msgs::msg::LandmarkType landmark_type_;
+    vortex_msgs::msg::LandmarkSubtype landmark_subtype_;
+    std::string landmarks_bb_key_;
+    std::string success_outcome_;
+};
+
+}  // namespace vortex_yasmin_utils
+
+#endif  // VORTEX_YASMIN_UTILS__LANDMARK_POLLING_STATE_HPP_

mission/vortex_yasmin_utils/include/vortex_yasmin_utils/trigger_wait_state.hpp

@@ -0,0 +1,54 @@
+#ifndef VORTEX_YASMIN_UTILS__TRIGGER_WAIT_STATE_HPP_
+#define VORTEX_YASMIN_UTILS__TRIGGER_WAIT_STATE_HPP_
+
+#include <chrono>
+#include <condition_variable>
+#include <mutex>
+#include <string>
+
+#include <rclcpp/rclcpp.hpp>
+#include <std_srvs/srv/trigger.hpp>
+
+#include <yasmin/state.hpp>
+
+namespace vortex_yasmin_utils {
+
+/**
+ * @brief A YASMIN state that blocks until a std_srvs/Trigger service is called.
+ *
+ * Creates a ROS service server and waits for it to be triggered.
+ * Returns SUCCEED when the service is called, CANCEL if the state is canceled,
+ * or TIMEOUT if the optional timeout expires.
+ *
+ * @param service_name  The fully-qualified ROS service name to advertise.
+ * @param timeout       How long to wait before timing out.
+ *                      A duration of zero (default) means wait indefinitely.
+ */
+class TriggerWaitState : public yasmin::State {
+   public:
+    explicit TriggerWaitState(const std::string& service_name,
+                              std::chrono::duration<double> timeout =
+                                  std::chrono::duration<double>(0));
+
+    std::string execute(yasmin::Blackboard::SharedPtr blackboard) override;
+    void cancel_state() override;
+
+    static constexpr const char* TIMEOUT = "timeout";
+
+   protected:
+    virtual void on_triggered(yasmin::Blackboard::SharedPtr blackboard);
+
+   private:
+    void service_callback(std_srvs::srv::Trigger::Request::SharedPtr req,
+                          std_srvs::srv::Trigger::Response::SharedPtr res);
+
+    rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr service_;
+    std::condition_variable cv_;
+    std::mutex mutex_;
+    bool triggered_{false};
+    std::chrono::duration<double> timeout_;
+};
+
+}  // namespace vortex_yasmin_utils
+
+#endif  // VORTEX_YASMIN_UTILS__TRIGGER_WAIT_STATE_HPP_

mission/vortex_yasmin_utils/package.xml

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>vortex_yasmin_utils</name>
+  <version>0.0.0</version>
+  <description>Reusable YASMIN state implementations for Vortex missions.</description>
+  <maintainer email="jorgen.fjermedal@hotmail.com">jorgenfj</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>rclcpp_action</depend>
+  <depend>yasmin</depend>
+  <depend>yasmin_ros</depend>
+  <depend>vortex_msgs</depend>
+  <depend>vortex_utils</depend>
+  <depend>vortex_utils_ros</depend>
+  <depend>geometry_msgs</depend>
+  <depend>std_srvs</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

mission/vortex_yasmin_utils/src/landmark_converge_state.cpp

@@ -0,0 +1,53 @@
+#include "vortex_yasmin_utils/landmark_converge_state.hpp"
+
+#include <vortex/utils/ros/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>
+#include <yasmin/logs.hpp>
+
+namespace vortex_yasmin_utils {
+
+LandmarkConvergeState::LandmarkConvergeState(
+    const std::string& action_server_name,
+    vortex::utils::waypoints::LandmarkConvergenceGoal convergence_goal,
+    const std::string& landmarks_bb_key)
+    : ActionState(action_server_name,
+                  std::bind(&LandmarkConvergeState::create_goal,
+                            this,
+                            std::placeholders::_1)),
+      convergence_goal_(std::move(convergence_goal)),
+      landmarks_bb_key_(landmarks_bb_key) {}
+
+WaypointManagerAction::Goal LandmarkConvergeState::create_goal(
+    yasmin::Blackboard::SharedPtr blackboard) {
+    auto landmarks = blackboard->get<std::vector<vortex_msgs::msg::Landmark>>(
+        landmarks_bb_key_);
+
+    if (landmarks.size() > 1) {
+        YASMIN_LOG_WARN(
+            "LandmarkConvergeState: received %zu landmarks, using first",
+            landmarks.size());
+    }
+
+    auto landmark = landmarks.front();
+
+    const auto landmark_pose =
+        vortex::utils::ros_conversions::ros_pose_to_pose(landmark.pose.pose);
+    const auto target_pose = vortex::utils::waypoints::apply_pose_offset(
+        landmark_pose, convergence_goal_.convergence_offset);
+
+    vortex_msgs::msg::Waypoint wp;
+    wp.pose = vortex::utils::ros_conversions::to_pose_msg(target_pose);
+    wp.waypoint_mode.mode = static_cast<uint8_t>(convergence_goal_.mode);
+
+    WaypointManagerAction::Goal goal;
+    goal.waypoints = {wp};
+    goal.persistent = false;
+    goal.convergence_threshold = convergence_goal_.convergence_threshold;
+
+    return goal;
+}
+
+}  // namespace vortex_yasmin_utils

mission/vortex_yasmin_utils/src/landmark_polling_state.cpp

@@ -0,0 +1,49 @@
+#include "vortex_yasmin_utils/landmark_polling_state.hpp"
+
+#include <vortex_msgs/msg/landmark.hpp>
+#include <yasmin_ros/basic_outcomes.hpp>
+
+namespace vortex_yasmin_utils {
+
+LandmarkPollingState::LandmarkPollingState(
+    const std::string& action_server_name,
+    vortex_msgs::msg::LandmarkType type,
+    vortex_msgs::msg::LandmarkSubtype subtype,
+    const std::string& landmarks_bb_key,
+    const std::string& success_outcome)
+    : ActionState(
+          action_server_name,
+          std::bind(&LandmarkPollingState::create_goal,
+                    this,
+                    std::placeholders::_1),
+          yasmin::Outcomes{success_outcome, yasmin_ros::basic_outcomes::ABORT},
+          std::bind(&LandmarkPollingState::result_handler,
+                    this,
+                    std::placeholders::_1,
+                    std::placeholders::_2)),
+      landmark_type_(type),
+      landmark_subtype_(subtype),
+      landmarks_bb_key_(landmarks_bb_key),
+      success_outcome_(success_outcome) {}
+
+LandmarkPollingAction::Goal LandmarkPollingState::create_goal(
+    yasmin::Blackboard::SharedPtr /*blackboard*/) {
+    LandmarkPollingAction::Goal goal;
+    goal.type = landmark_type_;
+    goal.subtype = landmark_subtype_;
+    return goal;
+}
+
+std::string LandmarkPollingState::result_handler(
+    yasmin::Blackboard::SharedPtr blackboard,
+    LandmarkPollingAction::Result::SharedPtr result) {
+    if (result->landmarks.landmarks.empty())
+        return yasmin_ros::basic_outcomes::ABORT;
+
+    blackboard->set<std::vector<vortex_msgs::msg::Landmark>>(
+        landmarks_bb_key_, result->landmarks.landmarks);
+
+    return success_outcome_;
+}
+
+}  // namespace vortex_yasmin_utils